placo 0.4.6__tar.gz → 0.4.7__tar.gz

{placo-0.4.6 → 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.6 → placo-0.4.7}/PKG-INFO

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

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

@@ -79,7 +79,6 @@ 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<>())
@@ -102,7 +101,6 @@ void exposeDynamics()
       .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, solve_overloads())
       .def<void (DynamicsSolver::*)(Task&)>("add_task", &DynamicsSolver::add_task)
       .def<void (DynamicsSolver::*)(Constraint&)>("add_constraint", &DynamicsSolver::add_constraint)
@@ -130,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,
@@ -233,6 +233,8 @@ void exposeDynamics()
             update_map<std::string, double>(task.djoints, py_dict);
           });
 
+  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);

{placo-0.4.6 → 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<>())
@@ -212,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.6 → placo-0.4.7}/bindings/expose-robot-wrapper.cpp

@@ -173,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.6 → 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.6 → 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)
@@ -1796,8 +1798,6 @@ class DynamicsSolver:
     """Adds a joints task. 
         
 
-    :param target: target joints values 
-
     :return: joints task"""
     ...
 
@@ -1994,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
@@ -2086,10 +2096,6 @@ class DynamicsSolver:
     """Decides if the floating base should be masked."""
     ...
 
-  optimize_contact_forces: bool # bool
-  """If true, the solver will try to optimize the contact forces by removing variables. 
-        """
-
   problem: Problem # placo::problem::Problem
   """Instance of the problem. 
         """
@@ -2153,17 +2159,6 @@ 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
@@ -2253,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,
@@ -3363,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
@@ -4180,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
@@ -4431,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. 
     """
@@ -7027,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.6 → 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.6"
+version = "0.4.7"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.4.6 → placo-0.4.7}/src/placo/dynamics/dynamics_solver.cpp

@@ -139,14 +139,14 @@ CoMTask& DynamicsSolver::add_com_task(Eigen::Vector3d target_world)
   return add_task(new CoMTask(target_world));
 }
 
-void DynamicsSolver::set_static(bool is_static_)
+JointsTask& DynamicsSolver::add_joints_task()
 {
-  is_static = is_static_;
+  return add_task(new JointsTask());
 }
 
-JointsTask& DynamicsSolver::add_joints_task()
+TorqueTask& DynamicsSolver::add_torque_task()
 {
-  return add_task(new JointsTask());
+  return add_task(new TorqueTask());
 }
 
 GearTask& DynamicsSolver::add_gear_task()
@@ -231,11 +231,6 @@ void DynamicsSolver::compute_limits_inequalities(Expression& tau)
     problem.add_constraint(tau.slice(6) >= -robot.model.effortLimit.bottomRows(N - 6));
   }
 
-  if (is_static)
-  {
-    return;
-  }
-
   int constraints = 0;
   if (joint_limits)
   {
@@ -371,10 +366,7 @@ void DynamicsSolver::clear()
 void DynamicsSolver::dump_status_stream(std::ostream& stream)
 {
   stream << "* Dynamics Tasks:" << std::endl;
-  if (is_static)
-  {
-    std::cout << "  * Solver is static (qdd is 0)" << std::endl;
-  }
+
   for (auto task : tasks)
   {
     task->update();
@@ -422,50 +414,22 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
     double qd = robot.get_joint_velocity(joint);
     int index = robot.get_joint_v_offset(joint);
     passive_indices.push_back(index);
-    passive_taus[k++] = q * pj.kp + qd * pj.kd;
+    passive_taus[k++] = -q * pj.kp - qd * pj.kd;
   }
 
   Expression qdd;
-  if (is_static)
-  {
-    qdd = Expression::from_vector(Eigen::VectorXd::Zero(robot.model.nv));
-  }
-  else
-  {
-    Variable& qdd_variable = problem.add_variable(robot.model.nv);
-    qdd = qdd_variable.expr();
+  Variable& qdd_variable = problem.add_variable(robot.model.nv);
+  qdd = qdd_variable.expr();
 
-    if (masked_fbase)
-    {
-      problem.add_constraint(qdd_variable.expr(0, 6) == 0.);
-    }
+  if (masked_fbase)
+  {
+    problem.add_constraint(qdd_variable.expr(0, 6) == 0.);
   }
 
+  // Updating tasks
   for (auto& task : tasks)
   {
     task->update();
-    if (task->A.rows() == 0)
-    {
-      continue;
-    }
-
-    if (!is_static)
-    {
-      ProblemConstraint::Priority task_priority = ProblemConstraint::Hard;
-      if (task->priority == Task::Priority::Soft)
-      {
-        task_priority = ProblemConstraint::Soft;
-      }
-      else if (task->priority == Task::Priority::Scaled)
-      {
-        throw std::runtime_error("DynamicsSolver::solve: Scaled priority is not supported");
-      }
-
-      Expression e;
-      e.A = task->A;
-      e.b = -task->b;
-      problem.add_constraint(e == 0).configure(task_priority, task->weight);
-    }
   }
 
   // We build the expression for tau, given the equation of motion
@@ -486,13 +450,6 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
 
   // J^T F
 
-  // Contacts that will result in a decision variable
-  std::vector<Contact*> variable_contacts;
-  // Contacts that will be optimized out
-  std::vector<Contact*> determined_contacts;
-  std::vector<int> determined_indices;
-  int determined_contacts_count = 0;
-
   for (auto& contact : contacts)
   {
     contact->update();
@@ -505,21 +462,10 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
     }
     else
     {
-      if (optimize_contact_forces && contact->is_internal() &&
-          determined_contacts_count + contact->size() <= passive_joints.size())
-      {
-        // This contact will be determined
-        determined_contacts.push_back(contact);
-        determined_contacts_count += contact->size();
-      }
-      else
-      {
-        // This contact will be an actual decision variable
-        Variable& f_variable = problem.add_variable(contact->size());
-        contact->f = f_variable.expr();
-        contact->add_constraints(problem);
-        variable_contacts.push_back(contact);
-      }
+      // This contact will be an actual decision variable
+      Variable& f_variable = problem.add_variable(contact->size());
+      contact->f = f_variable.expr();
+      contact->add_constraints(problem);
     }
   }
 
@@ -530,55 +476,45 @@ 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 = is_static ? 0 : N;
-  for (auto& contact : variable_contacts)
+  k = N;
+  for (auto& contact : contacts)
   {
     tau.A.block(0, k, N, contact->J.rows()) = -contact->J.transpose();
     tau.b -= contact->J.transpose() * contact->f.b;
     k += contact->J.rows();
   }
 
-  // Gathering the first N entries of the passive torque ids
-  determined_indices = std::vector<int>(passive_indices.begin(), passive_indices.begin() + determined_contacts_count);
+  // Computing limit inequalitie
+  compute_limits_inequalities(tau);
 
-  if (optimize_contact_forces && determined_contacts_count > 0)
+  // Adding tasks
+  for (auto& task : tasks)
   {
-    // Computing Jd, the jacobian of determined contact forces
-    Eigen::MatrixXd Jd = Eigen::MatrixXd::Zero(N, determined_contacts_count);
-    k = 0;
-    for (auto& contact : determined_contacts)
+    if (task->A.rows() == 0)
     {
-      Jd.block(0, k, N, contact->J.rows()) = contact->J.transpose();
-      k += contact->J.rows();
+      continue;
     }
 
-    // Building the expression for the determined forces
-    // Jd^T fd = M qdd + b - J^T F - tau_passive
-    Expression fd;
-    fd.A = tau.A(determined_indices, Eigen::all);
-    fd.b = tau.b(determined_indices);
-    fd.b -= passive_taus.topRows(determined_contacts_count);
-
-    // We use the inverse of the Jd matrix for those passive Dofs to express fd as a
-    // function of other variables
-    fd = Jd(determined_indices, Eigen::all).inverse() * fd;
-
-    // We feed the determined contacts with force expressed as other variables
-    k = 0;
-    for (auto& contact : determined_contacts)
+    ProblemConstraint::Priority task_priority = ProblemConstraint::Hard;
+    if (task->priority == Task::Priority::Soft)
     {
-      contact->f = fd.slice(k, contact->size());
-      contact->add_constraints(problem);
-      k += contact->size();
+      task_priority = ProblemConstraint::Soft;
+    }
+    else if (task->priority == Task::Priority::Scaled)
+    {
+      throw std::runtime_error("DynamicsSolver::solve: Scaled priority is not supported");
     }
 
-    // fd can now be added to tau
-    tau = tau - Jd * fd;
+    Expression e;
+    e.A = task->A;
+    e.b = -task->b;
+    if (task->tau_task)
+    {
+      e.A = e.A * tau.A;
+    }
+    problem.add_constraint(e == 0).configure(task_priority, task->weight);
   }
 
-  // Computing limit inequalitie
-  compute_limits_inequalities(tau);
-
   // Add constraints
   for (auto constraint : constraints)
   {
@@ -593,14 +529,10 @@ DynamicsSolver::Result DynamicsSolver::solve(bool integrate)
   }
 
   // Passive joints that are not determined have a tau constraint
-  if (passive_taus.size() > determined_contacts_count)
-  {
-    Expression passive_tau_expr;
-    passive_tau_expr.A = tau.A(passive_indices, Eigen::all);
-    passive_tau_expr.b = tau.b(passive_indices);
-    problem.add_constraint(passive_tau_expr.slice(determined_contacts_count) ==
-                           passive_taus.bottomRows(passive_taus.size() - determined_contacts_count));
-  }
+  Expression passive_tau_expr;
+  passive_tau_expr.A = tau.A(passive_indices, Eigen::all);
+  passive_tau_expr.b = tau.b(passive_indices);
+  problem.add_constraint(passive_tau_expr == passive_taus);
 
   // We want to minimize torques
   problem.add_constraint(tau == 0).configure(ProblemConstraint::Soft, 1e-3);