placo 0.4.9__tar.gz → 0.5.0__tar.gz

{placo-0.4.9 → placo-0.5.0}/PKG-INFO

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

{placo-0.4.9 → placo-0.5.0}/bindings/expose-kinematics.cpp

@@ -20,7 +20,6 @@ void exposeKinematics()
   class_<KinematicsSolver> solver_class =
       class__<KinematicsSolver>("KinematicsSolver", init<RobotWrapper&>())
           .add_property("problem", &KinematicsSolver::problem)
-          .add_property("noise", &KinematicsSolver::noise, &KinematicsSolver::noise)
           .add_property("dt", &KinematicsSolver::dt, &KinematicsSolver::dt)
           .add_property("N", &KinematicsSolver::N)
           .add_property("scale", &KinematicsSolver::scale)
@@ -111,7 +110,8 @@ void exposeKinematics()
           .def<void (KinematicsSolver::*)(Task&)>("remove_task", &KinematicsSolver::remove_task)
           .def<void (KinematicsSolver::*)(FrameTask&)>("remove_task", &KinematicsSolver::remove_task)
           .def("remove_constraint", &KinematicsSolver::remove_constraint)
-          .def("solve", &KinematicsSolver::solve);
+          .def("solve", &KinematicsSolver::solve)
+          .def("add_q_noise", &KinematicsSolver::add_q_noise);
 
   class__<Task, bases<tools::Prioritized>, boost::noncopyable>("Task", no_init)
       .add_property(

{placo-0.4.9 → placo-0.5.0}/bindings/expose-problem.cpp

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

{placo-0.4.9 → placo-0.5.0}/placo.pyi

@@ -4383,6 +4383,17 @@ class KinematicsSolver:
     :return: position task"""
     ...
 
+  def add_q_noise(
+    self: KinematicsSolver,
+    noise: float = 1e-5, # double
+
+  ) -> None:
+    """Adds some noise on the configuration of the robot (q) 
+        
+
+    :param noise: noise level, expressed in ratio of the joint limits"""
+    ...
+
   def add_regularization_task(
     self: KinematicsSolver,
     magnitude: float = 1e-6, # double
@@ -4532,10 +4543,6 @@ class KinematicsSolver:
     """Decides if the floating base should be masked."""
     ...
 
-  noise: float # double
-  """Some configuration noise added before solving. 
-        """
-
   problem: Problem # placo::problem::Problem
   """The underlying QP problem. 
         """

{placo-0.4.9 → placo-0.5.0}/pyproject.toml

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

{placo-0.4.9 → placo-0.5.0}/src/placo/humanoid/walk_tasks.cpp

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

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

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

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

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

{placo-0.4.9 → placo-0.5.0}/src/placo/kinematics/kinematics_solver.cpp

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

{placo-0.4.9 → placo-0.5.0}/src/placo/kinematics/kinematics_solver.h

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