placo 0.6.1__tar.gz → 0.6.3__tar.gz

{placo-0.6.1 → placo-0.6.3}/CMakeLists.txt

@@ -51,6 +51,7 @@ add_library(libplaco SHARED
     src/placo/tools/prioritized.cpp
     src/placo/tools/cubic_spline.cpp
     src/placo/tools/cubic_spline_3d.cpp
+    src/placo/tools/directions.cpp
 
     # Problem formulation
     src/placo/problem/problem.cpp

{placo-0.6.1 → placo-0.6.3}/PKG-INFO

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

{placo-0.6.1 → placo-0.6.3}/bindings/expose-dynamics.cpp

@@ -7,6 +7,7 @@
 #include "placo/dynamics/dynamics_solver.h"
 #include <Eigen/Dense>
 #include <boost/python.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 using namespace placo;
 using namespace placo::dynamics;
@@ -22,12 +23,9 @@ void exposeDynamics()
 {
   class__<DynamicsSolver::Result>("DynamicsSolverResult")
       .add_property("success", &DynamicsSolver::Result::success)
-      .add_property(
-          "tau", +[](const DynamicsSolver::Result& result) { return result.tau; })
-      .add_property(
-          "qdd", +[](const DynamicsSolver::Result& result) { return result.qdd; })
-      .add_property(
-          "tau_contacts", +[](const DynamicsSolver::Result& result) { return result.tau_contacts; })
+      .def_readwrite("tau", &DynamicsSolver::Result::tau)
+      .def_readwrite("qdd", &DynamicsSolver::Result::qdd)
+      .def_readwrite("tau_contacts", &DynamicsSolver::Result::tau_contacts)
       .def(
           "tau_dict", +[](const DynamicsSolver::Result& result, RobotWrapper& robot) {
             boost::python::dict dict;
@@ -46,17 +44,14 @@ void exposeDynamics()
       .def_readwrite("weight_forces", &Contact::weight_forces)
       .def_readwrite("weight_tangentials", &Contact::weight_tangentials)
       .def_readwrite("weight_moments", &Contact::weight_moments)
-      .add_property(
-          "wrench", +[](Contact& contact) { return contact.wrench; });
+      .def_readonly("wrench", &Contact::wrench);
 
   class__<PointContact, bases<Contact>>("PointContact", init<PositionTask&, bool>())
       .def(
           "position_task", +[](PointContact& contact) -> PositionTask& { return *contact.position_task; },
           return_internal_reference<>())
       .def_readwrite("unilateral", &PointContact::unilateral)
-      .add_property(
-          "R_world_surface", +[](PointContact& contact) { return contact.R_world_surface; },
-          +[](PointContact& contact, Eigen::Matrix3d R) { contact.R_world_surface = R; });
+      .def_readwrite("R_world_surface", &PointContact::R_world_surface);
 
   class__<Contact6D, bases<Contact>>("Contact6D", init<FrameTask&, bool>())
       .def(
@@ -86,8 +81,7 @@ void exposeDynamics()
 
   class__<ExternalWrenchContact, bases<Contact>>("ExternalWrenchContact", init<RobotWrapper::FrameIndex>())
       .add_property("frame_index", &ExternalWrenchContact::frame_index)
-      .add_property(
-          "w_ext", +[](ExternalWrenchContact& contact) { return contact.w_ext; }, &ExternalWrenchContact::w_ext);
+      .def_readwrite("w_ext", &ExternalWrenchContact::w_ext);
 
   class__<PuppetContact, bases<Contact>>("PuppetContact", init<>());
 
@@ -164,10 +158,8 @@ void exposeDynamics()
                                                                         &DynamicsSolver::add_frame_task);
 
   class__<Task, bases<tools::Prioritized>, boost::noncopyable>("DynamicsTask", no_init)
-      .add_property(
-          "A", +[](const Task& task) { return task.A; })
-      .add_property(
-          "b", +[](const Task& task) { return task.b; })
+      .def_readwrite("A", &Task::A)
+      .def_readwrite("b", &Task::b)
       .add_property("kp", &Task::kp, &Task::kp)
       .add_property("kd", &Task::kd, &Task::kd)
       .add_property("error", &Task::error)
@@ -175,54 +167,35 @@ void exposeDynamics()
 
   class__<PositionTask, bases<Task>>("DynamicsPositionTask", init<RobotWrapper::FrameIndex, Eigen::Vector3d>())
       .add_property("frame_index", &PositionTask::frame_index)
-      .add_property(
-          "target_world", +[](const PositionTask& task) { return task.target_world; }, &PositionTask::target_world)
-      .add_property(
-          "dtarget_world", +[](const PositionTask& task) { return task.dtarget_world; }, &PositionTask::dtarget_world)
-      .add_property(
-          "ddtarget_world", +[](const PositionTask& task) { return task.dtarget_world; }, &PositionTask::ddtarget_world)
+      .def_readwrite("target_world", &PositionTask::target_world)
+      .def_readwrite("dtarget_world", &PositionTask::dtarget_world)
+      .def_readwrite("ddtarget_world", &PositionTask::ddtarget_world)
       .add_property("mask", &PositionTask::mask, &PositionTask::mask);
 
   class__<CoMTask, bases<Task>>("DynamicsCoMTask", init<Eigen::Vector3d>())
-      .add_property(
-          "target_world", +[](const CoMTask& task) { return task.target_world; }, &CoMTask::target_world)
-      .add_property(
-          "dtarget_world", +[](const CoMTask& task) { return task.dtarget_world; }, &CoMTask::dtarget_world)
-      .add_property(
-          "ddtarget_world", +[](const CoMTask& task) { return task.dtarget_world; }, &CoMTask::ddtarget_world)
+      .def_readwrite("target_world", &CoMTask::target_world)
+      .def_readwrite("dtarget_world", &CoMTask::dtarget_world)
+      .def_readwrite("ddtarget_world", &CoMTask::ddtarget_world)
       .add_property("mask", &CoMTask::mask, &CoMTask::mask);
 
   class__<RelativePositionTask, bases<Task>>(
       "DynamicsRelativePositionTask", init<RobotWrapper::FrameIndex, RobotWrapper::FrameIndex, Eigen::Vector3d>())
-      .add_property(
-          "target", +[](const RelativePositionTask& task) { return task.target; }, &RelativePositionTask::target)
-      .add_property(
-          "dtarget", +[](const RelativePositionTask& task) { return task.dtarget; }, &RelativePositionTask::dtarget)
-      .add_property(
-          "ddtarget", +[](const RelativePositionTask& task) { return task.ddtarget; }, &RelativePositionTask::ddtarget)
+      .def_readwrite("target", &RelativePositionTask::target)
+      .def_readwrite("dtarget", &RelativePositionTask::dtarget)
+      .def_readwrite("ddtarget", &RelativePositionTask::ddtarget)
       .add_property("mask", &RelativePositionTask::mask, &RelativePositionTask::mask);
 
   class__<RelativeOrientationTask, bases<Task>>(
       "DynamicsRelativeOrientationTask", init<RobotWrapper::FrameIndex, RobotWrapper::FrameIndex, Eigen::Matrix3d>())
-      .add_property(
-          "R_a_b", +[](const RelativeOrientationTask& task) { return task.R_a_b; }, &RelativeOrientationTask::R_a_b)
-      .add_property(
-          "omega_a_b", +[](const RelativeOrientationTask& task) { return task.omega_a_b; },
-          &RelativeOrientationTask::omega_a_b)
-      .add_property(
-          "domega_a_b", +[](const RelativeOrientationTask& task) { return task.domega_a_b; },
-          &RelativeOrientationTask::domega_a_b)
+      .def_readwrite("R_a_b", &RelativeOrientationTask::R_a_b)
+      .def_readwrite("omega_a_b", &RelativeOrientationTask::omega_a_b)
+      .def_readwrite("domega_a_b", &RelativeOrientationTask::domega_a_b)
       .add_property("mask", &RelativeOrientationTask::mask, &RelativeOrientationTask::mask);
 
   class__<OrientationTask, bases<Task>>("DynamicsOrientationTask", init<RobotWrapper::FrameIndex, Eigen::Matrix3d>())
-      .add_property(
-          "R_world_frame", +[](const OrientationTask& task) { return task.R_world_frame; },
-          &OrientationTask::R_world_frame)
-      .add_property(
-          "omega_world", +[](const OrientationTask& task) { return task.omega_world; }, &OrientationTask::omega_world)
-      .add_property(
-          "domega_world", +[](const OrientationTask& task) { return task.domega_world; },
-          &OrientationTask::domega_world)
+      .def_readwrite("R_world_frame", &OrientationTask::R_world_frame)
+      .def_readwrite("omega_world", &OrientationTask::omega_world)
+      .def_readwrite("domega_world", &OrientationTask::domega_world)
       .add_property("mask", &OrientationTask::mask, &OrientationTask::mask);
 
   class__<FrameTask>("DynamicsFrameTask", init<>())

{placo-0.6.1 → placo-0.6.3}/bindings/expose-footsteps.cpp

@@ -8,6 +8,7 @@
 #include "placo/humanoid/footsteps_planner_repetitive.h"
 #include <Eigen/Dense>
 #include <boost/python.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 using namespace boost::python;
 using namespace placo::humanoid;
@@ -22,9 +23,7 @@ void exposeFootsteps()
   class__<FootstepsPlanner::Footstep>("Footstep", init<double, double>())
       .def("support_polygon", &FootstepsPlanner::Footstep::support_polygon)
       .add_property("side", &FootstepsPlanner::Footstep::side, &FootstepsPlanner::Footstep::side)
-      .add_property(
-          "frame", +[](const FootstepsPlanner::Footstep& footstep) { return footstep.frame; },
-          &FootstepsPlanner::Footstep::frame)
+      .def_readwrite("frame", &FootstepsPlanner::Footstep::frame)
       .add_property("foot_length", &FootstepsPlanner::Footstep::foot_length, &FootstepsPlanner::Footstep::foot_length)
       .add_property("foot_width", &FootstepsPlanner::Footstep::foot_width, &FootstepsPlanner::Footstep::foot_width)
       .def("support_polygon", &FootstepsPlanner::Footstep::support_polygon)

{placo-0.6.1 → placo-0.6.3}/bindings/expose-kinematics.cpp

@@ -7,6 +7,7 @@
 #include <boost/python/return_internal_reference.hpp>
 #include <Eigen/Dense>
 #include <boost/python.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 using namespace boost::python;
 using namespace placo;
@@ -117,10 +118,8 @@ void exposeKinematics()
           .def("solve", &KinematicsSolver::solve);
 
   class__<Task, bases<tools::Prioritized>, boost::noncopyable>("Task", no_init)
-      .add_property(
-          "A", +[](const Task& task) { return task.A; })
-      .add_property(
-          "b", +[](const Task& task) { return task.b; })
+      .def_readonly("A", &Task::A)
+      .def_readonly("b", &Task::b)
       .def("error", &Task::error)
       .def("error_norm", &Task::error_norm)
       .def("update", &Task::update);
@@ -135,8 +134,7 @@ void exposeKinematics()
       "RelativePositionTask", init<RobotWrapper::FrameIndex, RobotWrapper::FrameIndex, Eigen::Vector3d>())
       .add_property("frame_a", &RelativePositionTask::frame_a)
       .add_property("frame_b", &RelativePositionTask::frame_b)
-      .add_property(
-          "target", +[](const RelativePositionTask& task) { return task.target; }, &RelativePositionTask::target)
+      .def_readwrite("target", &RelativePositionTask::target)
       .add_property("mask", &RelativePositionTask::mask, &RelativePositionTask::mask);
 
   class__<CoMTask, bases<Task>>("CoMTask", init<Eigen::Vector3d>())
@@ -146,17 +144,14 @@ void exposeKinematics()
 
   class__<OrientationTask, bases<Task>>("OrientationTask", init<RobotWrapper::FrameIndex, Eigen::Matrix3d>())
       .add_property("frame_index", &OrientationTask::frame_index)
-      .add_property(
-          "R_world_frame", +[](const OrientationTask& task) { return task.R_world_frame; },
-          &OrientationTask::R_world_frame)
+      .def_readwrite("R_world_frame", &OrientationTask::R_world_frame)
       .add_property("mask", &OrientationTask::mask, &OrientationTask::mask);
 
   class__<RelativeOrientationTask, bases<Task>>(
       "RelativeOrientationTask", init<RobotWrapper::FrameIndex, RobotWrapper::FrameIndex, Eigen::Matrix3d>())
       .add_property("frame_a", &RelativeOrientationTask::frame_a)
       .add_property("frame_b", &RelativeOrientationTask::frame_b)
-      .add_property(
-          "R_a_b", +[](const RelativeOrientationTask& task) { return task.R_a_b; }, &RelativeOrientationTask::R_a_b)
+      .def_readwrite("R_a_b", &RelativeOrientationTask::R_a_b)
       .add_property("mask", &RelativeOrientationTask::mask, &RelativeOrientationTask::mask);
 
   class__<FrameTask>("FrameTask", init<>())
@@ -186,9 +181,7 @@ void exposeKinematics()
       .add_property("frame_index", &AxisAlignTask::frame_index)
       .add_property(
           "axis_frame", +[](const AxisAlignTask& task) { return task.axis_frame; }, &AxisAlignTask::axis_frame)
-      .add_property(
-          "targetAxis_world", +[](const AxisAlignTask& task) { return task.targetAxis_world; },
-          &AxisAlignTask::targetAxis_world);
+      .def_readwrite("targetAxis_world", &AxisAlignTask::targetAxis_world);
 
   class__<JointsTask, bases<Task>>("JointsTask", init<>())
       .def("set_joint", &JointsTask::set_joint)
@@ -206,8 +199,7 @@ void exposeKinematics()
       .add_property("joint", &WheelTask::joint)
       .add_property("radius", &WheelTask::radius)
       .add_property("omniwheel", &WheelTask::omniwheel)
-      .add_property(
-          "T_world_surface", +[](const WheelTask& task) { return task.T_world_surface; }, &WheelTask::T_world_surface);
+      .def_readwrite("T_world_surface", &WheelTask::T_world_surface);
 
   class__<DistanceTask, bases<Task>>("DistanceTask", init<RobotWrapper::FrameIndex, RobotWrapper::FrameIndex, double>())
       .add_property("frame_a", &DistanceTask::frame_a)

{placo-0.6.1 → placo-0.6.3}/bindings/expose-parameters.cpp

@@ -8,6 +8,7 @@
 #include "placo/humanoid/humanoid_parameters.h"
 #include <Eigen/Dense>
 #include <boost/python.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 using namespace boost::python;
 using namespace placo;

{placo-0.6.1 → placo-0.6.3}/bindings/expose-problem.cpp

@@ -16,6 +16,7 @@
 #include "placo/problem/qp_error.h"
 #include <Eigen/Dense>
 #include <boost/python.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 using namespace boost::python;
 using namespace placo;
@@ -84,14 +85,10 @@ void exposeProblem()
       .def("upper_shift_matrix", &Integrator::upper_shift_matrix)
       .staticmethod("upper_shift_matrix")
       .add_property("t_start", &Integrator::t_start, &Integrator::t_start)
-      .add_property(
-          "M", +[](const Integrator& i) { return i.M; })
-      .add_property(
-          "A", +[](const Integrator& i) { return i.A; })
-      .add_property(
-          "B", +[](const Integrator& i) { return i.B; })
-      .add_property(
-          "final_transition_matrix", +[](const Integrator& i) { return i.final_transition_matrix; })
+      .def_readonly("M", &Integrator::M)
+      .def_readonly("A", &Integrator::A)
+      .def_readonly("B", &Integrator::B)
+      .def_readonly("final_transition_matrix", &Integrator::final_transition_matrix)
       .def("expr", &Integrator::expr, integrator_expr_overloads())
       .def("expr_t", &Integrator::expr_t)
       .def("value", &Integrator::value)
@@ -129,10 +126,8 @@ void exposeProblem()
       .def("expr", &Variable::expr, expr_overloads());
 
   class__<Expression>("Expression")
-      .add_property(
-          "A", +[](Expression& e) { return e.A; })
-      .add_property(
-          "b", +[](Expression& e) { return e.b; })
+      .def_readwrite("A", &Expression::A)
+      .def_readwrite("b", &Expression::b)
       .def("__len__", &Expression::rows)
       .def("is_scalar", &Expression::is_scalar)
       .def("is_constant", &Expression::is_constant)

{placo-0.6.1 → placo-0.6.3}/bindings/expose-robot-wrapper.cpp

@@ -8,6 +8,7 @@
 #include "placo/kinematics/kinematics_solver.h"
 #include <Eigen/Dense>
 #include <boost/python.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 using namespace boost::python;
 using namespace placo;
@@ -138,16 +139,9 @@ void exposeRobotWrapper()
       .value("ignore_collisions", RobotWrapper::Flags::IGNORE_COLLISIONS);
 
   class__<RobotWrapper::State>("RobotWrapper_State")
-      .add_property(
-          "q", +[](const RobotWrapper::State& state) { return state.q; },
-          +[](RobotWrapper::State& state, const Eigen::VectorXd& q) { state.q = q; })
-      .add_property(
-          "qd", +[](const RobotWrapper::State& state) { return state.qd; },
-          +[](RobotWrapper::State& state, const Eigen::VectorXd& qd) { state.qd = qd; })
-      .add_property(
-          "qdd", +[](const RobotWrapper::State& state) { return state.qdd; },
-          +[](RobotWrapper::State& state, const Eigen::VectorXd& qdd) { state.qdd = qdd; });
-  ;
+      .def_readwrite("q", &RobotWrapper::State::q)
+      .def_readwrite("qd", &RobotWrapper::State::qd)
+      .def_readwrite("qdd", &RobotWrapper::State::qdd);
 
   class__<RobotWrapper::Collision>("Collision")
       .add_property("objA", &RobotWrapper::Collision::objA)
@@ -164,10 +158,8 @@ void exposeRobotWrapper()
       .add_property("objB", &RobotWrapper::Distance::objB)
       .add_property("parentA", &RobotWrapper::Distance::parentA)
       .add_property("parentB", &RobotWrapper::Distance::parentB)
-      .add_property(
-          "pointA", +[](RobotWrapper::Distance& distance) { return distance.pointA; })
-      .add_property(
-          "pointB", +[](RobotWrapper::Distance& distance) { return distance.pointB; })
+      .def_readwrite("pointA", &RobotWrapper::Distance::pointA)
+      .def_readwrite("pointB", &RobotWrapper::Distance::pointB)
       .add_property("min_distance", &RobotWrapper::Distance::min_distance);
 
   class_<RobotWrapper> robotWrapper =
@@ -210,9 +202,7 @@ void exposeRobotWrapper()
       .def(
           "get_support_side", +[](const HumanoidRobot& robot) { return robot.support_side; })
       .add_property("support_is_both", &HumanoidRobot::support_is_both, &HumanoidRobot::support_is_both)
-      .add_property(
-          "T_world_support", +[](HumanoidRobot& robot) { return robot.T_world_support; },
-          +[](HumanoidRobot& robot, Eigen::Affine3d T_world_support_) { robot.T_world_support = T_world_support_; });
+      .def_readwrite("T_world_support", &HumanoidRobot::T_world_support);
 
   exposeStdVector<RobotWrapper::Collision>("vector_Collision");
   exposeStdVector<RobotWrapper::Distance>("vector_Distance");

{placo-0.6.1 → placo-0.6.3}/bindings/expose-tools.cpp

@@ -9,10 +9,12 @@
 #include "placo/tools/cubic_spline_3d.h"
 #include "placo/tools/axises_mask.h"
 #include "placo/tools/prioritized.h"
+#include "placo/tools/directions.h"
 #include "expose-utils.hpp"
 #ifdef HAVE_RHOBAN_UTILS
 #include "rhoban_utils/history/history.h"
 #endif
+#include <eigenpy/eigen-to-python.hpp>
 
 using namespace boost::python;
 using namespace placo::tools;
@@ -23,6 +25,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(loadReplays_overloads, loadReplays, 1, 2)
 
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(set_axises_overloads, set_axises, 1, 2);
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(configure_overloads, configure, 2, 3);
+BOOST_PYTHON_FUNCTION_OVERLOADS(directions_3d_overloads, directions_3d, 1, 2);
 
 void exposeTools()
 {
@@ -32,6 +35,8 @@ void exposeTools()
   def("frame_yaw", &frame_yaw);
   def("flatten_on_floor", &flatten_on_floor);
   def("optimal_transformation", &optimal_transformation);
+  def("directions_2d", &directions_2d);
+  def("directions_3d", &directions_3d, directions_3d_overloads());
 
   exposeStdVector<int>("vector_int");
   exposeStdVector<double>("vector_double");
@@ -45,12 +50,8 @@ void exposeTools()
   class__<AxisesMask>("AxisesMask", init<>())
       .def<void (AxisesMask::*)(std::string, std::string)>("set_axises", &AxisesMask::set_axises,
                                                            set_axises_overloads())
-      .add_property(
-          "R_local_world", +[](AxisesMask& mask) { return mask.R_local_world; },
-          +[](AxisesMask& mask, Eigen::Matrix3d R) { mask.R_local_world = R; })
-      .add_property(
-          "R_custom_world", +[](AxisesMask& mask) { return mask.R_custom_world; },
-          +[](AxisesMask& mask, Eigen::Matrix3d R) { mask.R_custom_world = R; })
+      .def_readwrite("R_local_world", &AxisesMask::R_local_world)
+      .def_readwrite("R_custom_world", &AxisesMask::R_custom_world)
       .def("apply", &AxisesMask::apply);
 
   class__<Prioritized, boost::noncopyable>("Prioritized", no_init)

{placo-0.6.1 → placo-0.6.3}/bindings/expose-walk-pattern-generator.cpp

@@ -13,6 +13,7 @@
 #include "placo/humanoid/lipm.h"
 #include <Eigen/Dense>
 #include <boost/python.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 using namespace placo;
 using namespace boost::python;

{placo-0.6.1 → placo-0.6.3}/doxygen_parse.py

@@ -1,5 +1,6 @@
 import glob
 import re
+from typing import Union
 import xml.etree.ElementTree as ET
 
 # Mapping member definitions (id) to all informations
@@ -148,7 +149,7 @@ def parse_xml(xml_file: str):
             parse_compound(compounddef_node)
 
 
-def resolve_doxygen_id(id: list|str):
+def resolve_doxygen_id(id: Union[list,str]):
     if type(id) == list:
         tpl = resolve_doxygen_id(id[0])
         typ = resolve_doxygen_id(id[1])

{placo-0.6.1 → placo-0.6.3}/placo.pyi

@@ -7236,6 +7236,35 @@ class WheelTask:
     ...
 
 
+def directions_2d(
+  n: int, # int
+
+) -> numpy.ndarray:
+  """Generates a set of directions in 2D. 
+        
+
+  :param n: the number of directions 
+
+  :return: matrix of size n x 2"""
+  ...
+
+
+def directions_3d(
+  n: int, # int
+  epsilon: float = 0.5, # double
+
+) -> numpy.ndarray:
+  """Generates a set of directions in 3D, using Fibonacci lattice. 
+        
+
+  :param n: the number of directions 
+
+  :param epsilon: the epsilon parameter for the Fibonacci lattice 
+
+  :return: matrix of size n x 3"""
+  ...
+
+
 def flatten_on_floor(
   transformation: numpy.ndarray, # const Eigen::Affine3d &
 

{placo-0.6.1 → placo-0.6.3}/pyproject.toml

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

{placo-0.6.1 → placo-0.6.3}/src/placo/humanoid/walk_pattern_generator.cpp

@@ -342,8 +342,7 @@ void WalkPatternGenerator::planCoM(Trajectory& trajectory, Eigen::Vector2d initi
     for (int timestep = 1; timestep < kept_timesteps; timestep++)
     {
       Eigen::Vector2d jerk = old_trajectory->get_j_world_CoM(trajectory.t_start + timestep * parameters.dt()).head(2);
-      problem.add_constraint(lipm.jerk(timestep) == jerk)
-          .configure(ProblemConstraint::Soft, 1e-4);
+      problem.add_constraint(lipm.jerk(timestep) == jerk).configure(ProblemConstraint::Soft, 1e-4);
     }
   }
 
@@ -355,7 +354,8 @@ void WalkPatternGenerator::planCoM(Trajectory& trajectory, Eigen::Vector2d initi
     current_support = trajectory.supports[i];
     int step_timesteps = support_timesteps(current_support);
 
-    for (int timestep = constrained_timesteps; timestep < fmin(timesteps, constrained_timesteps + step_timesteps); timestep++)
+    for (int timestep = constrained_timesteps; timestep < fmin(timesteps, constrained_timesteps + step_timesteps);
+         timestep++)
     {
       // Ensuring ZMP remains in the support polygon
       if (timestep > kept_timesteps)
@@ -363,7 +363,7 @@ void WalkPatternGenerator::planCoM(Trajectory& trajectory, Eigen::Vector2d initi
         problem.add_constraint(PolygonConstraint::in_polygon_xy(
             lipm.zmp(timestep, omega_2), current_support.support_polygon(), parameters.zmp_margin));
       }
-      
+
       // ZMP reference trajectory : aiming for the center of single supports
       if (!current_support.is_both() || current_support.start || current_support.end)
       {
@@ -509,7 +509,10 @@ void WalkPatternGenerator::planSingleSupportTrajectory(TrajectoryPart& part, Tra
   trajectory.yaw(flying_side).add_point(t, frame_yaw(T_world_flyingTarget.rotation()), 0);
 
   // The trunk orientation follow the steps orientation
-  trajectory.trunk_yaw.add_point(t, frame_yaw(T_world_flyingTarget.rotation()), 0);
+  if (!parameters.has_double_support())
+  {
+    trajectory.trunk_yaw.add_point(t, frame_yaw(T_world_flyingTarget.rotation()), 0);
+  }
 
   // Support foot remaining steady
   trajectory.add_supports(t, part.support);
@@ -522,7 +525,14 @@ void WalkPatternGenerator::planFeetTrajectories(Trajectory& trajectory, Trajecto
   // Add the initial position to the trajectory
   trajectory.add_supports(t, trajectory.supports[0]);
 
-  trajectory.trunk_yaw.add_point(t, frame_yaw(trajectory.supports[0].frame().rotation()), 0);
+  if (old_trajectory == nullptr)
+  {
+    trajectory.trunk_yaw.add_point(t, frame_yaw(trajectory.supports[0].frame().rotation()), 0);
+  }
+  else
+  {
+    trajectory.trunk_yaw.add_point(t, old_trajectory->trunk_yaw.pos(t), old_trajectory->trunk_yaw.vel(t));
+  }
 
   if (!trajectory.supports[0].is_both())
   {
@@ -556,7 +566,6 @@ void WalkPatternGenerator::planFeetTrajectories(Trajectory& trajectory, Trajecto
         planSingleSupportTrajectory(part, trajectory, step, t, old_trajectory, t_replan);
       }
     }
-
     // Double support
     else
     {
@@ -625,7 +634,7 @@ WalkPatternGenerator::Trajectory WalkPatternGenerator::replan(std::vector<Footst
 bool WalkPatternGenerator::can_replan_supports(Trajectory& trajectory, double t_replan)
 {
   // We can't replan from an "end", a "start" or a "kick"
-  if (trajectory.get_support(t_replan).end || trajectory.get_support(t_replan).start || 
+  if (trajectory.get_support(t_replan).end || trajectory.get_support(t_replan).start ||
       trajectory.get_next_support(t_replan).end || trajectory.get_support(t_replan).kick())
   {
     return false;
@@ -673,7 +682,7 @@ std::vector<FootstepsPlanner::Support> WalkPatternGenerator::replan_supports(Foo
 
   std::vector<FootstepsPlanner::Support> supports;
   supports = FootstepsPlanner::make_supports(footsteps, false, parameters.has_double_support(), true);
-  
+
   if (current_support.is_both())
   {
     supports.erase(supports.begin());

{placo-0.6.1 → placo-0.6.3}/src/placo/humanoid/walk_pattern_generator.h

@@ -65,18 +65,18 @@ public:
 
     /**
      * @brief Returns the support corresponding to the given time in the trajectory
-    */
+     */
     FootstepsPlanner::Support get_support(double t);
 
     /**
      * @brief Returns the nth next support corresponding to the given time in the trajectory
      */
-    FootstepsPlanner::Support get_next_support(double t, int n=1);
+    FootstepsPlanner::Support get_next_support(double t, int n = 1);
 
     /**
      * @brief Returns the nth previous support corresponding to the given time in the trajectory
      */
-    FootstepsPlanner::Support get_prev_support(double t, int n=1);
+    FootstepsPlanner::Support get_prev_support(double t, int n = 1);
 
     std::vector<FootstepsPlanner::Support> get_supports();
     int remaining_supports(double t);

placo-0.6.3/src/placo/tools/directions.cpp

@@ -0,0 +1,42 @@
+#include "directions.h"
+
+namespace placo::tools
+{
+Eigen::MatrixXd directions_2d(int n)
+{
+  Eigen::MatrixXd directions(n, 2);
+
+  for (int i = 0; i < n; i++)
+  {
+    double angle = 2 * M_PI * i / n;
+    directions(i, 0) = cos(angle);
+    directions(i, 1) = sin(angle);
+  }
+
+  return directions;
+}
+
+Eigen::MatrixXd directions_3d(int n, double epsilon)
+{
+  Eigen::MatrixXd directions(n, 3);
+
+  // Golden ratio
+  double phi = (1 + sqrt(5)) / 2;
+
+  for (int i = 0; i < n; i++)
+  {
+    double x = fmod(i / phi, 1);
+    double y = (i + epsilon) / (n - 1 + 2 * epsilon);
+
+    double alpha = 2 * M_PI * x;
+    double beta = acos(1 - 2 * y);
+
+    directions(i, 0) = sin(beta) * cos(alpha);
+    directions(i, 1) = sin(beta) * sin(alpha);
+    directions(i, 2) = cos(beta);
+  }
+
+  return directions;
+}
+
+}  // namespace placo::tools

placo-0.6.3/src/placo/tools/directions.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <vector>
+#include <Eigen/Dense>
+
+namespace placo::tools
+{
+/**
+ * @brief Generates a set of directions in 2D
+ * @param n the number of directions
+ * @return matrix of size n x 2
+ */
+Eigen::MatrixXd directions_2d(int n);
+
+/**
+ * @brief Generates a set of directions in 3D, using Fibonacci lattice
+ * @param n the number of directions
+ * @param epsilon the epsilon parameter for the Fibonacci lattice
+ * @return matrix of size n x 3
+ */
+Eigen::MatrixXd directions_3d(int n, double epsilon = 0.5);
+}  // namespace placo::tools