placo 0.4.9__tar.gz → 0.5.1__tar.gz

{placo-0.4.9 → 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.4.9 → placo-0.5.1}/PKG-INFO

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

{placo-0.4.9 → 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.4.9 → placo-0.5.1}/bindings/expose-kinematics.cpp

@@ -20,7 +20,6 @@ void exposeKinematics()
   class_<KinematicsSolver> solver_class =
       class__<KinematicsSolver>("KinematicsSolver", init<RobotWrapper&>())
           .add_property("problem", &KinematicsSolver::problem)
-          .add_property("noise", &KinematicsSolver::noise, &KinematicsSolver::noise)
           .add_property("dt", &KinematicsSolver::dt, &KinematicsSolver::dt)
           .add_property("N", &KinematicsSolver::N)
           .add_property("scale", &KinematicsSolver::scale)
@@ -77,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<>())
@@ -111,7 +114,8 @@ void exposeKinematics()
           .def<void (KinematicsSolver::*)(Task&)>("remove_task", &KinematicsSolver::remove_task)
           .def<void (KinematicsSolver::*)(FrameTask&)>("remove_task", &KinematicsSolver::remove_task)
           .def("remove_constraint", &KinematicsSolver::remove_constraint)
-          .def("solve", &KinematicsSolver::solve);
+          .def("solve", &KinematicsSolver::solve)
+          .def("add_q_noise", &KinematicsSolver::add_q_noise);
 
   class__<Task, bases<tools::Prioritized>, boost::noncopyable>("Task", no_init)
       .add_property(
@@ -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.4.9 → placo-0.5.1}/bindings/expose-problem.cpp

@@ -22,6 +22,7 @@ using namespace placo;
 using namespace placo::problem;
 
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(expr_overloads, expr, 0, 2);
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(integrator_expr_overloads, expr, 1, 2);
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(configure_overloads, configure, 1, 2);
 
 void exposeProblem()
@@ -91,7 +92,7 @@ void exposeProblem()
           "B", +[](const Integrator& i) { return i.B; })
       .add_property(
           "final_transition_matrix", +[](const Integrator& i) { return i.final_transition_matrix; })
-      .def("expr", &Integrator::expr)
+      .def("expr", &Integrator::expr, integrator_expr_overloads())
       .def("expr_t", &Integrator::expr_t)
       .def("value", &Integrator::value)
       .def("get_trajectory", &Integrator::get_trajectory);

{placo-0.4.9 → 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.4.9 → 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
@@ -4383,6 +4423,17 @@ class KinematicsSolver:
     :return: position task"""
     ...
 
+  def add_q_noise(
+    self: KinematicsSolver,
+    noise: float = 1e-5, # double
+
+  ) -> None:
+    """Adds some noise on the configuration of the robot (q) 
+        
+
+    :param noise: noise level, expressed in ratio of the joint limits"""
+    ...
+
   def add_regularization_task(
     self: KinematicsSolver,
     magnitude: float = 1e-6, # double
@@ -4532,10 +4583,6 @@ class KinematicsSolver:
     """Decides if the floating base should be masked."""
     ...
 
-  noise: float # double
-  """Some configuration noise added before solving. 
-        """
-
   problem: Problem # placo::problem::Problem
   """The underlying QP problem. 
         """
@@ -4816,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. 
@@ -5692,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,
 
@@ -5785,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 &
@@ -7302,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.4.9 → 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.4.9"
+version = "0.5.1"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.4.9 → 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.4.9 → 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.4.9 → placo-0.5.1}/src/placo/humanoid/walk_tasks.cpp

@@ -6,20 +6,19 @@ namespace placo::humanoid
 using namespace placo::kinematics;
 using namespace placo::tools;
 
-
 void WalkTasks::initialize_tasks(KinematicsSolver* solver_, HumanoidRobot* robot_)
 {
   robot = robot_;
   solver = solver_;
 
   left_foot_task = solver->add_frame_task("left_foot", robot->get_T_world_left());
-  left_foot_task.configure("left_foot", scaled?"scaled":"soft", 1., 1.);
+  left_foot_task.configure("left_foot", scaled ? "scaled" : "soft", 1., 1.);
 
   right_foot_task = solver->add_frame_task("right_foot", robot->get_T_world_right());
-  right_foot_task.configure("right_foot", scaled?"scaled":"soft", 1., 1.);
+  right_foot_task.configure("right_foot", scaled ? "scaled" : "soft", 1., 1.);
 
   trunk_orientation_task = &solver->add_orientation_task("trunk", robot->get_T_world_trunk().rotation());
-  trunk_orientation_task->configure("trunk", scaled?"scaled":"soft", 1.);
+  trunk_orientation_task->configure("trunk", scaled ? "scaled" : "soft", 1.);
 
   update_com_task();
 }
@@ -36,7 +35,7 @@ void WalkTasks::update_com_task()
     if (trunk_task == nullptr)
     {
       trunk_task = &solver->add_position_task("trunk", robot->get_T_world_frame("trunk").translation());
-      trunk_task->configure("trunk", scaled?"scaled":"soft", 1.);
+      trunk_task->configure("trunk", scaled ? "scaled" : "soft", 1.);
     }
   }
   else
@@ -49,7 +48,7 @@ void WalkTasks::update_com_task()
     if (com_task == nullptr)
     {
       com_task = &solver->add_com_task(robot->com_world());
-      com_task->configure("com", scaled?"scaled":"soft", 1.);
+      com_task->configure("com", scaled ? "scaled" : "soft", 1.);
     }
   }
 }
@@ -69,13 +68,12 @@ void WalkTasks::reach_initial_pose(Eigen::Affine3d T_world_left, double feet_spa
 
   update_tasks(T_world_left, T_world_right, com_world, R_world_trunk);
 
-  // Adding strong noise to avoid singularities
-  solver->noise = 0.1;
   for (int i = 0; i < 100; i++)
   {
-    if (i == 10)
+    if (i <= 10)
     {
-      solver->noise = 1e-4;
+      // Adding strong noise to avoid singularities
+      solver->add_q_noise(0.1);
     }
 
     robot->update_kinematics();

{placo-0.4.9 → placo-0.5.1}/src/placo/kinematics/com_polygon_constraint.cpp

@@ -28,7 +28,6 @@ void CoMPolygonConstraint::add_constraint(placo::problem::Problem& problem)
     // Future DCM is c + J dq + J dq / (dt * w)
     //             = c + J dq (1 + 1 / (dt * w))
     com_xy.A *= (1. + 1. / (solver->dt * omega));
-    std::cout << "Coef: " << (1. + 1. / (solver->dt * omega)) << std::endl;
   }
   com_xy.b = com;
 

{placo-0.4.9 → placo-0.5.1}/src/placo/kinematics/gear_task.cpp

@@ -42,7 +42,6 @@ void GearTask::update()
     {
       int source = gear_source.first;
       double ratio = gear_source.second;
-      auto& gear = entry.second;
       double q_source = solver->robot.state.q[source + 1];
 
       A(k, source) = ratio;

{placo-0.4.9 → 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());
@@ -244,6 +273,23 @@ void KinematicsSolver::compute_limits_inequalities()
   }
 }
 
+void KinematicsSolver::add_q_noise(double noise)
+{
+  auto q_random = pinocchio::randomConfiguration(robot.model);
+
+  // Adding some noise in direction of a random configuration (except floating base)
+  for (int k = 7; k < robot.model.nq; k++)
+  {
+    if (robot.model.lowerPositionLimit(k) == std::numeric_limits<double>::lowest() ||
+        robot.model.upperPositionLimit(k) == std::numeric_limits<double>::max())
+    {
+      continue;
+    }
+
+    robot.state.q(k) += (q_random(k) - robot.state.q(k)) * noise;
+  }
+}
+
 Eigen::VectorXd KinematicsSolver::solve(bool apply)
 {
   // Ensure variable is created
@@ -255,26 +301,6 @@ Eigen::VectorXd KinematicsSolver::solve(bool apply)
   // Clear previously created constraints
   problem.clear_constraints();
 
-  // Adding some random noise
-  auto q_save = robot.state.q;
-
-  if (noise > 0)
-  {
-    auto q_random = pinocchio::randomConfiguration(robot.model);
-
-    // Adding some noise in direction of a random configuration (except floating base)
-    for (int k = 7; k < robot.model.nq; k++)
-    {
-      if (robot.model.lowerPositionLimit(k) == std::numeric_limits<double>::lowest() ||
-          robot.model.upperPositionLimit(k) == std::numeric_limits<double>::max())
-      {
-        continue;
-      }
-
-      robot.state.q(k) += (q_random(k) - robot.state.q(k)) * noise;
-    }
-  }
-
   has_scaling = false;
 
   // Updating all the task matrices
@@ -359,6 +385,9 @@ Eigen::VectorXd KinematicsSolver::solve(bool apply)
 
   if (apply)
   {
+    // Initial robot configuration
+    auto q_save = robot.state.q;
+
     robot.state.q = pinocchio::integrate(robot.model, robot.state.q, qd_sol);
     if (dt > 0)
     {
@@ -367,10 +396,6 @@ Eigen::VectorXd KinematicsSolver::solve(bool apply)
       robot.state.qdd = (robot.state.qd - qd_save) / dt;
     }
   }
-  else
-  {
-    robot.state.q = q_save;
-  }
 
   return qd_sol;
 }

{placo-0.4.9 → 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
@@ -299,6 +315,12 @@ public:
    */
   Eigen::VectorXd solve(bool apply = false);
 
+  /**
+   * @brief Adds some noise on the configuration of the robot (q)
+   * @param noise noise level, expressed in ratio of the joint limits
+   */
+  void add_q_noise(double noise = 1e-5);
+
   /**
    * @brief Masks (disables a DoF) from being used by the QP solver (it can't provide speed)
    * @param dof the dof name
@@ -379,11 +401,6 @@ public:
    */
   int N;
 
-  /**
-   * @brief Some configuration noise added before solving
-   */
-  double noise = 1e-5;
-
   /**
    * @brief solver dt (for speeds limiting)
    */