placo 0.5.4__tar.gz → 0.5.6__tar.gz

{placo-0.5.4 → placo-0.5.6}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: placo
-Version: 0.5.4
+Version: 0.5.6
 Summary: PlaCo: Rhoban Planning and Control
 Requires-Python: >= 3.8
 License-Expression: MIT
@@ -13,7 +13,7 @@ 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
+Requires-Dist: numpy<2
 Requires-Dist: ischedule
 Provides-Extra: build
 Requires-Dist: pin[build] >= 2.6.18, < 3 ; extra == "build"

{placo-0.5.4 → placo-0.5.6}/bindings/expose-dynamics.cpp

@@ -13,8 +13,8 @@ using namespace placo::dynamics;
 using namespace placo::model;
 
 // Overloads
-BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(set_passive_overloads, set_passive, 1, 3);
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(solve_overloads, solve, 0, 1);
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(set_torque_overloads, set_torque, 2, 4);
 
 void exposeDynamics()
 {
@@ -79,7 +79,7 @@ void exposeDynamics()
 
   class__<DynamicsSolver>("DynamicsSolver", init<RobotWrapper&>())
       .add_property("problem", &DynamicsSolver::problem)
-      .def_readwrite("friction", &DynamicsSolver::friction)
+      .def_readwrite("damping", &DynamicsSolver::damping)
       .def_readwrite("dt", &DynamicsSolver::dt)
       .def_readwrite("qdd_safe", &DynamicsSolver::qdd_safe)
       .def_readwrite("gravity_only", &DynamicsSolver::gravity_only)
@@ -99,9 +99,6 @@ void exposeDynamics()
            return_internal_reference<>())
       .def("add_reaction_ratio_constraint", &DynamicsSolver::add_reaction_ratio_constraint,
            return_internal_reference<>())
-      .def("set_passive", &DynamicsSolver::set_passive, set_passive_overloads())
-      .def("set_tau", &DynamicsSolver::set_tau)
-      .def("reset_joint", &DynamicsSolver::reset_joint)
       .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)
@@ -116,6 +113,8 @@ void exposeDynamics()
       .def<void (DynamicsSolver::*)(FrameTask&)>("remove_task", &DynamicsSolver::remove_task)
       .def("remove_contact", &DynamicsSolver::remove_contact)
       .def("remove_constraint", &DynamicsSolver::remove_constraint)
+      .def("set_kp", &DynamicsSolver::set_kp)
+      .def("set_kd", &DynamicsSolver::set_kd)
       .add_property(
           "robot", +[](const DynamicsSolver& solver) { return solver.robot; })
       .def(
@@ -239,7 +238,9 @@ void exposeDynamics()
             update_map<std::string, double>(task.djoints, py_dict);
           });
 
-  class__<TorqueTask, bases<Task>>("DynamicsTorqueTask", init<>()).def("set_torque", &TorqueTask::set_torque);
+  class__<TorqueTask, bases<Task>>("DynamicsTorqueTask", init<>())
+      .def("set_torque", &TorqueTask::set_torque, set_torque_overloads())
+      .def("reset_torque", &TorqueTask::reset_torque);
 
   class__<GearTask, bases<Task>>("DynamicsGearTask", init<>())
       .def("set_gear", &GearTask::set_gear)

{placo-0.5.4 → placo-0.5.6}/bindings/expose-robot-wrapper.cpp

@@ -52,6 +52,7 @@ void exposeRobotType(class_<RobotType, W1>& type)
       .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)
+      .def("centroidal_map", &RobotType::centroidal_map)
       .def("joint_names", &RobotType::joint_names, joint_names_overloads())
       .def("frame_names", &RobotType::frame_names)
       .def("self_collisions", &RobotType::self_collisions)

{placo-0.5.4 → placo-0.5.6}/bindings/expose-tools.cpp

@@ -31,6 +31,7 @@ void exposeTools()
   def("rotation_from_axis", &rotation_from_axis);
   def("frame_yaw", &frame_yaw);
   def("flatten_on_floor", &flatten_on_floor);
+  def("optimal_transformation", &optimal_transformation);
 
   exposeStdVector<int>("vector_int");
   exposeStdVector<double>("vector_double");

{placo-0.5.4 → placo-0.5.6}/bindings/expose-walk-pattern-generator.cpp

@@ -110,8 +110,10 @@ void exposeWalkPatternGenerator()
       .add_property("trunk_mode", &WalkTasks::trunk_mode, &WalkTasks::trunk_mode)
       .add_property("com_x", &WalkTasks::com_x, &WalkTasks::com_x)
       .add_property("com_y", &WalkTasks::com_y, &WalkTasks::com_y)
-      .add_property(
-          "trunk_orientation_task", +[](WalkTasks& tasks) { return *tasks.trunk_orientation_task; });
+      .add_property("trunk_orientation_task",
+                    make_function(
+                        +[](WalkTasks& tasks) -> OrientationTask& { return *tasks.trunk_orientation_task; },
+                        return_value_policy<reference_existing_object>()));
 
   class__<LIPM::Trajectory>("LIPMTrajectory", init<>())
       .def("pos", &LIPM::Trajectory::pos)

{placo-0.5.4 → placo-0.5.6}/placo.pyi

@@ -266,7 +266,7 @@ class AxisesMask:
   """Used to mask some task axises. 
     """
   R_custom_world: numpy.ndarray # Eigen::Matrix3d
-  """ROtation from world to custom rotation (provided by the user) 
+  """Rotation from world to custom rotation (provided by the user) 
         """
 
   R_local_world: numpy.ndarray # Eigen::Matrix3d
@@ -2047,6 +2047,10 @@ class DynamicsSolver:
   ) -> int:
     ...
 
+  damping: float # double
+  """Global damping that is added to all the joints. 
+        """
+
   dt: float # double
   """Solver dt (seconds) 
         """
@@ -2090,10 +2094,6 @@ class DynamicsSolver:
     """Enables the velocity vs torque inequalities."""
     ...
 
-  friction: float # double
-  """Global friction that is added to all the joints. 
-        """
-
   def get_contact(
     arg1: DynamicsSolver,
     arg2: int,
@@ -2154,50 +2154,28 @@ class DynamicsSolver:
     :param task: frame task"""
     ...
 
-  def reset_joint(
-    self: DynamicsSolver,
-    joint_name: str, # const std::string &
-
-  ) -> None:
-    """Resets a given joint so that its torque is no longer overriden. 
-        
-
-    :param joint_name: the joint"""
-    ...
-
   robot: RobotWrapper # placo::model::RobotWrapper
 
-  def set_passive(
+  def set_kd(
     self: DynamicsSolver,
-    joint_name: str, # const std::string &
-    kp: float = 0., # double
-    kd: float = 0., # double
+    kd: float, # double
 
   ) -> None:
-    """Sets a DoF as passive, the corresponding tau will be fixed in the equation of motion it can be purely passive joint or a spring-like behaviour. 
+    """Set the kp for all tasks. 
         
 
-    :param joint_name: the joint 
-
-    :param is_passive: true if the should the joint be passive 
-
-    :param kp: kp gain if the joint is a spring (0 by default) 
-
-    :param kd: kd gain if the joint is a spring (0 by default)"""
+    :param kpd:"""
     ...
 
-  def set_tau(
+  def set_kp(
     self: DynamicsSolver,
-    joint_name: str, # const std::string &
-    tau: float, # double
+    kp: float, # double
 
   ) -> None:
-    """Sets a custom torque to be applied by a given joint. 
+    """Set the kp for all tasks. 
         
 
-    :param joint_name: the joint 
-
-    :param tau: torque"""
+    :param kp:"""
     ...
 
   def solve(
@@ -2355,10 +2333,23 @@ class DynamicsTorqueTask:
   """Object priority. 
         """
 
+  def reset_torque(
+    self: DynamicsTorqueTask,
+    joint: str, # std::string
+
+  ) -> None:
+    """Removes a joint from this task. 
+        
+
+    :param joint: joint namle"""
+    ...
+
   def set_torque(
     self: DynamicsTorqueTask,
     joint: str, # std::string
     torque: float, # double
+    kp: float = 0.0, # double
+    kd: float = 0.0, # double
 
   ) -> None:
     """Sets the target for a given joint. 
@@ -2366,7 +2357,11 @@ class DynamicsTorqueTask:
 
     :param joint: joint name 
 
-    :param torque: target torque"""
+    :param torque: target torque 
+
+    :param kp: proportional gain (optional) 
+
+    :param kd: derivative gain (optional)"""
     ...
 
 
@@ -3232,6 +3227,16 @@ class HumanoidRobot:
   ) -> any:
     ...
 
+  def centroidal_map(
+    self: HumanoidRobot,
+
+  ) -> numpy.ndarray:
+    """Centroidal map. 
+        
+
+    :return: jacobian (6 x n matrix)"""
+    ...
+
   collision_model: any # pinocchio::GeometryModel
   """Pinocchio collision model. 
         """
@@ -5789,6 +5794,16 @@ class RobotWrapper:
     :param urdf_content: if it is not empty, it will be used as the URDF content instead of loading it from the file"""
     ...
 
+  def centroidal_map(
+    self: RobotWrapper,
+
+  ) -> numpy.ndarray:
+    """Centroidal map. 
+        
+
+    :return: jacobian (6 x n matrix)"""
+    ...
+
   collision_model: any # pinocchio::GeometryModel
   """Pinocchio collision model. 
         """
@@ -7213,6 +7228,15 @@ class map_string_double:
     ...
 
 
+def optimal_transformation(
+  points_in_A: numpy.ndarray, # Eigen::MatrixXd
+  points_in_B: numpy.ndarray, # Eigen::MatrixXd
+
+) -> numpy.ndarray:
+  """Finds the optimal transformation T_a_b that minimizes the sum of squared distances between the (same) points in A and T_a_b * points in B Points are stacked in lines (columns are x, y and z) in the matrices."""
+  ...
+
+
 def rotation_from_axis(
   axis: str, # std::string
   vector: numpy.ndarray, # Eigen::Vector3d

{placo-0.5.4 → placo-0.5.6}/pyproject.toml

@@ -16,14 +16,14 @@ dependencies = [
   "pin >= 2.6.18, < 3",
   "rhoban-cmeel-jsoncpp",
   "meshcat",
-  "numpy",
+  "numpy<2",
   "ischedule"
 ]
 description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.8"
-version = "0.5.4"
+version = "0.5.6"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.5.4 → placo-0.5.6}/src/placo/dynamics/contacts.cpp

@@ -190,6 +190,7 @@ PuppetContact::PuppetContact()
 void PuppetContact::update()
 {
   J = Eigen::MatrixXd(solver->N, solver->N);
+  J.setIdentity();
 }
 
 TaskContact::TaskContact(Task& task)

{placo-0.5.4 → placo-0.5.6}/src/placo/dynamics/dynamics_solver.cpp

@@ -5,27 +5,6 @@ namespace placo::dynamics
 {
 using namespace placo::problem;
 
-void DynamicsSolver::set_passive(const std::string& joint_name, double kp, double kd)
-{
-  OverrideJoint oj;
-  oj.passive = true;
-  oj.kp = kp;
-  oj.kd = kd;
-  override_joints[joint_name] = oj;
-}
-
-void DynamicsSolver::set_tau(const std::string& joint_name, double tau)
-{
-  OverrideJoint oj;
-  oj.tau = tau;
-  override_joints[joint_name] = oj;
-}
-
-void DynamicsSolver::reset_joint(const std::string& joint_name)
-{
-  override_joints.erase(joint_name);
-}
-
 PointContact& DynamicsSolver::add_point_contact(PositionTask& position_task)
 {
   return add_contact(new PointContact(position_task, false));
@@ -225,12 +204,6 @@ void DynamicsSolver::compute_limits_inequalities(Expression& tau)
     throw std::runtime_error("DynamicsSolver::compute_limits_inequalities: dt is not set");
   }
 
-  std::set<int> override_ids;
-  for (auto& override_joint : override_joints)
-  {
-    override_ids.insert(robot.get_joint_v_offset(override_joint.first));
-  }
-
   if (torque_limits)
   {
     problem.add_constraint(tau.slice(6) <= robot.model.effortLimit.bottomRows(N - 6));
@@ -240,11 +213,11 @@ void DynamicsSolver::compute_limits_inequalities(Expression& tau)
   int constraints = 0;
   if (joint_limits)
   {
-    constraints += 2 * (N - 6 - override_joints.size());
+    constraints += 2 * (N - 6);
   }
   if (velocity_limits)
   {
-    constraints += 2 * (N - 6 - override_joints.size());
+    constraints += 2 * (N - 6);
   }
 
   if (constraints > 0)
@@ -258,10 +231,6 @@ void DynamicsSolver::compute_limits_inequalities(Expression& tau)
     // Iterating for each actuated joints
     for (int k = 0; k < N - 6; k++)
     {
-      if (override_ids.count(k + 6) > 0)
-      {
-        continue;
-      }
       double q = robot.state.q[k + 7];
       double qd = robot.state.qd[k + 6];
 
@@ -408,29 +377,6 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
   problem.clear_constraints();
   problem.clear_variables();
 
-  // Computing target torque for passive joints
-  std::vector<int> override_indices;
-  Eigen::VectorXd override_taus = Eigen::VectorXd::Zero(override_joints.size());
-  int k = 0;
-  for (auto& entry : override_joints)
-  {
-    std::string joint = entry.first;
-    OverrideJoint oj = entry.second;
-    double q = robot.get_joint(joint);
-    double qd = robot.get_joint_velocity(joint);
-    int index = robot.get_joint_v_offset(joint);
-    override_indices.push_back(index);
-
-    if (oj.tau)
-    {
-      override_taus[k++] = oj.tau;
-    }
-    else
-    {
-      override_taus[k++] = -q * oj.kp - qd * oj.kd;
-    }
-  }
-
   Expression qdd;
   Variable& qdd_variable = problem.add_variable(robot.model.nv);
   qdd = qdd_variable.expr();
@@ -450,7 +396,7 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
   // tau = M qdd + b - J^T F
 
   // M qdd
-  Expression tau = robot.mass_matrix() * qdd + robot.state.qd * friction;
+  Expression tau = robot.mass_matrix() * qdd + robot.state.qd * damping;
 
   // b
   if (gravity_only)
@@ -498,7 +444,7 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
 
   // Now, tau = Ax + b with x = [qdd, f1, f2, ...], we copy J^T to the extended A
   // for forces that are decision variables
-  k = N;
+  int k = N;
   for (auto& contact : contacts)
   {
     if (contact->active)
@@ -558,15 +504,6 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
     problem.add_constraint(tau.slice(0, 6) == 0);
   }
 
-  // Enforce the override torques
-  if (override_taus.size() > 0)
-  {
-    Expression override_tau_expr;
-    override_tau_expr.A = tau.A(override_indices, Eigen::all);
-    override_tau_expr.b = tau.b(override_indices);
-    problem.add_constraint(override_tau_expr == override_taus);
-  }
-
   // We want to minimize torques
   problem.add_constraint(tau == 0).configure(ProblemConstraint::Soft, torque_cost);
 
@@ -648,6 +585,22 @@ void DynamicsSolver::remove_constraint(Constraint& constraint)
   }
 }
 
+void DynamicsSolver::set_kp(double kp)
+{
+  for (auto& task : tasks)
+  {
+    task->kp = kp;
+  }
+}
+
+void DynamicsSolver::set_kd(double kd)
+{
+  for (auto& task : tasks)
+  {
+    task->kd = kd;
+  }
+}
+
 void DynamicsSolver::add_task(Task& task)
 {
   task.solver = this;

{placo-0.5.4 → placo-0.5.6}/src/placo/dynamics/dynamics_solver.h

@@ -57,49 +57,12 @@ public:
     Eigen::VectorXd tau_contacts;
   };
 
-  struct OverrideJoint
-  {
-    // If passive is true, tau will be computed as a function of kp and kd
-    bool passive;
-    double kp;
-    double kd;
-
-    // Else, a custom tau will be used
-    double tau;
-  };
-
   DynamicsSolver(model::RobotWrapper& robot);
   virtual ~DynamicsSolver();
 
   // Contacts
   std::vector<Contact*> contacts;
 
-  // Override joints (passive or custom tau)
-  std::map<std::string, OverrideJoint> override_joints;
-
-  /**
-   * @brief Sets a DoF as passive, the corresponding tau will be fixed in the equation of motion
-   *        it can be purely passive joint or a spring-like behaviour
-   * @param joint_name the joint
-   * @param is_passive true if the should the joint be passive
-   * @param kp kp gain if the joint is a spring (0 by default)
-   * @param kd kd gain if the joint is a spring (0 by default)
-   */
-  void set_passive(const std::string& joint_name, double kp = 0., double kd = 0.);
-
-  /**
-   * @brief Sets a custom torque to be applied by a given joint
-   * @param joint_name the joint
-   * @param tau torque
-   */
-  void set_tau(const std::string& joint_name, double tau);
-
-  /**
-   * @brief Resets a given joint so that its torque is no longer overriden
-   * @param joint_name the joint
-   */
-  void reset_joint(const std::string& joint_name);
-
   /**
    * @brief Adds a position (in the world) task
    * @param frame_index target frame
@@ -414,9 +377,21 @@ public:
   void remove_constraint(Constraint& constraint);
 
   /**
-   * @brief Global friction that is added to all the joints
+   * @brief Set the kp for all tasks
+   * @param kp
+   */
+  void set_kp(double kp);
+
+  /**
+   * @brief Set the kp for all tasks
+   * @param kpd
+   */
+  void set_kd(double kd);
+
+  /**
+   * @brief Global damping that is added to all the joints
    */
-  double friction = 0.;
+  double damping = 0.;
 
   /**
    * @brief Solver dt (seconds)

{placo-0.5.4 → placo-0.5.6}/src/placo/dynamics/torque_task.cpp

@@ -8,9 +8,16 @@ TorqueTask::TorqueTask()
   tau_task = true;
 }
 
-void TorqueTask::set_torque(std::string joint, double torque)
+void TorqueTask::set_torque(std::string joint, double torque, double kp, double kd)
 {
-  torques[joint] = torque;
+  torques[joint].torque = torque;
+  torques[joint].kp = kp;
+  torques[joint].kd = kd;
+}
+
+void TorqueTask::reset_torque(std::string joint)
+{
+  torques.erase(joint);
 }
 
 void TorqueTask::update()
@@ -27,8 +34,10 @@ void TorqueTask::update()
   int k = 0;
   for (auto& entry : torques)
   {
+    TargetTau target = entry.second;
     A(k, solver->robot.get_joint_v_offset(entry.first)) = 1;
-    b(k, 0) = entry.second;
+    b(k, 0) = target.torque + target.kp * solver->robot.get_joint(entry.first) -
+              target.kd * solver->robot.get_joint_velocity(entry.first);
     k++;
   }
 }

placo-0.5.6/src/placo/dynamics/torque_task.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <map>
+#include <string>
+#include "placo/dynamics/task.h"
+#include "placo/model/robot_wrapper.h"
+#include "placo/tools/axises_mask.h"
+
+namespace placo::dynamics
+{
+class TorqueTask : public Task
+{
+public:
+  struct TargetTau
+  {
+    /**
+     * @brief Torque to apply
+     */
+    double torque = 0.0;
+
+    /**
+     * @brief Proportional gain
+     */
+    double kp = 0.0;
+
+    /**
+     * @brief Derivative gain (damping)
+     */
+    double kd = 0.0;
+  };
+
+  /**
+   * @brief see \ref placo::dynamics::DynamicsSolver::add_tau_task
+   */
+  TorqueTask();
+
+  /**
+   * @brief Target torques
+   */
+  std::map<std::string, TargetTau> torques;
+
+  /**
+   * @brief Sets the target for a given joint
+   * @param joint joint name
+   * @param torque target torque
+   * @param kp proportional gain (optional)
+   * @param kd derivative gain (optional)
+   */
+  void set_torque(std::string joint, double torque, double kp = 0.0, double kd = 0.0);
+
+  /**
+   * @brief Removes a joint from this task
+   * @param joint joint namle
+   */
+  void reset_torque(std::string joint);
+
+  void update() override;
+  std::string type_name() override;
+  std::string error_unit() override;
+};
+}  // namespace placo::dynamics

{placo-0.5.4 → placo-0.5.6}/src/placo/humanoid/humanoid_robot.cpp

@@ -11,7 +11,14 @@ HumanoidRobot::HumanoidRobot(std::string model_directory, int flags, std::string
   initialize();
 
   // Measuring some distances
-  dist_y_trunk_foot = fabs(get_T_a_b("trunk", "left_hip_yaw").translation().y());
+  if (model.existFrame("trunk") && model.existFrame("left_hip_yaw"))
+  {
+    dist_y_trunk_foot = fabs(get_T_a_b("trunk", "left_hip_yaw").translation().y());
+  }
+  else
+  {
+    std::cerr << "WARNING: Can't find trunk frames in the model, can't measure leg spacing" << std::endl;
+  }
 
   if (model.existFrame("head_base") && model.existFrame("head_pitch") && model.existFrame("camera"))
   {
@@ -140,7 +147,8 @@ Eigen::Vector3d HumanoidRobot::get_com_velocity(Side support, Eigen::Vector3d om
   return J_u_C * J_u_pinv * M + (J_a_C - J_u_C * J_u_pinv * J_a) * state.qd.block(6, 0, model.nv - 6, 1);
 }
 
-Eigen::VectorXd HumanoidRobot::get_torques(Eigen::VectorXd acc_a, Eigen::VectorXd contact_forces, bool use_non_linear_effects)
+Eigen::VectorXd HumanoidRobot::get_torques(Eigen::VectorXd acc_a, Eigen::VectorXd contact_forces,
+                                           bool use_non_linear_effects)
 {
   // Contact Jacobians
   Eigen::MatrixXd J_c = Eigen::MatrixXd::Zero(model.nv, 8);
@@ -267,12 +275,13 @@ void HumanoidRobot::read_from_histories(rhoban_utils::HistoryCollection& histori
     b << Eigen::Vector3d::Zero(), omega_trunk;
 
     Eigen::MatrixXd J(6, model.nv);
-    J << frame_jacobian(support_frame(), pinocchio::ReferenceFrame::LOCAL).topRows(3), frame_jacobian("trunk", "local").bottomRows(3);
+    J << frame_jacobian(support_frame(), pinocchio::ReferenceFrame::LOCAL).topRows(3),
+        frame_jacobian("trunk", "local").bottomRows(3);
     Eigen::MatrixXd J_bf = J.leftCols(6);
     Eigen::MatrixXd J_a = J.rightCols(model.nv - 6);
 
     Eigen::VectorXd qd_bf = J_bf.inverse() * (b - J_a * qd_joints);
-    for (int i=0; i<6; i++)
+    for (int i = 0; i < 6; i++)
     {
       state.qd[i] = qd_bf[i];
     }

{placo-0.5.4 → placo-0.5.6}/src/placo/kinematics/centroidal_momentum_task.cpp

@@ -10,9 +10,15 @@ CentroidalMomentumTask::CentroidalMomentumTask(Eigen::Vector3d L_world) : L_worl
 
 void CentroidalMomentumTask::update()
 {
-  auto Ag = solver->robot.centroidal_map();
+  Eigen::MatrixXd Ag = solver->robot.centroidal_map();
+  Eigen::MatrixXd Ag_angular = Ag.block(3, 0, 3, solver->N);
 
-  A = mask.apply(Ag);
+  if (solver->dt == 0)
+  {
+    throw std::runtime_error("CentroidalMomentumTask: you should set solver.dt to use this task");
+  }
+
+  A = mask.apply(Ag_angular) / solver->dt;
   b = mask.apply(L_world);
 }
 

{placo-0.5.4 → placo-0.5.6}/src/placo/tools/axises_mask.h

@@ -74,7 +74,7 @@ struct AxisesMask
   Eigen::Matrix3d R_local_world;
 
   /**
-   * @brief ROtation from world to custom rotation (provided by the user)
+   * @brief Rotation from world to custom rotation (provided by the user)
    */
   Eigen::Matrix3d R_custom_world;