placo 0.4.5__tar.gz → 0.4.7__tar.gz

{placo-0.4.5 → placo-0.4.7}/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/kinetic_energy_regularization_task.cpp
     src/placo/kinematics/constraint.cpp
     src/placo/kinematics/avoid_self_collisions_constraint.cpp
     src/placo/kinematics/com_polygon_constraint.cpp
@@ -93,6 +94,7 @@ add_library(libplaco SHARED
     src/placo/dynamics/relative_orientation_task.cpp
     src/placo/dynamics/relative_frame_task.cpp
     src/placo/dynamics/joints_task.cpp
+    src/placo/dynamics/torque_task.cpp
     src/placo/dynamics/gear_task.cpp
     src/placo/dynamics/com_task.cpp
     src/placo/dynamics/contacts.cpp

{placo-0.4.5 → placo-0.4.7}/PKG-INFO

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

{placo-0.4.5 → placo-0.4.7}/bindings/expose-dynamics.cpp

@@ -12,6 +12,10 @@ using namespace placo;
 using namespace placo::dynamics;
 using namespace placo::model;
 
+// Overloads
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(set_passive_overloads, set_passive, 1, 4);
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(solve_overloads, solve, 0, 1);
+
 void exposeDynamics()
 {
   class__<DynamicsSolver::Result>("DynamicsSolverResult")
@@ -75,7 +79,8 @@ void exposeDynamics()
       .def_readwrite("friction", &DynamicsSolver::friction)
       .def_readwrite("dt", &DynamicsSolver::dt)
       .def_readwrite("qdd_safe", &DynamicsSolver::qdd_safe)
-      .def_readwrite("optimize_contact_forces", &DynamicsSolver::optimize_contact_forces)
+      .def_readwrite("gravity_only", &DynamicsSolver::gravity_only)
+      .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_relative_point_contact", &DynamicsSolver::add_relative_point_contact, return_internal_reference<>())
@@ -90,14 +95,13 @@ void exposeDynamics()
            return_internal_reference<>())
       .def("add_reaction_ratio_constraint", &DynamicsSolver::add_reaction_ratio_constraint,
            return_internal_reference<>())
-      .def("set_passive", &DynamicsSolver::set_passive)
+      .def("set_passive", &DynamicsSolver::set_passive, set_passive_overloads())
       .def("enable_velocity_limits", &DynamicsSolver::enable_velocity_limits)
       .def("enable_velocity_vs_torque_limits", &DynamicsSolver::enable_velocity_vs_torque_limits)
       .def("enable_joint_limits", &DynamicsSolver::enable_joint_limits)
       .def("enable_torque_limits", &DynamicsSolver::enable_torque_limits)
       .def("dump_status", &DynamicsSolver::dump_status)
-      .def("set_static", &DynamicsSolver::set_static)
-      .def("solve", &DynamicsSolver::solve)
+      .def("solve", &DynamicsSolver::solve, solve_overloads())
       .def<void (DynamicsSolver::*)(Task&)>("add_task", &DynamicsSolver::add_task)
       .def<void (DynamicsSolver::*)(Constraint&)>("add_constraint", &DynamicsSolver::add_constraint)
       .def<void (DynamicsSolver::*)(Constraint&)>("add_constraint", &DynamicsSolver::add_constraint)
@@ -124,6 +128,8 @@ void exposeDynamics()
           "add_relative_frame_task", &DynamicsSolver::add_relative_frame_task)
       .def<JointsTask& (DynamicsSolver::*)()>("add_joints_task", &DynamicsSolver::add_joints_task,
                                               return_internal_reference<>())
+      .def<TorqueTask& (DynamicsSolver::*)()>("add_torque_task", &DynamicsSolver::add_torque_task,
+                                              return_internal_reference<>())
       .def<GearTask& (DynamicsSolver::*)()>("add_gear_task", &DynamicsSolver::add_gear_task,
                                             return_internal_reference<>())
       .def<CoMTask& (DynamicsSolver::*)(Eigen::Vector3d)>("add_com_task", &DynamicsSolver::add_com_task,
@@ -227,7 +233,11 @@ void exposeDynamics()
             update_map<std::string, double>(task.djoints, py_dict);
           });
 
-  class__<GearTask, bases<Task>>("DynamicsGearTask", init<>()).def("set_gear", &GearTask::set_gear);
+  class__<TorqueTask, bases<Task>>("DynamicsTorqueTask", init<>()).def("set_torque", &TorqueTask::set_torque);
+
+  class__<GearTask, bases<Task>>("DynamicsGearTask", init<>())
+      .def("set_gear", &GearTask::set_gear)
+      .def("add_gear", &GearTask::add_gear);
 
   class__<Constraint, bases<tools::Prioritized>, boost::noncopyable>("DynamicsConstraint", no_init);
 

{placo-0.4.5 → placo-0.4.7}/bindings/expose-kinematics.cpp

@@ -77,6 +77,10 @@ void exposeKinematics()
           // Regularization task
           .def("add_regularization_task", &KinematicsSolver::add_regularization_task, return_internal_reference<>())
 
+          // Kinetic energy regularization task
+          .def("add_kinetic_energy_regularization_task", &KinematicsSolver::add_kinetic_energy_regularization_task,
+               return_internal_reference<>())
+
           // Avoid self collisions constraint
           .def("add_avoid_self_collisions_constraint", &KinematicsSolver::add_avoid_self_collisions_constraint,
                return_internal_reference<>())
@@ -190,7 +194,9 @@ void exposeKinematics()
             update_map<std::string, double>(task.joints, py_dict);
           });
 
-  class__<GearTask, bases<Task>>("GearTask", init<>()).def("set_gear", &GearTask::set_gear);
+  class__<GearTask, bases<Task>>("GearTask", init<>())
+      .def("set_gear", &GearTask::set_gear)
+      .def("add_gear", &GearTask::add_gear);
 
   class__<WheelTask, bases<Task>>("WheelTask", init<std::string, double, bool>())
       .add_property("joint", &WheelTask::joint)
@@ -210,6 +216,8 @@ void exposeKinematics()
 
   class__<RegularizationTask, bases<Task>>("RegularizationTask");
 
+  class__<KineticEnergyRegularizationTask, bases<RegularizationTask>>("KineticEnergyRegularizationTask");
+
   class__<Constraint, bases<tools::Prioritized>, boost::noncopyable>("KinematicsConstraint", no_init);
 
   class__<AvoidSelfCollisionsConstraint, bases<Constraint>>("AvoidSelfCollisionsKinematicsConstraint", init<>())

{placo-0.4.5 → placo-0.4.7}/bindings/expose-robot-wrapper.cpp

@@ -55,6 +55,7 @@ void exposeRobotType(class_<RobotType, W1>& type)
       .def("generalized_gravity", &RobotType::generalized_gravity)
       .def("non_linear_effects", &RobotType::non_linear_effects)
       .def("mass_matrix", &RobotType::mass_matrix)
+      .def("set_gravity", &RobotType::set_gravity)
       .def("total_mass", &RobotType::total_mass)
       .def("integrate", &RobotType::integrate)
       .def(
@@ -172,6 +173,24 @@ void exposeRobotWrapper()
       .def("dcm", &HumanoidRobot::dcm)
       .def("zmp", &HumanoidRobot::zmp)
       .def("other_side", &HumanoidRobot::other_side)
+      .def(
+          "get_torques",
+          +[](HumanoidRobot& robot, Eigen::VectorXd qdd_a, Eigen::VectorXd contact_forces) {
+            return robot.get_torques(qdd_a, contact_forces);
+          })
+      .def(
+          "get_torques_dict",
+            +[](HumanoidRobot& robot, Eigen::VectorXd qdd_a, Eigen::VectorXd contact_forces) {
+            auto torques = robot.get_torques(qdd_a, contact_forces);
+            boost::python::dict dict;
+
+            for (auto& dof : robot.joint_names())
+            {
+              dict[dof] = torques[robot.get_joint_v_offset(dof) - 6];
+            }
+            
+            return dict;
+          })
 #ifdef HAVE_RHOBAN_UTILS
       .def("read_from_histories", &HumanoidRobot::read_from_histories, read_from_histories_overloads())
 #endif

{placo-0.4.5 → placo-0.4.7}/bindings/expose-tools.cpp

@@ -83,6 +83,7 @@ void exposeTools()
       .def("biggestTimestamp", &HistoryCollection::biggestTimestamp)
       .def("startNamedLog", &HistoryCollection::startNamedLog)
       .def("stopNamedLog", &HistoryCollection::stopNamedLog)
+      .def("getTimestamps", &HistoryCollection::getTimestamps)
       .def(
           "number", +[](HistoryCollection& collection, std::string name,
                         double t) { return collection.number(name)->interpolate(t); })

{placo-0.4.5 → placo-0.4.7}/placo.pyi

@@ -29,6 +29,7 @@ DynamicsRelativePositionTask = typing.NewType("DynamicsRelativePositionTask", No
 DynamicsSolver = typing.NewType("DynamicsSolver", None)
 DynamicsSolverResult = typing.NewType("DynamicsSolverResult", None)
 DynamicsTask = typing.NewType("DynamicsTask", None)
+DynamicsTorqueTask = typing.NewType("DynamicsTorqueTask", None)
 Exception = typing.NewType("Exception", None)
 Expression = typing.NewType("Expression", None)
 ExternalWrenchContact = typing.NewType("ExternalWrenchContact", None)
@@ -48,6 +49,7 @@ IntegratorTrajectory = typing.NewType("IntegratorTrajectory", None)
 JointsTask = typing.NewType("JointsTask", None)
 KinematicsConstraint = typing.NewType("KinematicsConstraint", None)
 KinematicsSolver = typing.NewType("KinematicsSolver", None)
+KineticEnergyRegularizationTask = typing.NewType("KineticEnergyRegularizationTask", None)
 LIPM = typing.NewType("LIPM", None)
 LIPMTrajectory = typing.NewType("LIPMTrajectory", None)
 OrientationTask = typing.NewType("OrientationTask", None)
@@ -1113,6 +1115,23 @@ class DynamicsGearTask:
   ) -> None:
     ...
 
+  def add_gear(
+    self: DynamicsGearTask,
+    target: str, # std::string
+    source: str, # std::string
+    ratio: float, # double
+
+  ) -> None:
+    """Adds a gear constraint, you can add multiple source for the same target, they will be summed. 
+        
+
+    :param target: target joint 
+
+    :param source: source joint 
+
+    :param ratio: ratio"""
+    ...
+
   b: numpy.ndarray # Eigen::MatrixXd
   """b vector in Ax = b, where x is the accelerations 
         """
@@ -1779,8 +1798,6 @@ class DynamicsSolver:
     """Adds a joints task. 
         
 
-    :param target: target joints values 
-
     :return: joints task"""
     ...
 
@@ -1977,6 +1994,16 @@ class DynamicsSolver:
     :return: task contact"""
     ...
 
+  def add_torque_task(
+    self: DynamicsSolver,
+
+  ) -> DynamicsTorqueTask:
+    """Adds a torque task. 
+        
+
+    :return: torque task"""
+    ...
+
   def add_unilateral_point_contact(
     self: DynamicsSolver,
     position_task: DynamicsPositionTask, # placo::dynamics::PositionTask
@@ -2057,10 +2084,18 @@ class DynamicsSolver:
   ) -> Contact:
     ...
 
-  optimize_contact_forces: bool # bool
-  """If true, the solver will try to optimize the contact forces by removing variables. 
+  gravity_only: bool # bool
+  """Use gravity only (no coriolis, no centrifugal) 
         """
 
+  def mask_fbase(
+    self: DynamicsSolver,
+    masked: bool, # bool
+
+  ) -> None:
+    """Decides if the floating base should be masked."""
+    ...
+
   problem: Problem # placo::problem::Problem
   """Instance of the problem. 
         """
@@ -2124,19 +2159,9 @@ class DynamicsSolver:
     :param kd: kd gain if the joint is a spring (0 by default)"""
     ...
 
-  def set_static(
-    self: DynamicsSolver,
-    is_static: bool, # bool
-
-  ) -> None:
-    """Sets the robot as static, this will impose the joint accelerations to be zero. 
-        
-
-    :param is_static: whether the robot should be static"""
-    ...
-
   def solve(
     self: DynamicsSolver,
+    integrate: bool = False, # bool
 
   ) -> DynamicsSolverResult:
     ...
@@ -2223,6 +2248,81 @@ class DynamicsTask:
         """
 
 
+class DynamicsTorqueTask:
+  A: numpy.ndarray # Eigen::MatrixXd
+  """A matrix in Ax = b, where x is the accelerations. 
+        """
+
+  def __init__(
+    arg1: object,
+
+  ) -> None:
+    ...
+
+  b: numpy.ndarray # Eigen::MatrixXd
+  """b vector in Ax = b, where x is the accelerations 
+        """
+
+  def configure(
+    self: DynamicsTorqueTask,
+    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"""
+    ...
+
+  critically_damped: bool # bool
+  """If this is true, kd will be computed from kp to have a critically damped system. 
+        """
+
+  derror: numpy.ndarray # Eigen::MatrixXd
+  """Current velocity error vector. 
+        """
+
+  error: numpy.ndarray # Eigen::MatrixXd
+  """Current error vector. 
+        """
+
+  kd: float # double
+  """D gain for position control. 
+        """
+
+  kp: float # double
+  """K gain for position control. 
+        """
+
+  name: str # std::string
+  """Object name. 
+        """
+
+  priority: any # placo::kinematics::ConeConstraint::Priority
+  """Object priority. 
+        """
+
+  def set_torque(
+    self: DynamicsTorqueTask,
+    joint: str, # std::string
+    torque: float, # double
+
+  ) -> None:
+    """Sets the target for a given joint. 
+        
+
+    :param joint: joint name 
+
+    :param torque: target torque"""
+    ...
+
+
 class Exception:
   def __init__(
     arg1: object,
@@ -2810,6 +2910,23 @@ class GearTask:
     """see KinematicsSolver::add_gear_task"""
     ...
 
+  def add_gear(
+    self: GearTask,
+    target: str, # std::string
+    source: str, # std::string
+    ratio: float, # double
+
+  ) -> None:
+    """Adds a gear constraint, you can add multiple source for the same target, they will be summed. 
+        
+
+    :param target: target joint 
+
+    :param source: source joint 
+
+    :param ratio: ratio"""
+    ...
+
   b: numpy.ndarray # Eigen::MatrixXd
   """Vector b in the task Ax = b, where x are the joint delta positions. 
         """
@@ -3316,6 +3433,30 @@ class HumanoidRobot:
   ) -> HumanoidRobot_Side:
     ...
 
+  def get_torques(
+    self: HumanoidRobot,
+    acc_a: numpy.ndarray, # Eigen::VectorXd
+    contact_forces: numpy.ndarray, # Eigen::VectorXd
+
+  ) -> numpy.ndarray:
+    """Compute the torques of the motors from the contact forces. 
+        
+
+    :param acc_a: Accelerations of the actuated DoFs 
+
+    :param contact_forces: Contact forces from the feet (forces are supposed normal to the ground) 
+
+    :return: Torques of the motors"""
+    ...
+
+  def get_torques_dict(
+    arg1: HumanoidRobot,
+    arg2: numpy.ndarray,
+    arg3: numpy.ndarray,
+
+  ) -> dict:
+    ...
+
   def integrate(
     self: HumanoidRobot,
     dt: float, # double
@@ -3443,6 +3584,14 @@ class HumanoidRobot:
     :param T_world_frameTarget: transformation matrix"""
     ...
 
+  def set_gravity(
+    self: HumanoidRobot,
+    gravity: numpy.ndarray, # Eigen::Vector3d
+
+  ) -> None:
+    """Sets the gravity vector."""
+    ...
+
   def set_joint(
     self: HumanoidRobot,
     name: str, # const std::string &
@@ -4125,6 +4274,19 @@ class KinematicsSolver:
     :return: joints task"""
     ...
 
+  def add_kinetic_energy_regularization_task(
+    self: KinematicsSolver,
+    magnitude: float = 1e-6, # double
+
+  ) -> KineticEnergyRegularizationTask:
+    """Adds a kinetic energy regularization task for a given magnitude. 
+        
+
+    :param magnitude: regularization magnitude 
+
+    :return: regularization task"""
+    ...
+
   def add_orientation_task(
     self: KinematicsSolver,
     frame: str, # std::string
@@ -4376,6 +4538,74 @@ class KinematicsSolver:
     ...
 
 
+class KineticEnergyRegularizationTask:
+  A: numpy.ndarray # Eigen::MatrixXd
+  """Matrix A in the task Ax = b, where x are the joint delta positions. 
+        """
+
+  def __init__(
+    arg1: object,
+
+  ) -> None:
+    ...
+
+  b: numpy.ndarray # Eigen::MatrixXd
+  """Vector b in the task Ax = b, where x are the joint delta positions. 
+        """
+
+  def configure(
+    self: KineticEnergyRegularizationTask,
+    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: KineticEnergyRegularizationTask,
+
+  ) -> numpy.ndarray:
+    """Task errors (vector) 
+        
+
+    :return: task errors"""
+    ...
+
+  def error_norm(
+    self: KineticEnergyRegularizationTask,
+
+  ) -> float:
+    """The task error norm. 
+        
+
+    :return: task error norm"""
+    ...
+
+  name: str # std::string
+  """Object name. 
+        """
+
+  priority: any # placo::kinematics::ConeConstraint::Priority
+  """Object priority. 
+        """
+
+  def update(
+    self: KineticEnergyRegularizationTask,
+
+  ) -> None:
+    """Update the task A and b matrices from the robot state and targets."""
+    ...
+
+
 class LIPM:
   """LIPM is an helper that can be used to build problem involving LIPM dynamics. The decision variables introduced here are jerks, which is piecewise constant. 
     """
@@ -5660,6 +5890,14 @@ class RobotWrapper:
     :param T_world_frameTarget: transformation matrix"""
     ...
 
+  def set_gravity(
+    self: RobotWrapper,
+    gravity: numpy.ndarray, # Eigen::Vector3d
+
+  ) -> None:
+    """Sets the gravity vector."""
+    ...
+
   def set_joint(
     self: RobotWrapper,
     name: str, # const std::string &
@@ -6964,4 +7202,4 @@ def wrap_angle(
   ...
 
 
-__groups__ = {'placo::dynamics': ['AvoidSelfCollisionsDynamicsConstraint', 'Contact', 'Contact6D', 'DynamicsCoMTask', 'DynamicsConstraint', 'DynamicsFrameTask', 'DynamicsGearTask', 'DynamicsJointsTask', 'DynamicsOrientationTask', 'DynamicsPositionTask', 'DynamicsReactionRatioConstraint', 'DynamicsRelativeFrameTask', 'DynamicsRelativeOrientationTask', 'DynamicsRelativePositionTask', 'DynamicsSolver', 'DynamicsSolverResult', 'DynamicsTask', 'ExternalWrenchContact', 'PointContact', 'PuppetContact', 'Relative6DContact', 'RelativePointContact', 'TaskContact'], 'placo::kinematics': ['AvoidSelfCollisionsKinematicsConstraint', 'AxisAlignTask', 'CentroidalMomentumTask', 'CoMPolygonConstraint', 'CoMTask', 'ConeConstraint', 'DistanceTask', 'FrameTask', 'GearTask', 'JointsTask', 'KinematicsConstraint', 'KinematicsSolver', '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', '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.5 → placo-0.4.7}/pyproject.toml

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