placo 0.9.4__tar.gz → 0.9.5__tar.gz

{placo-0.9.4 → placo-0.9.5}/CMakeLists.txt

@@ -44,6 +44,7 @@ add_library(libplaco SHARED
     src/placo/humanoid/walk_pattern_generator.cpp
     src/placo/humanoid/walk_tasks.cpp
     src/placo/humanoid/lipm.cpp
+    src/placo/humanoid/dummy_walk.cpp
 
     # Tools
     src/placo/tools/axises_mask.cpp

{placo-0.9.4 → placo-0.9.5}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: placo
-Version: 0.9.4
+Version: 0.9.5
 Summary: PlaCo: Rhoban Planning and Control
 Requires-Python: >= 3.9
 License-Expression: MIT

{placo-0.9.4 → placo-0.9.5}/bindings/expose-kinematics.cpp

@@ -24,8 +24,10 @@ void exposeKinematics()
           .add_property("dt", &KinematicsSolver::dt, &KinematicsSolver::dt)
           .add_property("N", &KinematicsSolver::N)
           .add_property("scale", &KinematicsSolver::scale)
-          .add_property(
-              "robot", +[](const KinematicsSolver& solver) { return solver.robot; })
+          .add_property("robot",
+                        make_function(
+                            +[](const KinematicsSolver& solver) -> model::RobotWrapper& { return solver.robot; },
+                            return_value_policy<reference_existing_object>()))
 
           // Position and CoM task
           .def<PositionTask& (KinematicsSolver::*)(std::string, Eigen::Vector3d)>(
@@ -211,7 +213,8 @@ void exposeKinematics()
       .add_property("joint", &WheelTask::joint)
       .add_property("radius", &WheelTask::radius)
       .add_property("omniwheel", &WheelTask::omniwheel)
-      .def_readwrite("T_world_surface", &WheelTask::T_world_surface);
+      .add_property("T_world_surface", make_getter(&WheelTask::T_world_surface, return_value_policy<return_by_value>()),
+                    make_setter(&WheelTask::T_world_surface));
 
   class__<DistanceTask, bases<Task>>("DistanceTask", init<RobotWrapper::FrameIndex, RobotWrapper::FrameIndex, double>())
       .add_property("frame_a", &DistanceTask::frame_a)

{placo-0.9.4 → placo-0.9.5}/bindings/expose-walk-pattern-generator.cpp

@@ -11,6 +11,7 @@
 #include "placo/humanoid/swing_foot_cubic.h"
 #include "placo/humanoid/walk_tasks.h"
 #include "placo/humanoid/lipm.h"
+#include "placo/humanoid/dummy_walk.h"
 #include <Eigen/Dense>
 #include <boost/python.hpp>
 #include <eigenpy/eigen-to-python.hpp>
@@ -180,4 +181,25 @@ void exposeWalkPatternGenerator()
       .add_property("y_var", &LIPM::y_var, &LIPM::y_var)
       .add_property("x", &LIPM::x, &LIPM::x)
       .add_property("y", &LIPM::y, &LIPM::y);
+
+  class__<DummyWalk>("DummyWalk", init<model::RobotWrapper&>())
+      .def("reset", &DummyWalk::reset, (arg("support_left") = false))
+      .def("update", &DummyWalk::update)
+      .def("next_step", &DummyWalk::next_step)
+      .def("update_T_world_support", &DummyWalk::update_T_world_support)
+      .add_property("robot", make_function(
+                                 +[](DummyWalk& walk) -> model::RobotWrapper& { return walk.robot; },
+                                 return_value_policy<reference_existing_object>()))
+      .add_property("solver", &DummyWalk::solver)
+      .def_readwrite("feet_spacing", &DummyWalk::feet_spacing)
+      .def_readwrite("trunk_height", &DummyWalk::trunk_height)
+      .def_readwrite("trunk_pitch", &DummyWalk::trunk_pitch)
+      .def_readwrite("support_left", &DummyWalk::support_left)
+      .def_readwrite("lift_height", &DummyWalk::lift_height)
+      .add_property("T_world_left", make_getter(&DummyWalk::T_world_left, return_value_policy<return_by_value>()),
+                    make_setter(&DummyWalk::T_world_left))
+      .add_property("T_world_right", make_getter(&DummyWalk::T_world_right, return_value_policy<return_by_value>()),
+                    make_setter(&DummyWalk::T_world_right))
+      .add_property("T_world_next", make_getter(&DummyWalk::T_world_next, return_value_policy<return_by_value>()),
+                    make_setter(&DummyWalk::T_world_next));
 }

{placo-0.9.4 → placo-0.9.5}/pyproject.toml

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

placo-0.9.5/src/placo/humanoid/dummy_walk.cpp

@@ -0,0 +1,130 @@
+#include "placo/humanoid/dummy_walk.h"
+
+namespace placo::humanoid
+{
+DummyWalk::DummyWalk(model::RobotWrapper& robot) : robot(robot), solver(robot)
+{
+  // Initializing solver
+  solver.enable_velocity_limits(true);
+  solver.dt = 0.1;
+
+  left_foot_task = solver.add_frame_task("left_foot");
+  left_foot_task.configure("left_foot", "soft", 1.0, 1.0);
+
+  right_foot_task = solver.add_frame_task("right_foot");
+  right_foot_task.configure("right_foot", "soft", 1.0, 1.0);
+
+  trunk_task = solver.add_frame_task("trunk");
+  trunk_task.configure("trunk", "soft", 1.0, 1.0);
+
+  reset();
+}
+
+void DummyWalk::reset(bool support_left_)
+{
+  // Initializing lift trajectory
+  lift_spline.clear();
+  lift_spline.add_point(0, 0, 0);
+  lift_spline.add_point(0.3, lift_height, 0);
+  lift_spline.add_point(0.7, lift_height, 0);
+  lift_spline.add_point(1, 0, 0);
+
+  robot.reset();
+  robot.update_kinematics();
+
+  support_left = support_left_;
+
+  T_world_left = translation(0, feet_spacing / 2, 0);
+  T_world_right = translation(0, -feet_spacing / 2, 0);
+
+  compute_next_support(0.0, 0.0, 0.0);
+  update(0.0);
+}
+
+void DummyWalk::next_step(double dx, double dy, double dtheta)
+{
+  if (support_left)
+  {
+    T_world_right = T_world_next;
+  }
+  else
+  {
+    T_world_left = T_world_next;
+  }
+
+  support_left = !support_left;
+  compute_next_support(dx, dy, dtheta);
+}
+
+void DummyWalk::update(double t)
+{
+  Eigen::Affine3d T_world_left_ = T_world_left;
+  Eigen::Affine3d T_world_right_ = T_world_right;
+
+  if (support_left)
+  {
+    T_world_right_ = tools::interpolate_frames(T_world_right, T_world_next, t);
+    T_world_right_.translation().z() = lift_spline.pos(t);
+  }
+  else
+  {
+    T_world_left_ = tools::interpolate_frames(T_world_left, T_world_next, t);
+    T_world_left_.translation().z() = lift_spline.pos(t);
+  }
+
+  Eigen::Affine3d T_world_mid = placo::tools::interpolate_frames(T_world_left_, T_world_right_, 0.5);
+  Eigen::Affine3d T_world_trunk = T_world_mid * translation(0, 0, trunk_height) *
+                                  Eigen::AngleAxisd(trunk_pitch, Eigen::Vector3d::UnitX()) *
+                                  translation(trunk_x_offset, 0, 0);
+
+  left_foot_task.set_T_world_frame(T_world_left_);
+  right_foot_task.set_T_world_frame(T_world_right_);
+  trunk_task.set_T_world_frame(T_world_trunk);
+
+  solve();
+}
+
+void DummyWalk::update_T_world_support(Eigen::Affine3d T_world_support)
+{
+  Eigen::Affine3d T_world_currentSupport = support_left ? T_world_left : T_world_right;
+  Eigen::Affine3d T = tools::flatten_on_floor(T_world_support * T_world_currentSupport.inverse());
+
+  T_world_left = tools::flatten_on_floor(T * T_world_left);
+  T_world_right = tools::flatten_on_floor(T * T_world_right);
+  T_world_next = tools::flatten_on_floor(T * T_world_next);
+
+  if (support_left)
+  {
+    robot.set_T_world_frame("left_foot", T_world_left);
+  }
+  else
+  {
+    robot.set_T_world_frame("right_foot", T_world_right);
+  }
+  robot.update_kinematics();
+}
+
+void DummyWalk::compute_next_support(double dx, double dy, double dtheta)
+{
+  double spacing = support_left ? -feet_spacing : feet_spacing;
+  T_world_next = support_left ? T_world_left : T_world_right;
+  T_world_next = T_world_next * translation(dx, spacing + dy, 0) * Eigen::AngleAxisd(dtheta, Eigen::Vector3d::UnitZ());
+}
+
+Eigen::Affine3d DummyWalk::translation(double x, double y, double z) const
+{
+  Eigen::Affine3d T = Eigen::Affine3d::Identity();
+  T.translation() = Eigen::Vector3d(x, y, z);
+  return T;
+}
+
+void DummyWalk::solve()
+{
+  for (int k = 0; k < 4; k++)
+  {
+    robot.add_q_noise(1e-3);
+    robot.update_kinematics();
+    solver.solve(true);
+  }
+}
+}  // namespace placo::humanoid

placo-0.9.5/src/placo/humanoid/dummy_walk.h

@@ -0,0 +1,114 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include "placo/kinematics/kinematics_solver.h"
+#include "placo/tools/cubic_spline.h"
+
+namespace placo::humanoid
+{
+class DummyWalk
+{
+public:
+  DummyWalk(model::RobotWrapper& robot);
+
+  /**
+   * @brief Reset the robot with a given support
+   * @param support_left whether the first support is left
+   */
+  void reset(bool support_left = false);
+
+  /**
+   * @brief Produce the next step, change support foot
+   * @param dx dx
+   * @param dy dy
+   * @param dtheta dtheta
+   */
+  void next_step(double dx, double dy, double dtheta);
+
+  /**
+   * @brief Updates the internal IK
+   * @param t phase in step from 0 to 1
+   */
+  void update(double t);
+
+  /**
+   * @brief Update the support to a given world pose
+   * @param T_world_support
+   */
+  void update_T_world_support(Eigen::Affine3d T_world_support);
+
+  /**
+   * @brief Robot wrapper
+   */
+  model::RobotWrapper& robot;
+
+  /**
+   * @brief Kinematics solver
+   */
+  kinematics::KinematicsSolver solver;
+
+  /**
+   * @brief Feet spacing [m]
+   */
+  double feet_spacing = 0.12;
+
+  /**
+   * @brief Trunk height [m]
+   */
+  double trunk_height = 0.4;
+
+  /**
+   * @brief Trunk pitch angle [rad]
+   */
+  double trunk_pitch = 0.0;
+
+  /**
+   * @brief Trunk x-offset [m]
+   */
+  double trunk_x_offset = 0.05;
+
+  /**
+   * @brief Lift height [m]
+   */
+  double lift_height = 0.04;
+
+  /**
+   * @brief Whether the current support is left or right
+   */
+  bool support_left;
+
+  /**
+   * @brief Cubic splines for the lift trajectory
+   */
+  tools::CubicSpline lift_spline;
+
+  /**
+   * @brief left foot in world, at begining of current step
+   */
+  Eigen::Affine3d T_world_left;
+
+  /**
+   * @brief right foot in world, at begining of current step
+   */
+  Eigen::Affine3d T_world_right;
+
+  /**
+   * @brief Target for the current flying foot (given by support_left)
+   */
+  Eigen::Affine3d T_world_next;
+
+  // Internal tasks
+  kinematics::FrameTask left_foot_task;
+  kinematics::FrameTask right_foot_task;
+  kinematics::FrameTask trunk_task;
+
+protected:
+  void compute_next_support(double dx, double dy, double dtheta);
+  Eigen::Affine3d translation(double x, double y, double z) const;
+
+  /**
+   * @brief Solve the internal kinematics problem
+   */
+  void solve();
+};
+}  // namespace placo::humanoid