placo 0.4.8__tar.gz → 0.5.0__tar.gz

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

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

{placo-0.4.8 → placo-0.5.0}/bindings/expose-eigen.cpp

@@ -34,9 +34,9 @@ void exposeEigen()
   eigenpy::enableEigenPy();
 
   // Ensuring types are exposed
-  eigenpy::exposeType<Eigen::Vector2d>();
-  eigenpy::exposeType<Eigen::Vector3d>();
-  eigenpy::exposeType<Eigen::VectorXd>();
+  // eigenpy::exposeType<Eigen::Vector2d>();
+  // eigenpy::exposeType<Eigen::Vector3d>();
+  // eigenpy::exposeType<Eigen::VectorXd>();
 
   // Enables eigen for specific matrix sizes
   eigenpy::enableEigenPySpecific<Eigen::Matrix<double, 4, 1>>();

{placo-0.4.8 → 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.8 → 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()
@@ -78,7 +79,7 @@ void exposeProblem()
       .def("in_polygon_xy", &PolygonConstraint::in_polygon_xy)
       .staticmethod("in_polygon_xy");
 
-  class__<Integrator>("Integrator", init<Variable&, Eigen::VectorXd, int, double>())
+  class__<Integrator>("Integrator", init<Variable&, Expression, int, double>())
       .def(init<Variable&, Eigen::VectorXd, Eigen::MatrixXd, double>())
       .def("upper_shift_matrix", &Integrator::upper_shift_matrix)
       .staticmethod("upper_shift_matrix")
@@ -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);
@@ -177,4 +178,6 @@ void exposeProblem()
       // Aggregation
       .def("sum", &Expression::sum)
       .def("mean", &Expression::mean);
+
+  implicitly_convertible<Eigen::VectorXd, Expression>();
 }

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

@@ -2383,7 +2383,7 @@ class Expression:
 
   def __init__(
     self: Expression,
-    other: Expression, # placo::problem::Expression
+    v: numpy.ndarray, # const Eigen::VectorXd &
 
   ) -> any:
     ...
@@ -3917,7 +3917,7 @@ class Integrator:
   def __init__(
     self: Integrator,
     variable: Variable, # placo::problem::Variable
-    X0: numpy.ndarray, # Eigen::VectorXd
+    X0: Expression, # placo::problem::Expression
     order: int, # int
     dt: float, # double
 
@@ -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.8 → 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.8"
+version = "0.5.0"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.4.8 → placo-0.5.0}/src/placo/humanoid/humanoid_robot.cpp

@@ -158,15 +158,16 @@ Eigen::VectorXd HumanoidRobot::get_torques(Eigen::VectorXd acc_a, Eigen::VectorX
 
   // Mass matrix and non linear effects
   Eigen::MatrixXd M = mass_matrix();
-  Eigen::MatrixXd M_u = M.topLeftCorner(6, 6);
+  Eigen::MatrixXd M_bf = M.topLeftCorner(6, 6);
+  Eigen::MatrixXd M_a = M.topRightCorner(6, model.nv - 6);
 
   Eigen::VectorXd h = use_non_linear_effects ? non_linear_effects() : generalized_gravity();
-  Eigen::VectorXd h_u = h.head(6);
+  Eigen::VectorXd h_bf = h.head(6);
 
   // Unactuated DoFs acceleration (floating base)
-  Eigen::VectorXd acc_u = M_u.inverse() * ((J_c * contact_forces).head(6) - h_u);
-  Eigen::VectorXd acc(acc_u.size() + acc_a.size());
-  acc << acc_u, acc_a;
+  Eigen::VectorXd acc_bf = M_bf.inverse() * ((J_c * contact_forces).head(6) - h_bf - M_a * acc_a);
+  Eigen::VectorXd acc(acc_bf.size() + acc_a.size());
+  acc << acc_bf, acc_a;
 
   return M * acc + h - J_c * contact_forces;
 }
@@ -259,23 +260,18 @@ void HumanoidRobot::read_from_histories(rhoban_utils::HistoryCollection& histori
   // Setting the floating base velocity (TODO : check if it is correct)
   if (qd_joints.size() > 1)
   {
-    std::cout << "Setting floating base velocity" << std::endl;
-    std::cout << "NOT TESTED !!!" << std::endl;
-    
-    Eigen::Matrix3d R_support_trunk  = get_T_a_b(support_frame(), trunk).linear();
     Eigen::Vector3d omega_trunk = Eigen::Vector3d(histories.number("gyro_x")->interpolate(timestamp),
                                                   histories.number("gyro_y")->interpolate(timestamp),
                                                   histories.number("gyro_z")->interpolate(timestamp));
-    Eigen::Vector3d omega_support = R_support_trunk * omega_trunk;
+    Eigen::VectorXd b(6);
+    b << Eigen::Vector3d::Zero(), omega_trunk;
 
-    Eigen::VectorXd twist_support(6);
-    twist_support << Eigen::Vector3d::Zero(), omega_support;
-    
-    Eigen::MatrixXd J_support = frame_jacobian(support_frame());
-    Eigen::MatrixXd J_support_bf = J_support.leftCols(6);
-    Eigen::MatrixXd J_support_a = J_support.rightCols(model.nv - 6);
+    Eigen::MatrixXd J(6, model.nv);
+    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_support_bf.completeOrthogonalDecomposition().pseudoInverse() * (twist_support - J_support_a * qd_joints);
+    Eigen::VectorXd qd_bf = J_bf.inverse() * (b - J_a * qd_joints);
     for (int i=0; i<6; i++)
     {
       state.qd[i] = qd_bf[i];

{placo-0.4.8 → placo-0.5.0}/src/placo/humanoid/lipm.cpp

@@ -47,8 +47,8 @@ LIPM::LIPM(Problem& problem, int timesteps, double dt, Eigen::Vector2d initial_p
   x_var = &problem.add_variable(timesteps);
   y_var = &problem.add_variable(timesteps);
 
-  x = Integrator(*x_var, Eigen::Vector3d(initial_pos.x(), initial_vel.x(), initial_acc.x()), 3, dt);
-  y = Integrator(*y_var, Eigen::Vector3d(initial_pos.y(), initial_vel.y(), initial_acc.y()), 3, dt);
+  x = Integrator(*x_var, Eigen::VectorXd(Eigen::Vector3d(initial_pos.x(), initial_vel.x(), initial_acc.x())), 3, dt);
+  y = Integrator(*y_var, Eigen::VectorXd(Eigen::Vector3d(initial_pos.y(), initial_vel.y(), initial_acc.y())), 3, dt);
 }
 
 Expression LIPM::pos(int timestep)

{placo-0.4.8 → 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.8 → 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.8 → 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.8 → 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.8 → 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)
    */

{placo-0.4.8 → placo-0.5.0}/src/placo/problem/expression.cpp

@@ -12,11 +12,7 @@ Expression::Expression()
 
 Expression Expression::from_vector(const Eigen::VectorXd& v)
 {
-  Expression e;
-  e.A = Eigen::MatrixXd(v.rows(), 0);
-  e.b = v;
-
-  return e;
+  return Expression(v);
 }
 
 Expression Expression::from_double(const double& value)
@@ -35,6 +31,12 @@ Expression::Expression(const Expression& other)
   b = other.b;
 }
 
+Expression::Expression(const Eigen::VectorXd& v)
+{
+  A = Eigen::MatrixXd(v.rows(), 0);
+  b = v;
+}
+
 bool Expression::is_scalar() const
 {
   return rows() == 1 && cols() == 0;

{placo-0.4.8 → placo-0.5.0}/src/placo/problem/expression.h

@@ -16,6 +16,7 @@ class Expression
 public:
   Expression();
   Expression(const Expression& other);
+  Expression(const Eigen::VectorXd& v);
 
   /**
    * @brief Expression A matrix, in Ax + b

{placo-0.4.8 → placo-0.5.0}/src/placo/problem/integrator.cpp

@@ -2,6 +2,7 @@
 #include <iostream>
 #include <unsupported/Eigen/MatrixFunctions>
 #include "placo/problem/integrator.h"
+#include "placo/problem/problem.h"
 
 namespace placo::problem
 {
@@ -46,7 +47,7 @@ Integrator::Integrator()
 {
 }
 
-Integrator::Integrator(Variable& variable_, Eigen::VectorXd X0, Eigen::MatrixXd system_matrix, double dt)
+Integrator::Integrator(Variable& variable_, Expression X0, Eigen::MatrixXd system_matrix, double dt)
   : variable(&variable_), X0(X0), dt(dt), M(system_matrix)
 {
   order = system_matrix.rows() - 1;
@@ -73,9 +74,13 @@ Integrator::Integrator(Variable& variable_, Eigen::VectorXd X0, Eigen::MatrixXd
   }
 }
 
-Integrator::Integrator(Variable& variable_, Eigen::VectorXd X0, int order, double dt)
+Integrator::Integrator(Variable& variable_, Expression X0, int order, double dt)
   : Integrator(variable_, X0, upper_shift_matrix(order), dt)
 {
+  if (X0.rows() != order)
+  {
+    throw std::runtime_error("Integrator: X0 should have " + std::to_string(order) + " rows (same as order)");
+  }
 }
 
 Integrator::~Integrator()
@@ -140,16 +145,17 @@ Expression Integrator::expr(int step, int diff)
     e.A = Eigen::MatrixXd(rows, variable->k_end);
     e.A.setZero();
     e.b = Eigen::VectorXd(rows);
+    e.b.setZero();
 
     if (diff == -1)
     {
       e.A.block(0, variable->k_start, rows, step) = final_transition_matrix.block(0, N - step, rows, step);
-      e.b = a_powers[step] * X0;
+      e = e + a_powers[step] * X0;
     }
     else
     {
       e.A.block(0, variable->k_start, 1, step) = final_transition_matrix.block(diff, N - step, 1, step);
-      e.b(0, 0) = (a_powers[step] * X0)(diff, 0);
+      e = e + (a_powers[step] * X0).slice(diff, 1);
     }
 
     return e;
@@ -222,7 +228,7 @@ void Integrator::update_trajectory()
     trajectory.variable_value = variable->value;
 
     // Updating keyframes
-    Eigen::VectorXd X = X0;
+    Eigen::VectorXd X = X0.value(variable->problem->x);
     trajectory.keyframes[0] = X;
 
     for (int k = 1; k <= variable->size(); k++)

{placo-0.4.8 → placo-0.5.0}/src/placo/problem/integrator.h

@@ -75,7 +75,7 @@ public:
    * @param order order
    * @param dt delta time
    */
-  Integrator(Variable& variable, Eigen::VectorXd X0, int order, double dt);
+  Integrator(Variable& variable, Expression X0, int order, double dt);
 
   /**
    * @brief Creates an integrator able to build expressions and values over a decision variable. A custom continuous
@@ -90,7 +90,7 @@ public:
    * @param dt delta time
    * @pyignore
    */
-  Integrator(Variable& variable, Eigen::VectorXd X0, Eigen::MatrixXd system_matrix, double dt);
+  Integrator(Variable& variable, Expression X0, Eigen::MatrixXd system_matrix, double dt);
 
   virtual ~Integrator();
 
@@ -173,7 +173,7 @@ public:
   /**
    * @brief Initial state
    */
-  Eigen::VectorXd X0;
+  Expression X0;
 
   /**
    * @brief Caching the discrete matrix for the last step

{placo-0.4.8 → placo-0.5.0}/src/placo/problem/problem.cpp

@@ -29,6 +29,7 @@ Problem::~Problem()
 Variable& Problem::add_variable(int size)
 {
   Variable* variable = new Variable;
+  variable->problem = this;
   variable->k_start = n_variables;
   variable->k_end = n_variables + size;
   n_variables += size;

{placo-0.4.8 → placo-0.5.0}/src/placo/problem/variable.h

@@ -5,6 +5,8 @@
 
 namespace placo::problem
 {
+class Problem;
+
 /**
  * @brief Represents a variable in a \ref Problem
  */
@@ -44,5 +46,10 @@ public:
    * @brief Variable version, incremented by \ref Problem after a solve
    */
   int version = 0;
+
+  /**
+   * @brief Variable's problem
+   */
+  Problem* problem = nullptr;
 };
 };  // namespace placo::problem