placo 0.5.9__tar.gz → 0.6.1__tar.gz

placo-0.6.1/PKG-INFO

@@ -0,0 +1,56 @@
+Metadata-Version: 2.1
+Name: placo
+Version: 0.6.1
+Summary: PlaCo: Rhoban Planning and Control
+Requires-Python: >= 3.8
+License-Expression: MIT
+Author-email: Rhoban team <team@rhoban.com>
+Project-URL: Changelog, https://github.com/rhoban/placo/blob/main/CHANGELOG.md
+Home-page: https://placo.readthedocs.io/en/latest/
+Project-URL: Repository, https://github.com/rhoban/placo.git
+Requires-Dist: cmeel
+Requires-Dist: eiquadprog >= 1.2.6, < 2
+Requires-Dist: pin >= 2.6.18, < 3
+Requires-Dist: rhoban-cmeel-jsoncpp
+Requires-Dist: meshcat
+Requires-Dist: numpy<2
+Requires-Dist: ischedule
+Provides-Extra: build
+Requires-Dist: pin[build] >= 2.6.18, < 3 ; extra == "build"
+Requires-Dist: cmeel-urdfdom[build] ; extra == "build"
+Description-Content-Type: text/markdown
+
+# PlaCo
+
+PlaCo is Rhoban's planning and control library.
+Its main features are:
+
+* Task-space Inverse Kinematics with constraints,
+* Task-space Inverse Dynamics with constraints,
+* QP problem formulation,
+* Built on the top of [pinocchio](https://github.com/stack-of-tasks/pinocchio)
+* Written in C++ with Python bindings
+
+[![Megabot demo](https://github.com/Rhoban/placo-examples/blob/master/kinematics/videos/quadruped_targets.gif?raw=true)](https://github.com/Rhoban/placo-examples/blob/master/kinematics/videos/quadruped_targets.mp4?raw=true)
+
+*Inverse Kinematics Example: a quadruped robot hitting targets with a leg while keeping its three legs on the ground*
+
+[source code (quadruped_targets.py)](https://github.com/Rhoban/placo-examples/blob/master/kinematics/quadruped_targets.py) / [more kinematics examples](https://placo.readthedocs.io/en/latest/kinematics/examples_gallery.html)
+
+[![Megabot demo](https://github.com/Rhoban/placo-examples/blob/master/dynamics/videos/megabot.gif?raw=true)](https://github.com/Rhoban/placo-examples/blob/master/dynamics/videos/megabot.mp4?raw=true)
+
+*Inverse Dynamics Example: a quadruped with many loop closure joints*
+
+[source code (megabot.py)](https://github.com/Rhoban/placo-examples/blob/master/dynamics/megabot.py) / [more dynamics examples](https://placo.readthedocs.io/en/latest/dynamics/examples_gallery.html)
+
+## Installing
+
+PlaCo is available from [pip](https://placo.readthedocs.io/en/latest/basics/installation_pip.html),
+or can be [built from sources](https://placo.readthedocs.io/en/latest/basics/installation_source.html).
+
+## Documentation
+
+Here is the [official documentation](https://placo.readthedocs.io/en/latest/)
+
+You can also find many examples in the [placo-examples](https://github.com/rhoban/placo-examples)
+repository.

placo-0.6.1/README.md

@@ -0,0 +1,34 @@
+# PlaCo
+
+PlaCo is Rhoban's planning and control library.
+Its main features are:
+
+* Task-space Inverse Kinematics with constraints,
+* Task-space Inverse Dynamics with constraints,
+* QP problem formulation,
+* Built on the top of [pinocchio](https://github.com/stack-of-tasks/pinocchio)
+* Written in C++ with Python bindings
+
+[![Megabot demo](https://github.com/Rhoban/placo-examples/blob/master/kinematics/videos/quadruped_targets.gif?raw=true)](https://github.com/Rhoban/placo-examples/blob/master/kinematics/videos/quadruped_targets.mp4?raw=true)
+
+*Inverse Kinematics Example: a quadruped robot hitting targets with a leg while keeping its three legs on the ground*
+
+[source code (quadruped_targets.py)](https://github.com/Rhoban/placo-examples/blob/master/kinematics/quadruped_targets.py) / [more kinematics examples](https://placo.readthedocs.io/en/latest/kinematics/examples_gallery.html)
+
+[![Megabot demo](https://github.com/Rhoban/placo-examples/blob/master/dynamics/videos/megabot.gif?raw=true)](https://github.com/Rhoban/placo-examples/blob/master/dynamics/videos/megabot.mp4?raw=true)
+
+*Inverse Dynamics Example: a quadruped with many loop closure joints*
+
+[source code (megabot.py)](https://github.com/Rhoban/placo-examples/blob/master/dynamics/megabot.py) / [more dynamics examples](https://placo.readthedocs.io/en/latest/dynamics/examples_gallery.html)
+
+## Installing
+
+PlaCo is available from [pip](https://placo.readthedocs.io/en/latest/basics/installation_pip.html),
+or can be [built from sources](https://placo.readthedocs.io/en/latest/basics/installation_source.html).
+
+## Documentation
+
+Here is the [official documentation](https://placo.readthedocs.io/en/latest/)
+
+You can also find many examples in the [placo-examples](https://github.com/rhoban/placo-examples)
+repository.

{placo-0.5.9 → placo-0.6.1}/bindings/expose-dynamics.cpp

@@ -13,6 +13,7 @@ using namespace placo::dynamics;
 using namespace placo::model;
 
 // Overloads
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(frame_configure_overloads, configure, 2, 4);
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(solve_overloads, solve, 0, 1);
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(set_torque_overloads, set_torque, 2, 4);
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(set_joint_overloads, set_joint, 2, 4);
@@ -43,6 +44,7 @@ void exposeDynamics()
       .def_readwrite("active", &Contact::active)
       .def_readwrite("mu", &Contact::mu)
       .def_readwrite("weight_forces", &Contact::weight_forces)
+      .def_readwrite("weight_tangentials", &Contact::weight_tangentials)
       .def_readwrite("weight_moments", &Contact::weight_moments)
       .add_property(
           "wrench", +[](Contact& contact) { return contact.wrench; });
@@ -68,6 +70,20 @@ void exposeDynamics()
       .def_readwrite("width", &Contact6D::width)
       .def("zmp", &Contact6D::zmp);
 
+  class__<LineContact, bases<Contact>>("LineContact", init<FrameTask&, bool>())
+      .def(
+          "position_task", +[](LineContact& contact) -> PositionTask& { return *contact.position_task; },
+          return_internal_reference<>())
+      .def(
+          "orientation_task", +[](LineContact& contact) -> OrientationTask& { return *contact.orientation_task; },
+          return_internal_reference<>())
+      .def_readwrite("unilateral", &LineContact::unilateral)
+      .def_readwrite("length", &LineContact::length)
+      .def("zmp", &LineContact::zmp)
+      .add_property(
+          "R_world_surface", +[](PointContact& contact) { return contact.R_world_surface; },
+          +[](PointContact& contact, Eigen::Matrix3d R) { contact.R_world_surface = R; });
+
   class__<ExternalWrenchContact, bases<Contact>>("ExternalWrenchContact", init<RobotWrapper::FrameIndex>())
       .add_property("frame_index", &ExternalWrenchContact::frame_index)
       .add_property(
@@ -85,11 +101,16 @@ void exposeDynamics()
       .def("set_torque_limit", &DynamicsSolver::set_torque_limit)
       .def_readwrite("gravity_only", &DynamicsSolver::gravity_only)
       .def_readwrite("torque_cost", &DynamicsSolver::torque_cost)
+      .add_property(
+          "extra_force", +[](DynamicsSolver& solver) { return solver.extra_force; },
+          +[](DynamicsSolver& solver, Eigen::VectorXd force) { solver.extra_force = force; })
       .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<>())
       .def("add_planar_contact", &DynamicsSolver::add_planar_contact, return_internal_reference<>())
       .def("add_fixed_contact", &DynamicsSolver::add_fixed_contact, return_internal_reference<>())
+      .def("add_line_contact", &DynamicsSolver::add_line_contact, return_internal_reference<>())
+      .def("add_unilateral_line_contact", &DynamicsSolver::add_unilateral_line_contact, return_internal_reference<>())
       .def<ExternalWrenchContact& (DynamicsSolver::*)(std::string, std::string)>(
           "add_external_wrench_contact", &DynamicsSolver::add_external_wrench_contact, return_internal_reference<>())
       .def("add_puppet_contact", &DynamicsSolver::add_puppet_contact, return_internal_reference<>())
@@ -212,7 +233,7 @@ void exposeDynamics()
       .def(
           "orientation", +[](const FrameTask& task) -> OrientationTask& { return *task.orientation; },
           return_internal_reference<>())
-      .def("configure", &FrameTask::configure)
+      .def("configure", &FrameTask::configure, frame_configure_overloads())
       .add_property("T_world_frame", &FrameTask::get_T_world_frame, &FrameTask::set_T_world_frame);
 
   class__<RelativeFrameTask>("DynamicsRelativeFrameTask", init<>())
@@ -228,6 +249,7 @@ void exposeDynamics()
 
   class__<JointsTask, bases<Task>>("DynamicsJointsTask", init<>())
       .def("set_joint", &JointsTask::set_joint, set_joint_overloads())
+      .def("get_joint", &JointsTask::get_joint)
       .def(
           "set_joints", +[](JointsTask& task,
                             boost::python::dict& py_dict) { update_map<std::string, double>(task.joints, py_dict); })

{placo-0.5.9 → placo-0.6.1}/bindings/expose-kinematics.cpp

@@ -192,6 +192,7 @@ void exposeKinematics()
 
   class__<JointsTask, bases<Task>>("JointsTask", init<>())
       .def("set_joint", &JointsTask::set_joint)
+      .def("get_joint", &JointsTask::get_joint)
       .def(
           "set_joints", +[](JointsTask& task, boost::python::dict& py_dict) {
             update_map<std::string, double>(task.joints, py_dict);

{placo-0.5.9 → placo-0.6.1}/bindings/expose-problem.cpp

@@ -109,6 +109,7 @@ void exposeProblem()
       .def("clear_variables", &Problem::clear_variables)
       .def("clear_constraints", &Problem::clear_constraints)
       .def("dump_status", &Problem::dump_status)
+      .add_property("x", &Problem::x)
       .add_property("n_variables", &Problem::n_variables, &Problem::n_variables)
       .add_property("n_inequalities", &Problem::n_inequalities, &Problem::n_inequalities)
       .add_property("n_equalities", &Problem::n_equalities, &Problem::n_equalities)
@@ -117,6 +118,7 @@ void exposeProblem()
       .add_property("slack_variables", &Problem::slack_variables, &Problem::slack_variables)
       .add_property("use_sparsity", &Problem::use_sparsity, &Problem::use_sparsity)
       .add_property("rewrite_equalities", &Problem::rewrite_equalities, &Problem::rewrite_equalities)
+      .add_property("regularization", &Problem::regularization, &Problem::regularization)
       .add_property(
           "slacks", +[](const Problem& problem) { return problem.slacks; });
 
@@ -133,6 +135,7 @@ void exposeProblem()
           "b", +[](Expression& e) { return e.b; })
       .def("__len__", &Expression::rows)
       .def("is_scalar", &Expression::is_scalar)
+      .def("is_constant", &Expression::is_constant)
       .def("slice", &Expression::slice)
       .def("rows", &Expression::rows)
       .def("cols", &Expression::cols)
@@ -149,6 +152,7 @@ void exposeProblem()
       .def(self - self)
       .def(self * float())
       .def(float() * self)
+      .def(self * self)
       .def(self + other<Eigen::VectorXd>())
       .def(other<Eigen::VectorXd>() + self)
       .def(self - other<Eigen::VectorXd>())
@@ -158,7 +162,7 @@ void exposeProblem()
       .def(self - float())
       .def(float() - self)
       .def(other<Eigen::MatrixXd>() * self)
-      .def("multiply", &Expression::multiply)
+      .def("left_multiply", &Expression::left_multiply)
       // Compare to build constraints
       .def(self >= self)
       .def(self <= self)

{placo-0.5.9 → placo-0.6.1}/bindings/expose-robot-wrapper.cpp

@@ -61,6 +61,8 @@ void exposeRobotType(class_<RobotType, W1>& type)
       .def("com_jacobian_time_variation", &RobotType::com_jacobian_time_variation)
       .def("generalized_gravity", &RobotType::generalized_gravity)
       .def("non_linear_effects", &RobotType::non_linear_effects)
+      .def("set_rotor_inertia", &RobotType::set_rotor_inertia)
+      .def("set_gear_ratio", &RobotType::set_gear_ratio)
       .def("mass_matrix", &RobotType::mass_matrix)
       .def("set_gravity", &RobotType::set_gravity)
       .def("total_mass", &RobotType::total_mass)

{placo-0.5.9 → placo-0.6.1}/placo.pyi

@@ -51,6 +51,7 @@ KinematicsSolver = typing.NewType("KinematicsSolver", None)
 KineticEnergyRegularizationTask = typing.NewType("KineticEnergyRegularizationTask", None)
 LIPM = typing.NewType("LIPM", None)
 LIPMTrajectory = typing.NewType("LIPMTrajectory", None)
+LineContact = typing.NewType("LineContact", None)
 ManipulabilityTask = typing.NewType("ManipulabilityTask", None)
 OrientationTask = typing.NewType("OrientationTask", None)
 PointContact = typing.NewType("PointContact", None)
@@ -635,6 +636,10 @@ class Contact:
   """Weight of moments for optimization (if relevant) 
         """
 
+  weight_tangentials: float # double
+  """Extra cost for tangential forces. 
+        """
+
   wrench: numpy.ndarray # Eigen::VectorXd
   """Wrench populated after the DynamicsSolver::solve call. 
         """
@@ -688,6 +693,10 @@ class Contact6D:
   """Weight of moments for optimization (if relevant) 
         """
 
+  weight_tangentials: float # double
+  """Extra cost for tangential forces. 
+        """
+
   width: float # double
   """Rectangular contact width along local y-axis. 
         """
@@ -1271,6 +1280,19 @@ class DynamicsJointsTask:
   """Current error vector. 
         """
 
+  def get_joint(
+    self: DynamicsJointsTask,
+    joint: str, # std::string
+
+  ) -> float:
+    """Returns the current target position of a joint. 
+        
+
+    :param joint: joint name 
+
+    :return: current target position"""
+    ...
+
   kd: float # double
   """D gain for position control (if negative, will be critically damped) 
         """
@@ -1781,6 +1803,19 @@ class DynamicsSolver:
     :return: joints task"""
     ...
 
+  def add_line_contact(
+    self: DynamicsSolver,
+    frame_task: DynamicsFrameTask, # placo::dynamics::FrameTask
+
+  ) -> LineContact:
+    """Adds a fixed line contact. 
+        
+
+    :param frame_task: associated frame task 
+
+    :return: line contact"""
+    ...
+
   def add_orientation_task(
     self: DynamicsSolver,
     frame_name: str, # std::string
@@ -1942,6 +1977,19 @@ class DynamicsSolver:
     :return: torque task"""
     ...
 
+  def add_unilateral_line_contact(
+    self: DynamicsSolver,
+    frame_task: DynamicsFrameTask, # placo::dynamics::FrameTask
+
+  ) -> LineContact:
+    """Adds a unilateral line contact, which is unilateral in the sense of the local body z-axis. 
+        
+
+    :param frame_task: associated frame task 
+
+    :return: unilateral line contact"""
+    ...
+
   def add_unilateral_point_contact(
     self: DynamicsSolver,
     position_task: DynamicsPositionTask, # placo::dynamics::PositionTask
@@ -2015,6 +2063,10 @@ class DynamicsSolver:
     """Enables the velocity vs torque inequalities."""
     ...
 
+  extra_force: numpy.ndarray # Eigen::VectorXd
+  """Extra force to be added to the system (similar to non-linear terms) 
+        """
+
   def get_contact(
     arg1: DynamicsSolver,
     arg2: int,
@@ -2367,6 +2419,16 @@ class Expression:
     :return: expression"""
     ...
 
+  def is_constant(
+    self: Expression,
+
+  ) -> bool:
+    """checks if the expression is constant (doesn't depend on decision variables) 
+        
+
+    :return: true if the expression is constant"""
+    ...
+
   def is_scalar(
     self: Expression,
 
@@ -2377,26 +2439,26 @@ class Expression:
     :return: true if the expression is a scalar"""
     ...
 
-  def mean(
+  def left_multiply(
     self: Expression,
+    M: numpy.ndarray, # const Eigen::MatrixXd
 
   ) -> Expression:
-    """Reduces a multi-rows expression to the mean of its items. 
+    """Multiply an expression on the left by a given matrix M. 
         
 
+    :param M: matrix 
+
     :return: expression"""
     ...
 
-  def multiply(
+  def mean(
     self: Expression,
-    M: numpy.ndarray, # const Eigen::MatrixXd
 
   ) -> Expression:
-    """Multiply an expression on the left by a given matrix M. 
+    """Reduces a multi-rows expression to the mean of its items. 
         
 
-    :param M: matrix 
-
     :return: expression"""
     ...
 
@@ -2493,6 +2555,10 @@ class ExternalWrenchContact:
   """Weight of moments for optimization (if relevant) 
         """
 
+  weight_tangentials: float # double
+  """Extra cost for tangential forces. 
+        """
+
   wrench: numpy.ndarray # Eigen::VectorXd
   """Wrench populated after the DynamicsSolver::solve call. 
         """
@@ -3622,6 +3688,15 @@ class HumanoidRobot:
     :param T_world_frameTarget: transformation matrix"""
     ...
 
+  def set_gear_ratio(
+    self: HumanoidRobot,
+    joint_name: str, # const std::string &
+    rotor_gear_ratio: float, # double
+
+  ) -> None:
+    """Updates the rotor gear ratio (used for apparent inertia computation in the dynamics)"""
+    ...
+
   def set_gravity(
     self: HumanoidRobot,
     gravity: numpy.ndarray, # Eigen::Vector3d
@@ -3689,6 +3764,15 @@ class HumanoidRobot:
     :param value: joint velocity"""
     ...
 
+  def set_rotor_inertia(
+    self: HumanoidRobot,
+    joint_name: str, # const std::string &
+    rotor_inertia: float, # double
+
+  ) -> None:
+    """Updates the rotor inertia (used for apparent inertia computation in the dynamics)"""
+    ...
+
   def set_torque_limit(
     self: HumanoidRobot,
     name: str, # const std::string &
@@ -4075,6 +4159,19 @@ class JointsTask:
     :return: task error norm"""
     ...
 
+  def get_joint(
+    self: JointsTask,
+    joint: str, # std::string
+
+  ) -> float:
+    """Returns the target value of a joint. 
+        
+
+    :param joint: joint 
+
+    :return: target value"""
+    ...
+
   name: str # std::string
   """Object name. 
         """
@@ -4803,6 +4900,77 @@ class LIPMTrajectory:
     ...
 
 
+class LineContact:
+  R_world_surface: numpy.ndarray # Eigen::Matrix3d
+  """rotation matrix expressing the surface frame in the world frame (for unilateral contact) 
+        """
+
+  def __init__(
+    self: LineContact,
+    frame_task: DynamicsFrameTask, # placo::dynamics::FrameTask
+    unilateral: bool, # bool
+
+  ) -> any:
+    """see DynamicsSolver::add_fixed_planar_contact and DynamicsSolver::add_unilateral_planar_contact"""
+    ...
+
+  active: bool # bool
+  """true if the contact is active (ignored by the solver else, this allow to enable/disable a contact without removing it from the solver) 
+        """
+
+  length: float # double
+  """Rectangular contact length along local x-axis. 
+        """
+
+  mu: float # double
+  """Coefficient of friction (if relevant) 
+        """
+
+  def orientation_task(
+    self: LineContact,
+
+  ) -> DynamicsOrientationTask:
+    """Associated orientation task."""
+    ...
+
+  def position_task(
+    self: LineContact,
+
+  ) -> DynamicsPositionTask:
+    """Associated position task."""
+    ...
+
+  unilateral: bool # bool
+  """true for unilateral contact with the ground 
+        """
+
+  weight_forces: float # double
+  """Weight of forces for the optimization (if relevant) 
+        """
+
+  weight_moments: float # double
+  """Weight of moments for optimization (if relevant) 
+        """
+
+  weight_tangentials: float # double
+  """Extra cost for tangential forces. 
+        """
+
+  wrench: numpy.ndarray # Eigen::VectorXd
+  """Wrench populated after the DynamicsSolver::solve call. 
+        """
+
+  def zmp(
+    self: LineContact,
+
+  ) -> numpy.ndarray:
+    """Returns the contact ZMP in the local frame. 
+        
+
+    :return: zmp"""
+    ...
+
+
 class ManipulabilityTask:
   A: numpy.ndarray # Eigen::MatrixXd
   """Matrix A in the task Ax = b, where x are the joint delta positions. 
@@ -5008,6 +5176,10 @@ class PointContact:
   """Weight of moments for optimization (if relevant) 
         """
 
+  weight_tangentials: float # double
+  """Extra cost for tangential forces. 
+        """
+
   wrench: numpy.ndarray # Eigen::VectorXd
   """Wrench populated after the DynamicsSolver::solve call. 
         """
@@ -5252,6 +5424,10 @@ class Problem:
   """Number of problem variables that need to be solved. 
         """
 
+  regularization: float # double
+  """Default internal regularization. 
+        """
+
   rewrite_equalities: bool # bool
   """If set to true, a QR factorization will be performed on the equality constraints, and the QP will be called with free variables only. 
         """
@@ -5275,6 +5451,10 @@ class Problem:
   """If set to true, some sparsity optimizations will be performed when building the problem Hessian. This optimization is generally not useful for small problems. 
         """
 
+  x: numpy.ndarray # Eigen::VectorXd
+  """Computed result. 
+        """
+
 
 class ProblemConstraint:
   """Represents a constraint to be enforced by a Problem. 
@@ -5340,6 +5520,10 @@ class PuppetContact:
   """Weight of moments for optimization (if relevant) 
         """
 
+  weight_tangentials: float # double
+  """Extra cost for tangential forces. 
+        """
+
   wrench: numpy.ndarray # Eigen::VectorXd
   """Wrench populated after the DynamicsSolver::solve call. 
         """
@@ -6032,6 +6216,15 @@ class RobotWrapper:
     :param T_world_frameTarget: transformation matrix"""
     ...
 
+  def set_gear_ratio(
+    self: RobotWrapper,
+    joint_name: str, # const std::string &
+    rotor_gear_ratio: float, # double
+
+  ) -> None:
+    """Updates the rotor gear ratio (used for apparent inertia computation in the dynamics)"""
+    ...
+
   def set_gravity(
     self: RobotWrapper,
     gravity: numpy.ndarray, # Eigen::Vector3d
@@ -6099,6 +6292,15 @@ class RobotWrapper:
     :param value: joint velocity"""
     ...
 
+  def set_rotor_inertia(
+    self: RobotWrapper,
+    joint_name: str, # const std::string &
+    rotor_inertia: float, # double
+
+  ) -> None:
+    """Updates the rotor inertia (used for apparent inertia computation in the dynamics)"""
+    ...
+
   def set_torque_limit(
     self: RobotWrapper,
     name: str, # const std::string &
@@ -6623,6 +6825,10 @@ class TaskContact:
   """Weight of moments for optimization (if relevant) 
         """
 
+  weight_tangentials: float # double
+  """Extra cost for tangential forces. 
+        """
+
   wrench: numpy.ndarray # Eigen::VectorXd
   """Wrench populated after the DynamicsSolver::solve call. 
         """
@@ -7332,4 +7538,4 @@ def wrap_angle(
   ...
 
 
-__groups__ = {'placo::dynamics': ['AvoidSelfCollisionsDynamicsConstraint', 'Contact', 'Contact6D', 'DynamicsCoMTask', 'DynamicsConstraint', 'DynamicsFrameTask', 'DynamicsGearTask', 'DynamicsJointsTask', 'DynamicsOrientationTask', 'DynamicsPositionTask', 'DynamicsRelativeFrameTask', 'DynamicsRelativeOrientationTask', 'DynamicsRelativePositionTask', 'DynamicsSolver', 'DynamicsSolverResult', 'DynamicsTask', 'DynamicsTorqueTask', 'ExternalWrenchContact', 'PointContact', 'PuppetContact', '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']}
+__groups__ = {'placo::dynamics': ['AvoidSelfCollisionsDynamicsConstraint', 'Contact', 'Contact6D', 'DynamicsCoMTask', 'DynamicsConstraint', 'DynamicsFrameTask', 'DynamicsGearTask', 'DynamicsJointsTask', 'DynamicsOrientationTask', 'DynamicsPositionTask', 'DynamicsRelativeFrameTask', 'DynamicsRelativeOrientationTask', 'DynamicsRelativePositionTask', 'DynamicsSolver', 'DynamicsSolverResult', 'DynamicsTask', 'DynamicsTorqueTask', 'ExternalWrenchContact', 'LineContact', 'PointContact', 'PuppetContact', '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.9 → placo-0.6.1}/pyproject.toml

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

{placo-0.5.9 → placo-0.6.1}/python/placo_utils/visualization.py

@@ -243,7 +243,7 @@ def contacts_viz(solver: placo.DynamicsSolver, ratio=0.1, radius=0.005):
                 origin = T_world_frame[:3, 3]
             else:
                 origin = T_world_frame[:3, 3] + T_world_frame[:3, :3] @ contact.zmp()
-            
+
             arrow_viz(
                 f"contact_{k}",
                 origin,
@@ -251,6 +251,23 @@ def contacts_viz(solver: placo.DynamicsSolver, ratio=0.1, radius=0.005):
                 color=0x00FFAA,
                 radius=radius,
             )
+        elif isinstance(contact, placo.LineContact):
+            frame_name = frames[contact.position_task().frame_index]
+            T_world_frame = robot.get_T_world_frame(frame_name)
+            wrench = T_world_frame[:3, :3] @ contact.wrench[:3]
+
+            if np.linalg.norm(wrench) < 1e-6:
+                origin = T_world_frame[:3, 3]
+            else:
+                origin = T_world_frame[:3, 3] + T_world_frame[:3, :3] @ contact.zmp()
+
+            arrow_viz(
+                f"contact_{k}",
+                origin,
+                origin + wrench * ratio,
+                color=0xFF33AA,
+                radius=radius,
+            )
         elif isinstance(contact, placo.ExternalWrenchContact):
             frame_name = frames[contact.frame_index]
             T_world_frame = robot.get_T_world_frame(frame_name)

{placo-0.5.9 → placo-0.6.1}/src/placo/dynamics/avoid_self_collisions_constraint.cpp

@@ -60,20 +60,12 @@ void AvoidSelfCollisionsConstraint::add_constraint(problem::Problem& problem, pr
       Eigen::VectorXd dJ = n.transpose() * (dJB - dJA).block(0, 0, 3, solver->N) * solver->robot.state.qd;
 
       // Computing xdd_safe from qdd_safe
-      double lambda = -1;
+      double xdd_safe = 0.0;
       for (int k = 6; k < solver->N; k++)
       {
-        if (fabs(J(0, k)) > 1e-6)
-        {
-          double lambda_i = fabs(solver->qdd_safe[k] / J(0, k));
-          if (lambda < 0 || lambda_i < lambda)
-          {
-            lambda = lambda_i;
-          }
-        }
+        xdd_safe += fabs(J(0, k)) * solver->qdd_safe[k];
       }
-
-      double xdd_safe = (lambda * (J * J.transpose()))(0, 0) + dJ[0];
+      xdd_safe = 0.5 * xdd_safe;
 
       if (distance.min_distance >= self_collisions_margin)
       {