[exotica_core] Add UnconstrainedEndPoseProblem::GetHessian

Implements analytical Hessian for UnconstrainedEndPoseProblem
ipab-slmc · Dec 8, 2021 · be580d1 · be580d1
1 parent 559b728
commit be580d1
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 0 deletions.
diff --git a/exotica_core/include/exotica_core/problems/unconstrained_end_pose_problem.h b/exotica_core/include/exotica_core/problems/unconstrained_end_pose_problem.h
@@ -58,6 +58,7 @@ class UnconstrainedEndPoseProblem : public PlanningProblem, public Instantiable<
 
  double GetScalarCost() const;
  Eigen::RowVectorXd GetScalarJacobian() const;
+ Eigen::MatrixXd GetHessian() const;
 
  /**
  * @brief GetScalarTaskCost get weighted sum-of-squares of cost vector

diff --git a/exotica_core/src/problems/unconstrained_end_pose_problem.cpp b/exotica_core/src/problems/unconstrained_end_pose_problem.cpp
@@ -93,6 +93,27 @@ Eigen::RowVectorXd UnconstrainedEndPoseProblem::GetScalarJacobian() const
  return cost.jacobian.transpose() * cost.S * cost.ydiff * 2.0;
 }
 
+Eigen::MatrixXd UnconstrainedEndPoseProblem::GetHessian() const
+{
+ Eigen::MatrixXd general_cost_hessian = Eigen::MatrixXd::Zero(N, N);
+
+ // Contract task-map Hessian
+ if (flags_ & KIN_H)
+ {
+ Eigen::RowVectorXd ydiffTS = cost.ydiff.transpose() * cost.S; // (1*m)
+ for (int i = 0; i < cost.length_jacobian; ++i) // length m
+ {
+ general_cost_hessian.noalias() += ydiffTS(i) * cost.hessian(i);
+ }
+ }
+ else
+ {
+ std::cerr << "[UnconstrainedEndPoseProblem::GetHessian] Hessian requested but DerivativeOrder < 2" << std::endl;
+ }
+
+ return 2. * general_cost_hessian;
+}
+
 double UnconstrainedEndPoseProblem::GetScalarTaskCost(const std::string& task_name) const
 {
  const Eigen::VectorXd ydiff = cost.GetTaskError(task_name);

diff --git a/exotica_python/src/pyexotica.cpp b/exotica_python/src/pyexotica.cpp
@@ -1062,6 +1062,7 @@ PYBIND11_MODULE(_pyexotica, module)
  unconstrained_end_pose_problem.def_property("q_nominal", &UnconstrainedEndPoseProblem::GetNominalPose, &UnconstrainedEndPoseProblem::SetNominalPose);
  unconstrained_end_pose_problem.def("get_scalar_cost", &UnconstrainedEndPoseProblem::GetScalarCost);
  unconstrained_end_pose_problem.def("get_scalar_jacobian", &UnconstrainedEndPoseProblem::GetScalarJacobian);
+ unconstrained_end_pose_problem.def("get_hessian", &UnconstrainedEndPoseProblem::GetHessian);
  unconstrained_end_pose_problem.def("get_scalar_task_cost", &UnconstrainedEndPoseProblem::GetScalarTaskCost);
  unconstrained_end_pose_problem.def_readonly("cost", &UnconstrainedEndPoseProblem::cost);