placo 0.5.0__tar.gz → 0.5.1__tar.gz

{placo-0.5.0 → placo-0.5.1}/CMakeLists.txt

@@ -78,6 +78,7 @@ add_library(libplaco SHARED
     src/placo/kinematics/gear_task.cpp
     src/placo/kinematics/wheel_task.cpp
     src/placo/kinematics/regularization_task.cpp
+    src/placo/kinematics/manipulability_task.cpp
     src/placo/kinematics/kinetic_energy_regularization_task.cpp
     src/placo/kinematics/constraint.cpp
     src/placo/kinematics/avoid_self_collisions_constraint.cpp

{placo-0.5.0 → placo-0.5.1}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: placo
-Version: 0.5.0
+Version: 0.5.1
 Summary: PlaCo: Rhoban Planning and Control
 Requires-Python: >= 3.8
 License-Expression: MIT

{placo-0.5.0 → placo-0.5.1}/bindings/expose-dynamics.cpp

@@ -24,6 +24,8 @@ void exposeDynamics()
           "tau", +[](const DynamicsSolver::Result& result) { return result.tau; })
       .add_property(
           "qdd", +[](const DynamicsSolver::Result& result) { return result.qdd; })
+      .add_property(
+          "tau_contacts", +[](const DynamicsSolver::Result& result) { return result.tau_contacts; })
       .def(
           "tau_dict", +[](const DynamicsSolver::Result& result, RobotWrapper& robot) {
             boost::python::dict dict;
@@ -81,6 +83,7 @@ void exposeDynamics()
       .def_readwrite("dt", &DynamicsSolver::dt)
       .def_readwrite("qdd_safe", &DynamicsSolver::qdd_safe)
       .def_readwrite("gravity_only", &DynamicsSolver::gravity_only)
+      .def_readwrite("torque_cost", &DynamicsSolver::torque_cost)
       .def("mask_fbase", &DynamicsSolver::mask_fbase)
       .def("add_point_contact", &DynamicsSolver::add_point_contact, return_internal_reference<>())
       .def("add_unilateral_point_contact", &DynamicsSolver::add_unilateral_point_contact, return_internal_reference<>())

{placo-0.5.0 → placo-0.5.1}/bindings/expose-kinematics.cpp

@@ -76,6 +76,10 @@ void exposeKinematics()
           // Regularization task
           .def("add_regularization_task", &KinematicsSolver::add_regularization_task, return_internal_reference<>())
 
+          // Manipulability task
+          .def<ManipulabilityTask& (KinematicsSolver::*)(std::string, std::string, double)>(
+              "add_manipulability_task", &KinematicsSolver::add_manipulability_task, return_internal_reference<>())
+
           // Kinetic energy regularization task
           .def("add_kinetic_energy_regularization_task", &KinematicsSolver::add_kinetic_energy_regularization_task,
                return_internal_reference<>())
@@ -216,6 +220,12 @@ void exposeKinematics()
 
   class__<RegularizationTask, bases<Task>>("RegularizationTask");
 
+  class__<ManipulabilityTask, bases<Task>>("ManipulabilityTask",
+                                           init<RobotWrapper::FrameIndex, ManipulabilityTask::Type, double>())
+      .def_readwrite("lambda", &ManipulabilityTask::lambda)
+      .def_readwrite("minimize", &ManipulabilityTask::minimize)
+      .def_readonly("manipulability", &ManipulabilityTask::manipulability);
+
   class__<KineticEnergyRegularizationTask, bases<RegularizationTask>>("KineticEnergyRegularizationTask");
 
   class__<Constraint, bases<tools::Prioritized>, boost::noncopyable>("KinematicsConstraint", no_init);

{placo-0.5.0 → placo-0.5.1}/bindings/expose-robot-wrapper.cpp

@@ -43,6 +43,12 @@ void exposeRobotType(class_<RobotType, W1>& type)
       .def("set_torque_limit", &RobotType::set_torque_limit)
       .def("set_joint_limits", &RobotType::set_joint_limits)
       .def("update_kinematics", &RobotType::update_kinematics)
+      .def("compute_hessians", &RobotType::compute_hessians)
+      .def(
+          "get_frame_hessian",
+          +[](RobotType& robot, const std::string frame, const std::string joint) {
+            return robot.get_frame_hessian(robot.model.getFrameId(frame), robot.get_joint_v_offset(joint));
+          })
       .def("get_T_world_fbase", &RobotType::get_T_world_fbase)
       .def("set_T_world_fbase", &RobotType::set_T_world_fbase)
       .def("com_world", &RobotType::com_world)

{placo-0.5.0 → placo-0.5.1}/placo.pyi

@@ -52,6 +52,7 @@ KinematicsSolver = typing.NewType("KinematicsSolver", None)
 KineticEnergyRegularizationTask = typing.NewType("KineticEnergyRegularizationTask", None)
 LIPM = typing.NewType("LIPM", None)
 LIPMTrajectory = typing.NewType("LIPMTrajectory", None)
+ManipulabilityTask = typing.NewType("ManipulabilityTask", None)
 OrientationTask = typing.NewType("OrientationTask", None)
 PointContact = typing.NewType("PointContact", None)
 PolygonConstraint = typing.NewType("PolygonConstraint", None)
@@ -2206,6 +2207,10 @@ class DynamicsSolver:
   ) -> DynamicsSolverResult:
     ...
 
+  torque_cost: float # double
+  """Cost for torque regularization. 
+        """
+
 
 class DynamicsSolverResult:
   def __init__(
@@ -2220,6 +2225,8 @@ class DynamicsSolverResult:
 
   tau: numpy.ndarray # Eigen::VectorXd
 
+  tau_contacts: numpy.ndarray # Eigen::VectorXd
+
   def tau_dict(
     arg1: DynamicsSolverResult,
     arg2: RobotWrapper,
@@ -3256,6 +3263,13 @@ class HumanoidRobot:
     """Gets the CoM position in the world."""
     ...
 
+  def compute_hessians(
+    self: HumanoidRobot,
+
+  ) -> None:
+    """Compute kinematics hessians."""
+    ...
+
   def dcm(
     self: HumanoidRobot,
     com_velocity: numpy.ndarray, # Eigen::Vector2d
@@ -3406,6 +3420,15 @@ class HumanoidRobot:
     :return: Center of mass velocity"""
     ...
 
+  def get_frame_hessian(
+    self: HumanoidRobot,
+    frame: any, # pinocchio::FrameIndex
+    joint_v_index: int, # int
+
+  ) -> numpy.ndarray:
+    """Get the component for the hessian of a given frame for a given joint."""
+    ...
+
   def get_joint(
     self: HumanoidRobot,
     name: str, # const std::string &
@@ -4351,6 +4374,23 @@ class KinematicsSolver:
     :return: regularization task"""
     ...
 
+  def add_manipulability_task(
+    self: KinematicsSolver,
+    frame: str, # std::string
+    type: str = "both", # std::string
+    lambda_: float = 1.0, # double
+
+  ) -> ManipulabilityTask:
+    """Adds a manipulability regularization task for a given magnitude. 
+        
+
+    :param frame: the reference frame 
+
+    :param type: type (position, orientation or both) 
+
+    :return: manipulability task"""
+    ...
+
   def add_orientation_task(
     self: KinematicsSolver,
     frame: str, # std::string
@@ -4823,6 +4863,89 @@ class LIPMTrajectory:
     ...
 
 
+class ManipulabilityTask:
+  A: numpy.ndarray # Eigen::MatrixXd
+  """Matrix A in the task Ax = b, where x are the joint delta positions. 
+        """
+
+  def __init__(
+    self: ManipulabilityTask,
+    frame_index: any, # pinocchio::FrameIndex
+    type: any, # placo::kinematics::ManipulabilityTask::Type
+    lambda: float = 1.0, # double
+
+  ) -> any:
+    ...
+
+  b: numpy.ndarray # Eigen::MatrixXd
+  """Vector b in the task Ax = b, where x are the joint delta positions. 
+        """
+
+  def configure(
+    self: ManipulabilityTask,
+    name: str, # std::string
+    priority: any, # placo::kinematics::ConeConstraint::Priority
+    weight: float = 1.0, # double
+
+  ) -> None:
+    """Configures the object. 
+        
+
+    :param name: task name 
+
+    :param priority: task priority (hard, soft or scaled) 
+
+    :param weight: task weight"""
+    ...
+
+  def error(
+    self: ManipulabilityTask,
+
+  ) -> numpy.ndarray:
+    """Task errors (vector) 
+        
+
+    :return: task errors"""
+    ...
+
+  def error_norm(
+    self: ManipulabilityTask,
+
+  ) -> float:
+    """The task error norm. 
+        
+
+    :return: task error norm"""
+    ...
+
+  lambda: float # double
+  """Importance of the hessian regularization. 
+        """
+
+  manipulability: bool # bool
+  """The last computed manipulability value. 
+        """
+
+  minimize: bool # bool
+  """Should the manipulability be minimized (can be useful to find singularities) 
+        """
+
+  name: str # std::string
+  """Object name. 
+        """
+
+  priority: any # placo::kinematics::ConeConstraint::Priority
+  """Object priority. 
+        """
+
+  def update(
+    self: ManipulabilityTask,
+
+  ) -> None:
+    """Update the task A and b matrices from the robot state and targets."""
+    ...
+
+
 class OrientationTask:
   A: numpy.ndarray # Eigen::MatrixXd
   """Matrix A in the task Ax = b, where x are the joint delta positions. 
@@ -5699,6 +5822,13 @@ class RobotWrapper:
     """Gets the CoM position in the world."""
     ...
 
+  def compute_hessians(
+    self: RobotWrapper,
+
+  ) -> None:
+    """Compute kinematics hessians."""
+    ...
+
   def distances(
     self: RobotWrapper,
 
@@ -5792,6 +5922,15 @@ class RobotWrapper:
     :return: transformation"""
     ...
 
+  def get_frame_hessian(
+    self: RobotWrapper,
+    frame: any, # pinocchio::FrameIndex
+    joint_v_index: int, # int
+
+  ) -> numpy.ndarray:
+    """Get the component for the hessian of a given frame for a given joint."""
+    ...
+
   def get_joint(
     self: RobotWrapper,
     name: str, # const std::string &
@@ -7309,4 +7448,4 @@ def wrap_angle(
   ...
 
 
-__groups__ = {'placo::dynamics': ['AvoidSelfCollisionsDynamicsConstraint', 'Contact', 'Contact6D', 'DynamicsCoMTask', 'DynamicsConstraint', 'DynamicsFrameTask', 'DynamicsGearTask', 'DynamicsJointsTask', 'DynamicsOrientationTask', 'DynamicsPositionTask', 'DynamicsReactionRatioConstraint', 'DynamicsRelativeFrameTask', 'DynamicsRelativeOrientationTask', 'DynamicsRelativePositionTask', 'DynamicsSolver', 'DynamicsSolverResult', 'DynamicsTask', 'DynamicsTorqueTask', 'ExternalWrenchContact', 'PointContact', 'PuppetContact', 'Relative6DContact', 'RelativePointContact', 'TaskContact'], 'placo::kinematics': ['AvoidSelfCollisionsKinematicsConstraint', 'AxisAlignTask', 'CentroidalMomentumTask', 'CoMPolygonConstraint', 'CoMTask', 'ConeConstraint', 'DistanceTask', 'FrameTask', 'GearTask', 'JointsTask', 'KinematicsConstraint', 'KinematicsSolver', 'KineticEnergyRegularizationTask', 'OrientationTask', 'PositionTask', 'RegularizationTask', 'RelativeFrameTask', 'RelativeOrientationTask', 'RelativePositionTask', 'Task', 'WheelTask'], 'placo::tools': ['AxisesMask', 'CubicSpline', 'CubicSpline3D', 'Prioritized'], 'placo::model': ['Collision', 'Distance', 'RobotWrapper', 'RobotWrapper_State'], 'placo::problem': ['Expression', 'Integrator', 'IntegratorTrajectory', 'PolygonConstraint', 'Problem', 'ProblemConstraint', 'QPError', 'Sparsity', 'SparsityInterval', 'Variable'], 'placo::humanoid': ['Footstep', 'FootstepsPlanner', 'FootstepsPlannerNaive', 'FootstepsPlannerRepetitive', 'HumanoidParameters', 'HumanoidRobot', 'LIPM', 'LIPMTrajectory', 'Support', 'SwingFoot', 'SwingFootCubic', 'SwingFootCubicTrajectory', 'SwingFootQuintic', 'SwingFootQuinticTrajectory', 'SwingFootTrajectory', 'WalkPatternGenerator', 'WalkTasks', 'WalkTrajectory']}
+__groups__ = {'placo::dynamics': ['AvoidSelfCollisionsDynamicsConstraint', 'Contact', 'Contact6D', 'DynamicsCoMTask', 'DynamicsConstraint', 'DynamicsFrameTask', 'DynamicsGearTask', 'DynamicsJointsTask', 'DynamicsOrientationTask', 'DynamicsPositionTask', 'DynamicsReactionRatioConstraint', 'DynamicsRelativeFrameTask', 'DynamicsRelativeOrientationTask', 'DynamicsRelativePositionTask', 'DynamicsSolver', 'DynamicsSolverResult', 'DynamicsTask', 'DynamicsTorqueTask', 'ExternalWrenchContact', 'PointContact', 'PuppetContact', 'Relative6DContact', 'RelativePointContact', 'TaskContact'], 'placo::kinematics': ['AvoidSelfCollisionsKinematicsConstraint', 'AxisAlignTask', 'CentroidalMomentumTask', 'CoMPolygonConstraint', 'CoMTask', 'ConeConstraint', 'DistanceTask', 'FrameTask', 'GearTask', 'JointsTask', 'KinematicsConstraint', 'KinematicsSolver', 'KineticEnergyRegularizationTask', 'ManipulabilityTask', 'OrientationTask', 'PositionTask', 'RegularizationTask', 'RelativeFrameTask', 'RelativeOrientationTask', 'RelativePositionTask', 'Task', 'WheelTask'], 'placo::tools': ['AxisesMask', 'CubicSpline', 'CubicSpline3D', 'Prioritized'], 'placo::model': ['Collision', 'Distance', 'RobotWrapper', 'RobotWrapper_State'], 'placo::problem': ['Expression', 'Integrator', 'IntegratorTrajectory', 'PolygonConstraint', 'Problem', 'ProblemConstraint', 'QPError', 'Sparsity', 'SparsityInterval', 'Variable'], 'placo::humanoid': ['Footstep', 'FootstepsPlanner', 'FootstepsPlannerNaive', 'FootstepsPlannerRepetitive', 'HumanoidParameters', 'HumanoidRobot', 'LIPM', 'LIPMTrajectory', 'Support', 'SwingFoot', 'SwingFootCubic', 'SwingFootCubicTrajectory', 'SwingFootQuintic', 'SwingFootQuinticTrajectory', 'SwingFootTrajectory', 'WalkPatternGenerator', 'WalkTasks', 'WalkTrajectory']}

{placo-0.5.0 → placo-0.5.1}/pyproject.toml

@@ -23,7 +23,7 @@ description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.8"
-version = "0.5.0"
+version = "0.5.1"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.5.0 → placo-0.5.1}/src/placo/dynamics/dynamics_solver.cpp

@@ -462,6 +462,11 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
     tau = tau + robot.non_linear_effects();
   }
 
+  if (extra_force.size() > 0)
+  {
+    tau = tau + extra_force;
+  }
+
   // J^T F
   for (auto& contact : contacts)
   {
@@ -563,7 +568,7 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
   }
 
   // We want to minimize torques
-  problem.add_constraint(tau == 0).configure(ProblemConstraint::Soft, 1e-3);
+  problem.add_constraint(tau == 0).configure(ProblemConstraint::Soft, torque_cost);
 
   try
   {
@@ -574,12 +579,14 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
     // Exporting result values
     result.tau = tau.value(problem.x);
     result.qdd = qdd.value(problem.x);
+    result.tau_contacts = Eigen::VectorXd::Zero(N);
 
     for (auto& contact : contacts)
     {
       if (contact->active)
       {
         contact->wrench = contact->f.value(problem.x);
+        result.tau_contacts += contact->J.transpose() * contact->wrench;
       }
     }
 

{placo-0.5.0 → placo-0.5.1}/src/placo/dynamics/dynamics_solver.h

@@ -38,11 +38,23 @@ public:
     // Checks if the gravity computation is a success
     bool success;
 
+    // The following equation should hold: M qdd + b = tau + tau_contacts
+
+    // With:
+    // - M: the mass matrix
+    // - qdd: joint-space acceleration
+    // - b: non-linear (bias) terms
+    // - tau: applied torques vector
+    // - tau_contacts: contact forces
+
     // Torques computed by the solver
     Eigen::VectorXd tau;
 
     // Accelerations computed by the solver
     Eigen::VectorXd qdd;
+
+    // Contact forces computed by the solver
+    Eigen::VectorXd tau_contacts;
   };
 
   struct OverrideJoint
@@ -426,6 +438,16 @@ public:
    */
   bool gravity_only = false;
 
+  /**
+   * @brief Cost for torque regularization
+   */
+  double torque_cost = 1e-3;
+
+  /**
+   * @brief Extra force to be added to the system (similar to non-linear terms)
+   */
+  Eigen::VectorXd extra_force = Eigen::VectorXd::Zero(0);
+
   /**
    * @brief Instance of the problem
    */

{placo-0.5.0 → placo-0.5.1}/src/placo/kinematics/kinematics_solver.cpp

@@ -162,6 +162,35 @@ RegularizationTask& KinematicsSolver::add_regularization_task(double magnitude)
   return task;
 }
 
+ManipulabilityTask& KinematicsSolver::add_manipulability_task(model::RobotWrapper::FrameIndex frame,
+                                                              ManipulabilityTask::Type type, double lambda)
+{
+  return add_task(new ManipulabilityTask(frame, type, lambda));
+}
+
+ManipulabilityTask& KinematicsSolver::add_manipulability_task(std::string frame, std::string type, double lambda)
+{
+  ManipulabilityTask::Type type_;
+  if (type == "position")
+  {
+    type_ = ManipulabilityTask::Type::POSITION;
+  }
+  else if (type == "orientation")
+  {
+    type_ = ManipulabilityTask::Type::ORIENTATION;
+  }
+  else if (type == "both")
+  {
+    type_ = ManipulabilityTask::Type::BOTH;
+  }
+  else
+  {
+    throw std::runtime_error("Unknown manipulability type: " + type);
+  }
+
+  return add_manipulability_task(robot.get_frame_index(frame), type_, lambda);
+}
+
 KineticEnergyRegularizationTask& KinematicsSolver::add_kinetic_energy_regularization_task(double magnitude)
 {
   KineticEnergyRegularizationTask& task = add_task(new KineticEnergyRegularizationTask());

{placo-0.5.0 → placo-0.5.1}/src/placo/kinematics/kinematics_solver.h

@@ -19,6 +19,7 @@
 #include "placo/kinematics/gear_task.h"
 #include "placo/kinematics/wheel_task.h"
 #include "placo/kinematics/regularization_task.h"
+#include "placo/kinematics/manipulability_task.h"
 #include "placo/kinematics/kinetic_energy_regularization_task.h"
 #include "placo/kinematics/centroidal_momentum_task.h"
 #include "placo/kinematics/axis_align_task.h"
@@ -249,6 +250,21 @@ public:
    */
   RegularizationTask& add_regularization_task(double magnitude = 1e-6);
 
+  /**
+   * @brief Adds a manipulability regularization task for a given magnitude
+   * @return manipulability task
+   */
+  ManipulabilityTask& add_manipulability_task(model::RobotWrapper::FrameIndex frame, ManipulabilityTask::Type type,
+                                              double lambda = 1.0);
+
+  /**
+   * @brief Adds a manipulability regularization task for a given magnitude
+   * @param frame the reference frame
+   * @param type type (position, orientation or both)
+   * @return manipulability task
+   */
+  ManipulabilityTask& add_manipulability_task(std::string frame, std::string type = "both", double lambda_ = 1.0);
+
   /**
    * @brief Adds a kinetic energy regularization task for a given magnitude
    * @param magnitude regularization magnitude

placo-0.5.1/src/placo/kinematics/manipulability_task.cpp

@@ -0,0 +1,73 @@
+#include "placo/kinematics/manipulability_task.h"
+#include "placo/kinematics/kinematics_solver.h"
+
+namespace placo::kinematics
+{
+ManipulabilityTask::ManipulabilityTask(model::RobotWrapper::FrameIndex frame_index, Type type, double lambda)
+  : frame_index(frame_index), type(type), lambda(lambda)
+{
+}
+
+Eigen::MatrixXd ManipulabilityTask::mask_matrix(Eigen::MatrixXd M)
+{
+  if (type == POSITION)
+  {
+    return M.block(0, 6, 3, M.cols() - 6);
+  }
+  else if (type == ORIENTATION)
+  {
+    return M.block(3, 6, 3, M.cols() - 6);
+  }
+  else
+  {
+    return M.block(0, 6, 6, M.cols() - 6);
+  }
+}
+
+void ManipulabilityTask::update()
+{
+  // Computing the Jacobian matrix
+  Eigen::MatrixXd J_unmasked = solver->robot.frame_jacobian(frame_index, pinocchio::LOCAL);
+  Eigen::MatrixXd J = mask_matrix(J_unmasked);
+  Eigen::MatrixXd JJT_inv = (J * J.transpose()).inverse();
+  double manipulability = sqrt(fmax(0., (J * J.transpose()).determinant()));
+  solver->robot.compute_hessians();
+
+  Eigen::VectorXd manipulability_gradient(solver->N - 6);
+  manipulability_gradient.setZero();
+
+  for (int dof = 6; dof < solver->N; dof++)
+  {
+    Eigen::MatrixXd H_dof_unmasked = solver->robot.get_frame_hessian(frame_index, dof);
+    Eigen::MatrixXd H_dof = mask_matrix(H_dof_unmasked);
+    Eigen::MatrixXd JH = J * H_dof.transpose();
+
+    manipulability_gradient(dof - 6) = manipulability * JH.cwiseProduct(JJT_inv).sum();
+  }
+
+  // Regularization magnitude is handled through the task weight (see add_regularization_task)
+  // Floating base is not regularized by this task
+  Eigen::MatrixXd I = Eigen::MatrixXd(solver->N, solver->N);
+  I.setIdentity();
+
+  A = Eigen::MatrixXd(solver->N - 6, solver->N);
+  A.block(0, 0, solver->N - 6, solver->N) = I.block(6, 0, solver->N - 6, solver->N) * lambda;
+
+  b = (1 / (2. * lambda)) * manipulability_gradient;
+
+  if (minimize)
+  {
+    b = -b;
+  }
+}
+
+std::string ManipulabilityTask::type_name()
+{
+  return "manipulability";
+}
+
+std::string ManipulabilityTask::error_unit()
+{
+  return "none";
+}
+}  // namespace placo::kinematics

placo-0.5.1/src/placo/kinematics/manipulability_task.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include "placo/kinematics/task.h"
+
+namespace placo::kinematics
+{
+class KinematicsSolver;
+struct ManipulabilityTask : public Task
+{
+  enum Type
+  {
+    POSITION = 0,
+    ORIENTATION = 1,
+    BOTH = 2
+  };
+
+  ManipulabilityTask(model::RobotWrapper::FrameIndex frame_index, Type type, double lambda = 1.0);
+
+  virtual void update();
+  virtual std::string type_name();
+  virtual std::string error_unit();
+
+  Eigen::MatrixXd mask_matrix(Eigen::MatrixXd M);
+
+  /**
+   * @brief Index of the frame we want to set manipulability
+   */
+  model::RobotWrapper::FrameIndex frame_index;
+
+  /**
+   * @brief Importance of the hessian regularization
+   */
+  double lambda;
+
+  /**
+   * @brief Type of frame manipulability to compute
+   */
+  Type type;
+
+  /**
+   * @brief Should the manipulability be minimized (can be useful to find singularities)
+   */
+  bool minimize = false;
+
+  /**
+   * @brief The last computed manipulability value
+   */
+  bool manipulability = 0.;
+};
+}  // namespace placo::kinematics

{placo-0.5.0 → placo-0.5.1}/src/placo/model/robot_wrapper.cpp

@@ -3,6 +3,7 @@
 #include "pinocchio/algorithm/center-of-mass.hpp"
 #include "pinocchio/algorithm/compute-all-terms.hpp"
 #include "pinocchio/algorithm/geometry.hpp"
+#include "pinocchio/algorithm/kinematics-derivatives.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
 #include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/centroidal.hpp"
@@ -253,6 +254,26 @@ void RobotWrapper::update_kinematics()
   pinocchio::framesForwardKinematics(model, *data, state.q);
   pinocchio::computeJointJacobians(model, *data, state.q);
   pinocchio::computeJointJacobiansTimeVariation(model, *data, state.q, state.qd);
+  pinocchio::updateFramePlacements(model, *data);
+}
+
+void RobotWrapper::compute_hessians()
+{
+  pinocchio::computeJointKinematicHessians(model, *data);
+}
+
+Eigen::MatrixXd RobotWrapper::get_frame_hessian(FrameIndex frame, int joint_v_index)
+{
+  // Frame parent joint index
+  pinocchio::JointIndex joint_id = model.frames[frame].parent;
+  pinocchio::SE3 T = model.frames[frame].placement;
+
+  pinocchio::Tensor<double, 3, 0> H = pinocchio::getJointKinematicHessian(model, *data, joint_id, pinocchio::LOCAL);
+  const Eigen::DenseIndex matrix_offset = 6 * model.nv;
+
+  Eigen::Map<Eigen::MatrixXd> hessian(H.data() + joint_v_index * matrix_offset, 6, model.nv);
+
+  return T.inverse().toActionMatrix() * hessian;
 }
 
 RobotWrapper::State RobotWrapper::neutral_state()

{placo-0.5.0 → placo-0.5.1}/src/placo/model/robot_wrapper.h

@@ -224,6 +224,16 @@ public:
    */
   void update_kinematics();
 
+  /**
+   * @brief Compute kinematics hessians
+   */
+  void compute_hessians();
+
+  /**
+   * @brief Get the component for the hessian of a given frame for a given joint
+   */
+  Eigen::MatrixXd get_frame_hessian(FrameIndex frame, int joint_v_index);
+
   /**
    * @brief Gets the CoM position in the world
    */