Merge branch 'whole-body-fixed-base' into whole-body

CMakeLists.txt

@@ -51,11 +51,17 @@ set(${PROJECT_NAME}_HEADERS
   include/qrw/Types.h
   include/qrw/Params.hpp
   include/qrw/Joystick.hpp
+  include/qrw/Estimator.hpp
+  include/qrw/Filter.hpp
+  include/qrw/ComplementaryFilter.hpp
   )
 
 set(${PROJECT_NAME}_SOURCES
   src/Params.cpp
   src/Joystick.cpp
+  src/Estimator.cpp
+  src/Filter.cpp
+  src/ComplementaryFilter.cpp
   )
 
 add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})

config/walk_parameters.yaml

@@ -11,7 +11,7 @@ robot:
     envID: 0  # Identifier of the environment to choose in which one the simulation will happen
     use_flat_plane: true  # If True the ground is flat, otherwise it has bumps
     predefined_vel: true  # If we are using a predefined reference velocity (True) or a joystick (False)
-    N_SIMULATION: 10000  # Number of simulated wbc time steps
+    N_SIMULATION: 15000  # Number of simulated wbc time steps
     enable_corba_viewer: false  # Enable/disable Corba Viewer
     enable_multiprocessing: false  # Enable/disable running the MPC in another process in parallel of the main loop
     perfect_estimator: true  # Enable/disable perfect estimator by using data directly from PyBullet
@@ -25,7 +25,8 @@ robot:
     dt_mpc: 0.015  # Time step of the model predictive control
     type_MPC: 3  # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
     save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
-    interpolate_mpc: false # true to interpolate the impedance quantities between nodes of the MPC
+    movement: "circle" # name of the movement to perform
+    interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
     interpolation_type: 3 # 0,1,2,3 decide which kind of interpolation is used
     # Kp_main: [0.0, 0.0, 0.0]  # Proportional gains for the PD+
     # Kd_main: [0., 0., 0.]  # Derivative gains for the PD+

include/bindings/python.hpp

@@ -13,5 +13,7 @@ namespace bp = boost::python;
 
 void exposeJoystick();
 void exposeParams();
+void exposeEstimator();
+void exposeFilter();
 
 #endif
\ No newline at end of file

include/qrw/ComplementaryFilter.hpp

+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Estimator and ComplementaryFilter classes
+///
+/// \details These classes estimate the state of the robot based on sensor measurements
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef COMPLEMENTARY_FILTER_H_INCLUDED
+#define COMPLEMENTARY_FILTER_H_INCLUDED
+
+#include "qrw/Types.h"
+
+class ComplementaryFilter {
+ public:
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Constructor
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ComplementaryFilter();
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Constructor witht initialization
+  ///
+  /// \param[in] dt Time step of the complementary filter
+  /// \param[in] HighPass Initial value for the high pass filter
+  /// \param[in] LowPass Initial value for the low pass filter
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ComplementaryFilter(double dt, Vector3 HighPass, Vector3 LowPass);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Destructor
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ~ComplementaryFilter() {}  // Empty destructor
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Initialize
+  ///
+  /// \param[in] dt Time step of the complementary filter
+  /// \param[in] HighPass Initial value for the high pass filter
+  /// \param[in] LowPass Initial value for the low pass filter
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void initialize(double dt, Vector3 HighPass, Vector3 LowPass);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Compute the filtered output of the complementary filter
+  ///
+  /// \param[in] x Quantity handled by the filter
+  /// \param[in] dx Derivative of the quantity
+  /// \param[in] alpha Filtering coefficient between x and dx quantities
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  Vector3 compute(Vector3 const& x, Vector3 const& dx, Vector3 const& alpha);
+
+  Vector3 getX() { return x_; }              // Get the input quantity
+  Vector3 getDx() { return dx_; }            // Get the derivative of the input quantity
+  Vector3 getHighPass() { return HighPass_; }     // Get the high-passed internal quantity
+  Vector3 getLowPass() { return LowPass_; }      // Get the low-passed internal quantity
+  Vector3 getAlpha() { return alpha_; }      // Get the alpha coefficient of the filter
+  Vector3 getFilteredX() { return filteredX_; }  // Get the filtered output
+
+ private:
+  double dt_;          // Time step of the complementary filter
+  Vector3 HighPass_;   // Initial value for the high pass filter
+  Vector3 LowPass_;    // Initial value for the low pass filter
+  Vector3 alpha_;      // Filtering coefficient between x and dx quantities
+  Vector3 x_;          // Quantity to filter
+  Vector3 dx_;         // Quantity to filter derivative's
+  Vector3 filteredX_;  // Filtered output
+};
+
+#endif  // COMPLEMENTARY_FILTER_H_INCLUDED

include/qrw/Estimator.hpp

+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Estimator and ComplementaryFilter classes
+///
+/// \details These classes estimate the state of the robot based on sensor measurements
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef ESTIMATOR_H_INCLUDED
+#define ESTIMATOR_H_INCLUDED
+
+#include <deque>
+
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "qrw/ComplementaryFilter.hpp"
+#include "qrw/Params.hpp"
+
+class Estimator {
+ public:
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Constructor
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  Estimator();
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Destructor.
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ~Estimator() {}  // Empty destructor
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Initialize with given data
+  ///
+  /// \param[in] params Object that stores parameters
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void initialize(Params& params);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Run one iteration of the estimator to get the position and velocity states of the robot
+  ///
+  /// \param[in] gait Gait matrix that stores current and future contact status of the feet
+  /// \param[in] goals Target positions of the four feet
+  /// \param[in] baseLinearAcceleration Linear acceleration of the IMU (gravity compensated)
+  /// \param[in] baseAngularVelocity Angular velocity of the IMU
+  /// \param[in] baseOrientation Quaternion orientation of the IMU
+  /// \param[in] q_mes Position of the 12 actuators
+  /// \param[in] v_mes Velocity of the 12 actuators
+  /// \param[in] perfectPosition Position of the robot in world frame
+  /// \param[in] b_perfectVelocity Velocity of the robot in base frame
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void run(MatrixN const& gait, MatrixN const& goals, VectorN const& baseLinearAcceleration,
+           VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q_mes,
+           VectorN const& v_mes, VectorN const& perfectPosition, Vector3 const& b_perfectVelocity);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Update state vectors of the robot (q and v)
+  ///        Update transformation matrices between world and horizontal frames
+  ///
+  /// \param[in] joystick_v_ref Reference velocity from the joystick
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void updateReferenceState(VectorN const& vRef);
+
+  VectorN getQEstimate() { return qEstimate_; }
+  VectorN getVEstimate() { return vEstimate_; }
+  VectorN getVSecurity() { return vSecurity_; }
+  VectorN getFeetStatus() { return feetStatus_; }
+  MatrixN getFeetTargets() { return feetTargets_; }
+  Vector3 getBaseVelocityFK() { return baseVelocityFK_; }
+  Vector3 getBasePositionFK() { return basePositionFK_; }
+  Vector3 getFeetPositionBarycenter() { return feetPositionBarycenter_; }
+  Vector3 getBBaseVelocity() { return b_baseVelocity_; }
+
+  Vector3 getFilterVelX() { return velocityFilter_.getX(); }
+  Vector3 getFilterVelDX() { return velocityFilter_.getDx(); }
+  Vector3 getFilterVelAlpha() { return velocityFilter_.getAlpha(); }
+  Vector3 getFilterVelFiltX() { return velocityFilter_.getFilteredX(); }
+
+  Vector3 getFilterPosX() { return positionFilter_.getX(); }
+  Vector3 getFilterPosDX() { return positionFilter_.getDx(); }
+  Vector3 getFilterPosAlpha() { return positionFilter_.getAlpha(); }
+  Vector3 getFilterPosFiltX() { return positionFilter_.getFilteredX(); }
+
+  VectorN getQReference() { return qRef_; }
+  VectorN getVReference() { return vRef_; }
+  VectorN getBaseVelRef() { return baseVelRef_; }
+  VectorN getBaseAccRef() { return baseAccRef_; }
+  VectorN getHV() { return h_v_; }
+  VectorN getVFiltered() { return vFiltered_; }
+  VectorN getHVFiltered() { return h_vFiltered_; }
+  Matrix3 getoRh() { return oRh_; }
+  Matrix3 gethRb() { return hRb_; }
+  Vector3 getoTh() { return oTh_; }
+
+ private:
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Retrieve and update IMU data
+  ///
+  /// \param[in] baseLinearAcceleration Linear acceleration of the IMU (gravity compensated)
+  /// \param[in] baseAngularVelocity Angular velocity of the IMU
+  /// \param[in] baseOrientation Euler orientation of the IMU
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void updateIMUData(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
+                     Vector3 const& baseOrientation, VectorN const& perfectPosition);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Retrieve and update position and velocity of the 12 actuators
+  ///
+  /// \param[in] q Position of the 12 actuators
+  /// \param[in] v Velocity of the 12 actuators
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void updateJointData(Vector12 const& q, Vector12 const& v);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Update the feet relative data
+  /// \details update feetStatus_, feetTargets_, feetStancePhaseDuration_ and phaseRemainingDuration_
+  ///
+  /// \param[in] gait Gait matrix that stores current and future contact status of the feet
+  /// \param[in] feetTargets Target positions of the four feet
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void updatFeetStatus(MatrixN const& gait, MatrixN const& feetTargets);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Estimate base position and velocity using Forward Kinematics
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void updateForwardKinematics();
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Compute barycenter of feet in contact
+  ///
+  /// \param[in] feet_status Contact status of the four feet
+  /// \param[in] goals Target positions of the four feet
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void computeFeetPositionBarycenter();
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Estimate the velocity of the base with forward kinematics using a contact point that
+  ///        is supposed immobile in world frame
+  ///
+  /// \param[in] contactFrameId Frame ID of the contact foot
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  Vector3 computeBaseVelocityFromFoot(int footId);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Estimate the position of the base with forward kinematics using a contact point that
+  ///        is supposed immobile in world frame
+  ///
+  /// \param[in] footId Frame ID of the contact foot
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  Vector3 computeBasePositionFromFoot(int footId);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Compute the alpha coefficient for the complementary filter
+  /// \return alpha
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  double computeAlphaVelocity();
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Estimates the velocity vector
+  /// \details The complementary filter combines data from the FK and the IMU acceleration data
+  ///
+  /// \param[in] b_perfectVelocity Perfect velocity of the base in the base frame
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void estimateVelocity(Vector3 const& b_perfectVelocity);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Estimates the configuration vector
+  /// \details The complementary filter combines data from the FK and the estimated velocity
+  ///
+  /// \param[in] perfectPosition Perfect position of the base in the world frame
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void estimatePosition(Vector3 const& perfectPosition);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Filter the estimated velocity over a moving window
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void filterVelocity();
+
+  bool perfectEstimator_;  // Enable perfect estimator (directly from the PyBullet simulation)
+  bool solo3D_;            // Perfect estimator including yaw angle
+  double dt_;              // Time step of the estimator
+  bool initialized_;       // Is intiialized after the first update of the IMU
+  Vector4 feetFrames_;     // Frame indexes of the four feet
+  double footRadius_;      // radius of a foot
+  Vector3 alphaPos_;       // Alpha coeefficient for the position complementary filter
+  double alphaVelMax_;     // Maximum alpha value for the velocity complementary filter
+  double alphaVelMin_;     // Minimum alpha value for the velocity complementary filter
+  double alphaSecurity_;   // Low pass coefficient for the outputted filtered velocity for security check
+
+  pinocchio::SE3 b_M_IMU_;              // Transform between the base frame and the IMU frame
+  double IMUYawOffset_;                 // Yaw orientation of the IMU at startup
+  Vector3 IMULinearAcceleration_;       // Linear acceleration of the IMU (gravity compensated)
+  Vector3 IMUAngularVelocity_;          // Angular velocity of the IMU
+  Vector3 IMURpy_;                      // Roll Pitch Yaw orientation of the IMU
+  pinocchio::SE3::Quaternion IMUQuat_;  // Quaternion orientation of the IMU
+
+  Vector12 qActuators_;  // Measured positions of actuators
+  Vector12 vActuators_;  // Measured velocities of actuators
+
+  int phaseRemainingDuration_;       // Number of iterations left for the current gait phase
+  Vector4 feetStancePhaseDuration_;  // Number of loops during which each foot has been in contact
+  Vector4 feetStatus_;               // Contact status of the four feet
+  Matrix34 feetTargets_;             // Target positions of the four feet
+
+  pinocchio::Model velocityModel_, positionModel_;  // Pinocchio models for frame computations and forward kinematics
+  pinocchio::Data velocityData_, positionData_;     // Pinocchio datas for frame computations and forward kinematics
+  Vector19 q_FK_;                                   // Configuration vector for Forward Kinematics
+  Vector18 v_FK_;                                   // Velocity vector for Forward Kinematics
+  Vector3 baseVelocityFK_;                          // Base linear velocity estimated by Forward Kinematics
+  Vector3 basePositionFK_;                          // Base position estimated by Forward Kinematics
+  Vector3 b_baseVelocity_;                          // Filtered estimated velocity at center base (base frame)
+  Vector3 feetPositionBarycenter_;                  // Barycenter of feet in contact
+
+  ComplementaryFilter positionFilter_;  // Complementary filter for base position
+  ComplementaryFilter velocityFilter_;  // Complementary filter for base velocity
+  Vector19 qEstimate_;                  // Filtered output configuration
+  Vector18 vEstimate_;                  // Filtered output velocity
+  Vector12 vSecurity_;                  // Filtered output velocity for security check
+
+  int windowSize_;                                     // Number of samples in the averaging window
+  Vector6 vFiltered_;                                  // Base velocity (in base frame) filtered by averaging window
+  std::deque<double> vx_queue_, vy_queue_, vz_queue_;  // Queues that hold samples
+
+  Vector18 qRef_;        // Configuration vector in ideal world frame
+  Vector18 vRef_;        // Velocity vector in ideal world frame
+  Vector6 baseVelRef_;   // Reference velocity vector
+  Vector6 baseAccRef_;   // Reference acceleration vector
+  Matrix3 oRh_;          // Rotation between horizontal and world frame
+  Matrix3 hRb_;          // Rotation between base and horizontal frame
+  Vector3 oTh_;          // Translation between horizontal and world frame
+  Vector6 h_v_;          // Velocity vector in horizontal frame
+  Vector6 h_vFiltered_;  // Base velocity (in horizontal frame) filtered by averaging window
+};
+#endif  // ESTIMATOR_H_INCLUDED

include/qrw/Filter.hpp

+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Filter class
+///
+/// \details This class applies a low pass filter to estimated data to avoid keeping high frequency components into
+///          what is given to the "low frequency" model predictive control
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef FILTER_H_INCLUDED
+#define FILTER_H_INCLUDED
+
+#include <deque>
+
+#include "qrw/Params.hpp"
+
+class Filter {
+ public:
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Constructor
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  Filter();
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Initialize with given data
+  ///
+  /// \param[in] params Object that stores parameters
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void initialize(Params& params);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Destructor.
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ~Filter() {}  // Empty destructor
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Run one iteration of the filter and return the filtered measurement
+  ///
+  /// \param[in] x Quantity to filter
+  /// \param[in] check_modulo Check for the +-pi modulo of orientation if true
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  VectorN filter(Vector6 const& x, bool check_modulo);
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  ///
+  /// \brief Add or remove 2 PI to all elements in the queues to handle +- pi modulo
+  ///
+  /// \param[in] a Angle that needs change (3, 4, 5 for Roll, Pitch, Yaw respectively)
+  /// \param[in] dir Direction of the change (+pi to -pi or -pi to +pi)
+  ///
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+  void handle_modulo(int a, bool dir);
+
+  VectorN getFilt() { return y_; }
+
+ private:
+  double b_;       // Denominator coefficients of the filter transfer function
+  Vector2 a_;      // Numerator coefficients of the filter transfer function
+  Vector6 x_;      // Latest measurement
+  VectorN y_;      // Latest result
+  Vector6 accum_;  // Used to compute the result (accumulation)
+
+  std::deque<Vector6> x_queue_;  // Store the last measurements for product with denominator coefficients
+  std::deque<Vector6> y_queue_;  // Store the last results for product with numerator coefficients
+
+  bool init_;  // Initialisation flag
+};
+
+#endif  // FILTER_H_INCLUDED

include/qrw/Params.hpp

@@ -93,6 +93,7 @@ class Params {
   int N_periods;                // Number of gait periods in the MPC prediction horizon
   int type_MPC;                 // Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
   bool save_guess;              // true to save the initial result of the mpc
+  std::string movement;         // Name of the mmovemnet to perform
   bool interpolate_mpc;         // true to interpolate the impedance quantities, otherwise integrate
   int interpolation_type;       // type of interpolation used
   bool kf_enabled;              // Use complementary filter (False) or kalman filter (True) for the estimator

python/CMakeLists.txt

@@ -2,6 +2,8 @@ set(${PY_NAME}_SOURCES
   main.cpp
   Joystick.cpp
   Params.cpp
+  Estimator.cpp
+  Filter.cpp
 )
 
 set(PY_LIB ${${PY_NAME}_LIB})
@@ -40,6 +42,7 @@ set(${PY_NAME}_TOOLS_PYTHON
   __init__.py
   LoggerControl.py
   PyBulletSimulator.py
+  Utils.py
   qualisysClient.py
   kinematics_utils.py
   )

python/Estimator.cpp

+#include "qrw/Estimator.hpp"
+
+#include "bindings/python.hpp"
+
+template <typename Estimator>
+struct EstimatorVisitor : public bp::def_visitor<EstimatorVisitor<Estimator>> {
+  template <class PyClassEstimator>
+  void visit(PyClassEstimator& cl) const {
+    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+        .def("initialize", &Estimator::initialize, bp::args("params"), "Initialize Estimator from Python.\n")
+        .def("update_reference_state", &Estimator::updateReferenceState, bp::args("v_ref"), "Update robot state.\n")
+
+        .def("get_q_estimate", &Estimator::getQEstimate, "Get filtered configuration.\n")
+        .def("get_v_estimate", &Estimator::getVEstimate, "Get filtered velocity.\n")
+        .def("get_v_security", &Estimator::getVSecurity, "Get filtered velocity for security check.\n")
+        .def("get_feet_status", &Estimator::getFeetStatus, "")
+        .def("get_feet_targets", &Estimator::getFeetTargets, "")
+        .def("get_base_velocity_FK", &Estimator::getBaseVelocityFK, "")
+        .def("get_base_position_FK", &Estimator::getBasePositionFK, "")
+        .def("get_feet_position_barycenter", &Estimator::getFeetPositionBarycenter, "")
+        .def("get_b_base_velocity", &Estimator::getBBaseVelocity, "")
+        .def("get_filter_vel_X", &Estimator::getFilterVelX, "")
+        .def("get_filter_vel_DX", &Estimator::getFilterVelDX, "")
+        .def("get_filter_vel_Alpha", &Estimator::getFilterVelAlpha, "")
+        .def("get_filter_vel_FiltX", &Estimator::getFilterVelFiltX, "")
+        .def("get_filter_pos_X", &Estimator::getFilterPosX, "")
+        .def("get_filter_pos_DX", &Estimator::getFilterPosDX, "")
+        .def("get_filter_pos_Alpha", &Estimator::getFilterPosAlpha, "")
+        .def("get_filter_pos_FiltX", &Estimator::getFilterPosFiltX, "")
+        .def("get_q_reference", &Estimator::getQReference, "")
+        .def("get_v_reference", &Estimator::getVReference, "")
+        .def("get_base_vel_ref", &Estimator::getBaseVelRef, "")
+        .def("get_base_acc_ref", &Estimator::getBaseAccRef, "")
+        .def("get_h_v", &Estimator::getHV, "")
+        .def("get_v_filtered", &Estimator::getVFiltered, "Get filtered velocity.\n")
+        .def("get_h_v_filtered", &Estimator::getHVFiltered, "")
+        .def("get_oRh", &Estimator::getoRh, "")
+        .def("get_hRb", &Estimator::gethRb, "")
+        .def("get_oTh", &Estimator::getoTh, "")
+
+        .def("run", &Estimator::run,
+             bp::args("gait", "goals", "baseLinearAcceleration", "baseAngularVelocity", "baseOrientation", "q_mes",
+                      "v_mes", "base_position", "b_base_velocity"),
+             "Run Estimator from Python.\n");
+  }
+
+  static void expose() { bp::class_<Estimator>("Estimator", bp::no_init).def(EstimatorVisitor<Estimator>()); }
+};
+
+void exposeEstimator() { EstimatorVisitor<Estimator>::expose(); }

python/Filter.cpp

+#include "qrw/Filter.hpp"
+
+#include "bindings/python.hpp"
+template <typename Filter>
+struct FilterVisitor : public bp::def_visitor<FilterVisitor<Filter>> {
+  template <class PyClassFilter>
+  void visit(PyClassFilter& cl) const {
+    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+        .def("initialize", &Filter::initialize, bp::args("params"), "Initialize Filter from Python.\n")
+        .def("filter", &Filter::filter, bp::args("x", "check_modulo"), "Run Filter from Python.\n");
+  }
+
+  static void expose() { bp::class_<Filter>("Filter", bp::no_init).def(FilterVisitor<Filter>()); }
+};
+
+void exposeFilter() { FilterVisitor<Filter>::expose(); }
\ No newline at end of file

python/Params.cpp

@@ -27,6 +27,7 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
         .def_readwrite("use_flat_plane", &Params::use_flat_plane)
         .def_readwrite("predefined_vel", &Params::predefined_vel)
         .def_readwrite("save_guess", &Params::save_guess)
+        .def_readwrite("movement", &Params::movement)
         .def_readwrite("interpolate_mpc", &Params::interpolate_mpc)
         .def_readwrite("interpolation_type", &Params::interpolation_type)
         .def_readwrite("kf_enabled", &Params::kf_enabled)
@@ -50,6 +51,7 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
         .def_readwrite("CoM_offset", &Params::CoM_offset)
         .def_readwrite("h_ref", &Params::h_ref)
         .def_readwrite("shoulders", &Params::shoulders)
+        .def_readwrite("max_height", &Params::max_height)
         .def_readwrite("lock_time", &Params::lock_time)
         .def_readwrite("vert_time", &Params::vert_time)
         .def_readwrite("footsteps_init", &Params::footsteps_init)

python/main.cpp

@@ -3,4 +3,6 @@
 BOOST_PYTHON_MODULE(quadruped_reactive_walking_pywrap) {
   exposeJoystick();
   exposeParams();
+  exposeEstimator();
+  exposeFilter();
 }
\ No newline at end of file

python/quadruped_reactive_walking/Controller.py

@@ -4,8 +4,10 @@ import numpy as np
 import pinocchio as pin
 import pybullet as pyb
 
+import quadruped_reactive_walking as qrw
 from . import WB_MPC_Wrapper
 from .WB_MPC.Target import Target
+from .tools.Utils import init_robot, quaternionToRPY
 
 COLORS = ["#1f77b4", "#ff7f0e", "#2ca02c"]
 
@@ -154,18 +156,29 @@ class Controller:
         self.gait = np.concatenate([np.repeat(np.array([1, 1, 1, 1]).reshape((1, 4)), self.pd.init_steps, axis=0),
                                     np.repeat(np.array([1, 0, 1, 1]).reshape((1, 4)), self.pd.target_steps - 1, axis=0)])
         self.q_init = pd.q0
+        init_robot(q_init, params)
 
         self.k = 0
         self.error = False
         self.initialized = False
 
+        self.estimator = qrw.Estimator()
+        self.estimator.initialize(params)
+        self.q = np.zeros(18)
+
         self.result = Result(params)
         self.result.q_des = self.pd.q0[7:].copy()
         self.result.v_des = self.pd.v0[6:].copy()
+        self.result.FF = self.params.Kff_main * np.ones(12)
+
+        self.target = Target(params)
+        self.footsteps = []
+        for k in range(self.pd.T * self.pd.mpc_wbc_ratio):
+            self.target_footstep = self.target.compute(k).copy()
+            if k % self.pd.mpc_wbc_ratio == 0:
+                self.footsteps.append(self.target_footstep.copy())
 
-        self.target = Target(pd)
-        footsteps = [self.target.footstep(t) for t in range(pd.T)]
-        self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, footsteps, self.gait)
+        self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, self.footsteps, self.gait)
         self.mpc_solved = False
         self.k_result = 0
         self.k_solve = 0
@@ -198,25 +211,48 @@ class Controller:
         t_start = time.time()
 
         m = self.read_state(device)
+
+        # oRh, hRb, oTh = self.run_estimator(device)
+
         t_measures = time.time()
         self.t_measures = t_measures - t_start
 
+        self.target_footstep = self.target.compute(
+            self.k + self.pd.T * self.pd.mpc_wbc_ratio
+        )
+
         if self.k % self.pd.mpc_wbc_ratio == 0:
-            self.target.shift()
             if self.mpc_solved:
                 self.k_solve = self.k
                 self.mpc_solved = False
 
             try:
-                footstep = self.target.footstep(self.pd.T)
                 self.mpc.solve(
                     self.k,
                     m["x_m"],
-                    footstep,
+                    self.target_footstep.copy(),
                     self.gait,
                     self.xs_init,
                     self.us_init,
                 )
+                # if self.initialized:
+                #     self.mpc.solve(
+                #         self.k,
+                #         self.mpc_result.xs[1],
+                #         self.target_footstep.copy(),
+                #         self.gait,
+                #         self.xs_init,
+                #         self.us_init,
+                #     )
+                # else:
+                #     self.mpc.solve(
+                #         self.k,
+                #         m["x_m"],
+                #         self.target_footstep.copy(),
+                #         self.gait,
+                #         self.xs_init,
+                #         self.us_init,
+                #     )
             except ValueError:
                 self.error = True
                 print("MPC Problem")
@@ -276,8 +312,8 @@ class Controller:
         t_send = time.time()
         self.t_send = t_send - t_mpc
 
-        # self.clamp_result(device)
-        # self.security_check(m)
+        self.clamp_result(device)
+        self.security_check(m)
 
         if self.error:
             self.set_null_control()
@@ -398,9 +434,47 @@ class Controller:
         )
         print("Initial guess saved")
 
-    def tuple_to_array(self, tup):
-        a = np.array([element for tupl in tup for element in tupl])
-        return a
+    def run_estimator(
+        self, device, q_perfect=np.zeros(6), b_baseVel_perfect=np.zeros(3)
+    ):
+        """
+        Call the estimator and retrieve the reference and estimated quantities.
+        Run a filter on q, h_v and v_ref.
+
+        @param device device structure holding simulation data
+        @param q_perfect 6D perfect position of the base in world frame
+        @param v_baseVel_perfect 3D perfect linear velocity of the base in base frame
+        """
+
+        self.estimator.run(
+            self.gait,
+            self.target.compute(self.k),
+            device.imu.linear_acceleration,
+            device.imu.gyroscope,
+            device.imu.attitude_euler,
+            device.joints.positions,
+            device.joints.velocities,
+            q_perfect,
+            b_baseVel_perfect,
+        )
+
+        self.estimator.update_reference_state(np.zeros(6))
+        # Add joystck reference velocity when needed
+
+        oRh = self.estimator.get_oRh()
+        hRb = self.estimator.get_hRb()
+        oTh = self.estimator.get_oTh().reshape((3, 1))
+
+        self.v_ref = self.estimator.get_base_vel_ref()
+        self.h_v = self.estimator.get_h_v()
+        self.h_v_windowed = self.estimator.get_h_v_filtered()
+
+        self.q[:3] = self.estimator.get_q_estimate()[:3]
+        self.q[6:] = self.estimator.get_q_estimate()[7:]
+        self.q[3:6] = quaternionToRPY(self.estimator.get_q_estimate()[3:7]).ravel()
+        self.v = self.estimator.get_v_reference()
+
+        return oRh, hRb, oTh
 
     def read_state(self, device):
         qj_m = device.joints.positions
@@ -427,7 +501,6 @@ class Controller:
         )
         # x_diff = self.mpc_result.xs[0] - m["x_m"]
         tau = self.mpc_result.us[0] - np.dot(self.mpc_result.K[0], x_diff)
-        # tau = self.mpc_result.us[0]
         return tau
 
     def integrate_x(self, m):

python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py

@@ -17,7 +17,7 @@ MAXITER = 1
 def createProblem():
     params = qrw.Params()
     pd = ProblemDataFull(params)
-    target = Target(pd)
+    target = Target(params)
 
     x0 = pd.x0_reduced
 

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

 from tracemalloc import start
+from xxlimited import foo
 
 from .ProblemData import ProblemData
 from .Target import Target
@@ -19,15 +20,26 @@ class OCP:
         self.t_update_last_model = 0.0
         self.t_shift = 0.0
 
-        self.initialize_models(gait, footsteps)
+        rm, tm = self.initialize_models(gait, footsteps)
 
         self.x0 = self.pd.x0
 
         self.problem = crocoddyl.ShootingProblem(
-            self.x0, self.models, self.terminal_model
-        )
+            self.x0, rm, tm)
         self.ddp = crocoddyl.SolverFDDP(self.problem)
 
+    def initialize_models(self, gait, footsteps):
+        models = []
+        for t, step in enumerate(gait):
+            tasks = self.make_task(footsteps[t])
+            support_feet = [self.pd.allContactIds[i] for i in np.nonzero(step == 1)[0]]
+            models.append(self.create_model(support_feet, tasks))
+
+        support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]]
+        terminal_model = self.create_model(support_feet, is_terminal=True)
+
+        return models, terminal_model
+
     def solve(self, x0, footstep, gait, xs_init=None, us_init=None):
         t_start = time()
         self.x0 = x0
@@ -65,14 +77,15 @@ class OCP:
         pin.updateFramePlacements(self.pd.model, self.pd.rdata)
 
         if self.initialized:
-            task = self.make_task(np.reshape(footstep, (3, 4), order="F"))
-            support_feet = [self.pd.allContactIds[i]
-                            for i in np.nonzero(gait[-1] == 1)[0]]
-
-            self.nodes[0].update_model(support_feet, task)
+            tasks = self.make_task(footstep)
+            support_feet = [
+                self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]
+            ]
+            self.update_model(self.problem.runningModels[0], tasks, support_feet)
 
             self.problem.circularAppend(
-                self.nodes[0].model, self.nodes[0].model.createData()
+                self.problem.runningModels[0],
+                self.problem.runningModels[0].createData(),
             )
 
         self.problem.x0 = self.x0
@@ -80,28 +93,13 @@ class OCP:
         self.initialized = True
 
     def make_task(self, footstep):
-        task = []
+        task = [[], []]
         for foot in range(4):
             if footstep[:, foot].any():
-                task += [[self.pd.allContactIds[foot], footstep[:, foot]]]
+                task[0].append(self.pd.allContactIds[foot])
+                task[1].append(footstep[:, foot])
         return task
 
-    def initialize_models(self, gait, footsteps):
-        self.nodes = []
-        for t, step in enumerate(gait):
-            task = self.make_task(footsteps[t])
-            support_feet = [self.pd.allContactIds[i]
-                            for i in np.nonzero(step == 1)[0]]
-            self.nodes.append(Node(self.pd, self.state, support_feet, task))
-
-        support_feet = [self.pd.allContactIds[i]
-                        for i in np.nonzero(gait[-1] == 1)[0]]
-        self.terminal_node = Node(
-            self.pd, self.state, support_feet, isTerminal=True)
-
-        self.models = [node.model for node in self.nodes]
-        self.terminal_model = self.terminal_node.model
-
     def get_results(self):
         return (
             self.ddp.xs.tolist().copy(),
@@ -143,206 +141,135 @@ class OCP:
          for m in self.ddp.problem.runningDatas]
         return acc
 
+    def update_model(self, model, tasks, support_feet):
+        for i in self.pd.allContactIds:
+            name = self.pd.model.frames[i].name + "_contact"
+            model.differential.contacts.changeContactStatus(name, i in support_feet)
 
-class Node:
-    def __init__(
-        self, pd, state, supportFootIds=[], swingFootTask=[], isTerminal=False
-    ):
-        self.pd = pd
-        self.isTerminal = isTerminal
-
-        self.state = state
-        self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
-        self.control = crocoddyl.ControlParametrizationModelPolyZero(
-            self.actuation.nu)
-        self.nu = self.actuation.nu
+        self.update_tracking_costs(model.differential.costs, tasks)
 
-        self.createStandardModel(supportFootIds)
-        if isTerminal:
-            self.make_terminal_model()
-        else:
-            self.make_running_model(supportFootIds, swingFootTask)
 
-    def createStandardModel(self, supportFootIds):
-        """Action model for a swing foot phase.
+    def create_model(self, support_feet=[], tasks=[], is_terminal=False):
+        """
+        Create the action model
 
-        :param timeStep: step duration of the action model
-        :param supportFootIds: Ids of the constrained feet
-        :param comTask: CoM task
-        :param swingFootTask: swinging foot task
+        :param state: swinging foot task
+        :param support_feet: list of support feet ids
+        :param task: list of support feet ids and associated tracking reference
+        :param isTterminal: true for the terminal node
         :return action model for a swing foot phase
         """
+        model = self.create_standard_model(support_feet)
+        if is_terminal:
+            self.make_terminal_model(model)
+        else:
+            self.make_running_model(model, support_feet, tasks)
 
-        self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
-        for i in supportFootIds:
-            supportContactModel = crocoddyl.ContactModel3D(
-                self.state, i, np.array(
-                    [0.0, 0.0, 0.0]), self.nu, np.array([0, 50])
-            )
-            self.contactModel.addContact(
-                self.pd.model.frames[i].name + "_contact", supportContactModel
-            )
-
-        # Creating the cost model for a contact phase
-        costModel = crocoddyl.CostModelSum(self.state, self.nu)
+        return model
 
-        stateResidual = crocoddyl.ResidualModelState(
-            self.state, self.pd.xref, self.nu)
-        stateActivation = crocoddyl.ActivationModelWeightedQuad(
-            self.pd.state_reg_w**2
-        )
-        stateReg = crocoddyl.CostModelResidual(
-            self.state, stateActivation, stateResidual
-        )
-        costModel.addCost("stateReg", stateReg, 1)
+    def create_standard_model(self, support_feet):
+        """
+        Create a standard action model
 
-        self.costModel = costModel
+        :param state: swinging foot task
+        :param support_feet: list of support feet ids
+        :return action model for a swing foot phase
+        """
+        if self.pd.useFixedBase == 0:
+            actuation = crocoddyl.ActuationModelFloatingBase(self.state)
+        else:
+            actuation = crocoddyl.ActuationModelFull(self.state)
+        nu = actuation.nu
 
-        self.dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
-            self.state, self.actuation, self.contactModel, self.costModel, 0.0, True
-        )
-        self.model = crocoddyl.IntegratedActionModelEuler(
-            self.dmodel, self.control, self.pd.dt
-        )
+        control = crocoddyl.ControlParametrizationModelPolyZero(nu)
 
-    def make_terminal_model(self):
-        self.isTerminal = True
-        stateResidual = crocoddyl.ResidualModelState(
-            self.state, self.pd.xref, self.nu)
-        stateActivation = crocoddyl.ActivationModelWeightedQuad(
-            self.pd.terminal_velocity_w**2
-        )
-        stateReg = crocoddyl.CostModelResidual(
-            self.state, stateActivation, stateResidual
+        contacts = crocoddyl.ContactModelMultiple(self.state, nu)
+        for i in self.pd.allContactIds:
+            name = self.pd.model.frames[i].name + "_contact"
+            contact = crocoddyl.ContactModel3D(
+                self.state, i, np.zeros(3), nu, np.zeros(2)
+            )
+            contacts.addContact(name, contact)
+            if i not in support_feet:
+                contacts.changeContactStatus(name, False)
+
+        costs = crocoddyl.CostModelSum(self.state, nu)
+        residual = crocoddyl.ResidualModelState(self.state, self.pd.xref, nu)
+        activation = crocoddyl.ActivationModelWeightedQuad(self.pd.state_reg_w**2)
+        state_cost = crocoddyl.CostModelResidual(self.state, activation, residual)
+        costs.addCost("state_reg", state_cost, 1)
+
+        differential = crocoddyl.DifferentialActionModelContactFwdDynamics(
+            self.state, actuation, contacts, costs, 0.0, True
         )
-        self.costModel.addCost("terminalVelocity", stateReg, 1)
-
-    def make_running_model(self, supportFootIds, swingFootTask):
+        model = crocoddyl.IntegratedActionModelEuler(differential, control, self.pd.dt)
 
-        ctrlResidual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
-        ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
-        self.costModel.addCost("ctrlReg", ctrlReg, self.pd.control_reg_w)
+        return model
 
-        ctrl_bound_residual = crocoddyl.ResidualModelControl(
-            self.state, self.nu)
-        ctrl_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
-            crocoddyl.ActivationBounds(-self.pd.effort_limit,
-                                       self.pd.effort_limit)
-        )
-        ctrl_bound = crocoddyl.CostModelResidual(
-            self.state, ctrl_bound_activation, ctrl_bound_residual
+    def make_terminal_model(self, model):
+        """
+        Add the final velocity cost to the terminal model
+        """
+        nu = model.differential.actuation.nu
+        residual = crocoddyl.ResidualModelState(self.state, self.pd.xref, nu)
+        activation = crocoddyl.ActivationModelWeightedQuad(
+            self.pd.terminal_velocity_w**2
         )
-        self.costModel.addCost("ctrlBound", ctrl_bound,
-                               self.pd.control_bound_w)
-
-        self.add_friction_cone(supportFootIds)
-        #self.add_force_reg(supportFootIds)
-        self.tracking_cost(swingFootTask, supportFootIds)
-
-    def update_contact_model(self, supportFootIds):
-        self.remove_contacts()
-        self.remove_friction_cone()
-        self.remove_force_reg()
-
-        self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
-        for i in supportFootIds:
-            supportContactModel = crocoddyl.ContactModel3D(
-                self.state, i, np.array(
-                    [0.0, 0.0, 0.0]), self.nu, np.array([0, 50])
-            )
-            self.dmodel.contacts.addContact(
-                self.pd.model.frames[i].name + "_contact", supportContactModel
-            )
+        state_cost = crocoddyl.CostModelResidual(self.state, activation, residual)
+        model.differential.costs.addCost("terminal_velocity", state_cost, 1)
 
-        self.add_friction_cone(supportFootIds)
-        #self.add_force_reg(supportFootIds)
-
-    def add_friction_cone(self, supportFootIds):
-        for i in supportFootIds:
+    def make_running_model(self, model, support_feet, tasks):
+        """
+        Add all the costs to the running models
+        """
+        nu = model.differential.actuation.nu
+        costs = model.differential.costs
+        for i in self.pd.allContactIds:
             cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False)
-            coneResidual = crocoddyl.ResidualModelContactFrictionCone(
-                self.state, i, cone, self.nu
+            residual = crocoddyl.ResidualModelContactFrictionCone(
+                self.state, i, cone, nu
             )
-            coneActivation = crocoddyl.ActivationModelQuadraticBarrier(
+            activation = crocoddyl.ActivationModelQuadraticBarrier(
                 crocoddyl.ActivationBounds(cone.lb, cone.ub)
             )
-            frictionCone = crocoddyl.CostModelResidual(
-                self.state, coneActivation, coneResidual
-            )
-            self.costModel.addCost(
-                self.pd.model.frames[i].name + "_frictionCone",
-                frictionCone,
-                self.pd.friction_cone_w,
+            friction_cone = crocoddyl.CostModelResidual(
+                self.state, activation, residual
             )
+            friction_name = self.pd.model.frames[i].name + "_friction_cone"
+            costs.addCost(friction_name, friction_cone, self.pd.friction_cone_w)
+            costs.changeCostStatus(friction_name, i in support_feet)
 
-    def add_force_reg(self, supportFootIds):
-        robotweight = -sum([Y.mass for Y in self.pd.model.inertias]) * self.pd.model.gravity.linear[2]
-        for cid in supportFootIds:
-            R = self.pd.rdata.oMf[cid].rotation
-            cFo = np.linalg.inv(R) @ np.array([0, 0, robotweight/len(supportFootIds)])
-            forceRefResidual = crocoddyl.ResidualModelContactForce(
-                self.state, cid, pin.Force(cFo, 0* cFo), 3, self.actuation.nu )
-            forceRefCost = crocoddyl.CostModelResidual(self.state, forceRefResidual)
-            self.costModel.addCost( self.pd.model.frames[cid].name + "_forceReg", 
-                                    forceRefCost,
-                                    self.pd.force_reg_w
+            name = self.pd.model.frames[i].name + "_foot_tracking"
+            residual = crocoddyl.ResidualModelFrameTranslation(
+                self.state, i, np.zeros(3), nu
             )
+            foot_tracking = crocoddyl.CostModelResidual(self.state, residual)
+            costs.addCost(name, foot_tracking, self.pd.foot_tracking_w)
+
+            costs.changeCostStatus(name, False)
+
+        control_residual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
+        control_reg = crocoddyl.CostModelResidual(self.state, control_residual)
+        costs.addCost("control_reg", control_reg, self.pd.control_reg_w)
 
-    def tracking_cost(self, task, supportFootIds):
-        if task is not None:
-            for (id, pose) in task:
-                if id not in supportFootIds:
-                    frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
-                        self.state, id, pose, self.nu
-                    )
-                    footTrack = crocoddyl.CostModelResidual(
-                        self.state, frameTranslationResidual
-                    )
-                    if (
-                        self.pd.model.frames[id].name + "_footTrack"
-                        in self.dmodel.costs.active.tolist()
-                    ):
-                        self.dmodel.costs.removeCost(
-                            self.pd.model.frames[id].name + "_footTrack"
-                        )
-                    self.costModel.addCost(
-                        self.pd.model.frames[id].name + "_footTrack",
-                        footTrack,
-                        self.pd.foot_tracking_w,
-                    )
-
-    def remove_running_costs(self):
-        runningCosts = self.dmodel.costs.active.tolist()
-        idx = runningCosts.index("stateReg")
-        runningCosts.pop(idx)
-        for cost in runningCosts:
-            if cost in self.dmodel.costs.active.tolist():
-                self.dmodel.costs.removeCost(cost)
-
-    def remove_terminal_cost(self):
-        if "terminalVelocity" in self.dmodel.costs.active.tolist():
-            self.dmodel.costs.removeCost("terminalVelocity")
-
-    def remove_contacts(self):
-        allContacts = self.dmodel.contacts.contacts.todict()
-        for c in allContacts:
-            self.dmodel.contacts.removeContact(c)
-
-    # TODO Remove just the useless friction cone instead of all
-    def remove_friction_cone(self):
-        fc_names = [c for c in self.dmodel.costs.active.tolist()
-                    if "frictionCone" in c]
-        for c in fc_names:
-            self.dmodel.costs.removeCost(c)
-
-    # TODO Remove just the useless friction cone instead of all
-    def remove_force_reg(self):
-        freg_names = [c for c in self.dmodel.costs.active.tolist()
-                    if "forceReg" in c]
-        for c in freg_names:
-            self.dmodel.costs.removeCost(c)
-
-    def update_model(self, support_feet=[], task=[]):
-        self.update_contact_model(support_feet)
-        if not self.isTerminal:
-            self.tracking_cost(task, support_feet)
+        control_bound_residual = crocoddyl.ResidualModelControl(self.state, nu)
+        control_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
+            crocoddyl.ActivationBounds(-self.pd.effort_limit, self.pd.effort_limit)
+        )
+        control_bound = crocoddyl.CostModelResidual(
+            self.state, control_bound_activation, control_bound_residual
+        )
+        costs.addCost("control_bound", control_bound, self.pd.control_bound_w)
+
+        self.update_tracking_costs(costs, tasks)
+    
+    def update_tracking_costs(self, costs, tasks):
+        for i in self.pd.allContactIds:
+            name = self.pd.model.frames[i].name + "_foot_tracking"
+            index = 0
+            if i in tasks[0]:
+                costs.changeCostStatus(name, True)
+                costs.costs[name].cost.residual.reference = tasks[1][index]
+                index += 1
+            else:
+                costs.changeCostStatus(name, False)

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -8,9 +8,9 @@ class problemDataAbstract:
         self.dt = param.dt_mpc  # OCP dt
         self.dt_wbc = param.dt_wbc
         self.mpc_wbc_ratio = int(self.dt / self.dt_wbc)
-        self.init_steps = 10
-        self.target_steps = 90
-        self.T = self.init_steps + self.target_steps - 1
+        self.init_steps = 0
+        self.target_steps = param.gait.shape[0]
+        self.T = param.gait.shape[0] - 1
 
         self.robot = erd.load("solo12")
         self.q0 = self.robot.q0
@@ -43,7 +43,7 @@ class problemDataAbstract:
         )  # -1 to take into account the freeflyer
         self.ntau = self.nv
 
-        self.effort_limit = np.ones(self.nu) * 3
+        self.effort_limit = np.ones(self.nu) * 2.5
 
         self.v0 = np.zeros(18)
         self.x0 = np.concatenate([self.q0, self.v0])
@@ -103,9 +103,8 @@ class ProblemData(problemDataAbstract):
                                     + [1e-1] * 3
                                     + [1e1] * 6
                                     )
-        self.terminal_velocity_w = np.array(
-            [0] * self.nv + [1e3] * self.nv)
-        self.force_reg_w = 1e0
+        self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18)
+        self.control_bound_w = 1e3
 
         self.xref = self.x0
         self.uref =self.u0
@@ -113,27 +112,22 @@ class ProblemData(problemDataAbstract):
 
 class ProblemDataFull(problemDataAbstract):
     def __init__(self, param):
-        frozen_names = [
-            "root_joint"]
+        frozen_names = ["root_joint"]
 
         super().__init__(param, frozen_names)
 
         self.useFixedBase = 1
 
-        # Cost function weights
         # Cost function weights
         self.mu = 0.7
-        self.foot_tracking_w = 1e3
-        # self.friction_cone_w = 1e3 * 0
+        self.foot_tracking_w = 1e4
+        self.friction_cone_w = 1e3
         self.control_bound_w = 1e3
         self.control_reg_w = 1e0
-        self.state_reg_w = np.array([1e0] * 3
-                                    + [1e-5] * 3
-                                    + [1e0] * 6
-                                    + [1e1] * 3
-                                    + [1e0] * 3
-                                    + [1e1] * 6
-                                    )
+        self.state_reg_w = np.array(
+            [1e2] * 3 + [1e-2] * 3 + [1e2] * 6 +
+            [1e1] * 3 + [1e0] * 3 + [1e1] * 6
+        )
         self.terminal_velocity_w = np.array([0] * 12 + [1e3] * 12)
 
         self.q0_reduced = self.q0[7:]

python/quadruped_reactive_walking/WB_MPC/Target.py

+from tracemalloc import take_snapshot
 import numpy as np
 from .ProblemData import ProblemData
 import pinocchio as pin
 
 
 class Target:
-    def __init__(self, pd: ProblemData):
-        self.dt = pd.dt
-        pin.forwardKinematics(pd.model, pd.rdata, pd.q0, pd.v0)
-        pin.updateFramePlacements(pd.model, pd.rdata)
-        self.foot_pose = pd.rdata.oMf[pd.rfFootId].translation.copy()
-        self.A = np.array([0, 0.05, 0.05])
-        self.offset = np.array([0.05, 0, 0.06])
-        self.freq = np.array([0, 0.5 * 0, 0.5*0])
-        self.phase = np.array([0, 0, np.pi / 2])
-        self.t_offset = 0
-
-    def shift(self):
-        self.t_offset += 1
-
-    def evaluate_circle(self, t):
-        return (
-            self.foot_pose
-            + self.offset
-            + self.A
-            * np.sin(2 * np.pi * self.freq * (self.t_offset + t) * self.dt + self.phase)
+    def __init__(self, params):
+        self.params = params
+        self.dt_wbc = params.dt_wbc
+        self.k_per_step = 160
+
+        self.position = np.array(params.footsteps_under_shoulders).reshape(
+            (3, 4), order="F"
         )
 
-    def footstep(self, t):
+        if params.movement == "circle":
+            self.A = np.array([0, 0.03, 0.03])
+            self.offset = np.array([0.05, -0.02, 0.06])
+            self.freq = np.array([0, 0.5, 0.5])
+            self.phase = np.array([0, np.pi / 2, 0])
+        elif params.movement == "step":
+            self.initial = self.position[:, 1].copy()
+            self.target = self.position[:, 1].copy() + np.array([0.1, 0.0, 0.0])
+
+            self.vert_time = params.vert_time
+            self.max_height = params.max_height
+            self.T = self.k_per_step * self.dt_wbc
+            self.A = np.zeros((6, 3))
+
+            self.update_time = -1
+        else:
+            self.target_footstep = self.position + np.array([0.0, 0.0, 0.10])
+
+    def compute(self, k):
         footstep = np.zeros((3, 4))
-        footstep[:, 1] = self.evaluate_circle(t)
+        if self.params.movement == "circle":
+            footstep[:, 1] = self.evaluate_circle(k)
+        elif self.params.movement == "step":
+            footstep[:, 1] = self.evaluate_step(1, k)
+        else:
+            footstep = self.target_footstep.copy()
+
         return footstep
+
+    def evaluate_circle(self, k):
+        return (
+            self.position[:, 1]
+            + self.offset
+            + self.A * np.sin(2 * np.pi * self.freq * k * self.dt_wbc + self.phase)
+        )
+
+    def evaluate_step(self, j, k):
+        n_step = k // self.k_per_step
+        if n_step % 2 == 0:
+            return self.initial.copy() if (n_step % 4 == 0) else self.target.copy()
+
+        if n_step % 4 == 1:
+            initial = self.initial
+            target = self.target
+        else:
+            initial = self.target
+            target = self.initial
+
+        k_step = k % self.k_per_step
+        if n_step != self.update_time:
+            self.update_polynomial(initial, target)
+            self.update_time = n_step
+
+        t = k_step * self.dt_wbc
+        return self.compute_position(j, t)
+
+    def update_polynomial(self, initial, target):
+
+        x0 = initial[0]
+        y0 = initial[1]
+
+        x1 = target[0]
+        y1 = target[1]
+
+        # elapsed time
+        t = 0
+        d = self.T - 2 * self.vert_time
+
+        A = np.zeros((6, 3))
+
+        A[0, 0] = 12 * (x0 - x1) / (2 * (t - d) ** 5)
+        A[1, 0] = 30 * (x1 - x0) * (t + d) / (2 * (t - d) ** 5)
+        A[2, 0] = 20 * (x0 - x1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
+        A[3, 0] = 60 * (x1 - x0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
+        A[4, 0] = 60 * (x0 - x1) * (t**2 * d**2) / (2 * (t - d) ** 5)
+        A[5, 0] = (
+            2 * x1 * t**5
+            - 10 * x1 * t**4 * d
+            + 20 * x1 * t**3 * d**2
+            - 20 * x0 * t**2 * d**3
+            + 10 * x0 * t * d**4
+            - 2 * x0 * d**5
+        ) / (2 * (t - d) ** 5)
+
+        A[0, 1] = 12 * (y0 - y1) / (2 * (t - d) ** 5)
+        A[1, 1] = 30 * (y1 - y0) * (t + d) / (2 * (t - d) ** 5)
+        A[2, 1] = 20 * (y0 - y1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
+        A[3, 1] = 60 * (y1 - y0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
+        A[4, 1] = 60 * (y0 - y1) * (t**2 * d**2) / (2 * (t - d) ** 5)
+        A[5, 1] = (
+            2 * y1 * t**5
+            - 10 * y1 * t**4 * d
+            + 20 * y1 * t**3 * d**2
+            - 20 * y0 * t**2 * d**3
+            + 10 * y0 * t * d**4
+            - 2 * y0 * d**5
+        ) / (2 * (t - d) ** 5)
+
+        A[0, 2] = -self.max_height / ((self.T / 2) ** 6)
+        A[1, 2] = 3 * self.T * self.max_height / ((self.T / 2) ** 6)
+        A[2, 2] = -3 * self.T**2 * self.max_height / ((self.T / 2) ** 6)
+        A[3, 2] = self.T**3 * self.max_height / ((self.T / 2) ** 6)
+
+        self.A = A
+
+    def compute_position(self, j, t):
+        A = self.A.copy()
+
+        t_xy = t - self.vert_time
+        t_xy = max(0.0, t_xy)
+        t_xy = min(t_xy, self.T - 2 * self.vert_time)
+        self.position[:2, j] = (
+            A[5, :2]
+            + A[4, :2] * t_xy
+            + A[3, :2] * t_xy**2
+            + A[2, :2] * t_xy**3
+            + A[1, :2] * t_xy**4
+            + A[0, :2] * t_xy**5
+        )
+
+        self.position[2, j] = (
+            A[3, 2] * t**3 + A[2, 2] * t**4 + A[1, 2] * t**5 + A[0, 2] * t**6
+        )
+
+        return self.position[:, j]
+
+    def evaluate_circle(self, k):
+        return (
+            self.position[:, 1]
+            + self.offset
+            + self.A * np.sin(2 * np.pi * self.freq * k * self.dt_wbc + self.phase)
+        )
+
+    def evaluate_step(self, j, k):
+        n_step = k // self.k_per_step
+        if n_step % 2 == 0:
+            return self.initial.copy() if (n_step % 4 == 0) else self.target.copy()
+
+        if n_step % 4 == 1:
+            initial = self.initial
+            target = self.target
+        else:
+            initial = self.target
+            target = self.initial
+
+        k_step = k % self.k_per_step
+        if n_step != self.update_time:
+            self.update_polynomial(initial, target)
+            self.update_time = n_step
+
+        t = k_step * self.dt_wbc
+        return self.compute_position(j, t)
+
+    def update_polynomial(self, initial, target):
+
+        x0 = initial[0]
+        y0 = initial[1]
+
+        x1 = target[0]
+        y1 = target[1]
+
+        # elapsed time
+        t = 0
+        d = self.T - 2 * self.vert_time
+
+        A = np.zeros((6, 3))
+
+        A[0, 0] = 12 * (x0 - x1) / (2 * (t - d) ** 5)
+        A[1, 0] = 30 * (x1 - x0) * (t + d) / (2 * (t - d) ** 5)
+        A[2, 0] = 20 * (x0 - x1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
+        A[3, 0] = 60 * (x1 - x0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
+        A[4, 0] = 60 * (x0 - x1) * (t**2 * d**2) / (2 * (t - d) ** 5)
+        A[5, 0] = (
+            2 * x1 * t**5
+            - 10 * x1 * t**4 * d
+            + 20 * x1 * t**3 * d**2
+            - 20 * x0 * t**2 * d**3
+            + 10 * x0 * t * d**4
+            - 2 * x0 * d**5
+        ) / (2 * (t - d) ** 5)
+
+        A[0, 1] = 12 * (y0 - y1) / (2 * (t - d) ** 5)
+        A[1, 1] = 30 * (y1 - y0) * (t + d) / (2 * (t - d) ** 5)
+        A[2, 1] = 20 * (y0 - y1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
+        A[3, 1] = 60 * (y1 - y0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
+        A[4, 1] = 60 * (y0 - y1) * (t**2 * d**2) / (2 * (t - d) ** 5)
+        A[5, 1] = (
+            2 * y1 * t**5
+            - 10 * y1 * t**4 * d
+            + 20 * y1 * t**3 * d**2
+            - 20 * y0 * t**2 * d**3
+            + 10 * y0 * t * d**4
+            - 2 * y0 * d**5
+        ) / (2 * (t - d) ** 5)
+
+        A[0, 2] = -self.max_height / ((self.T / 2) ** 6)
+        A[1, 2] = 3 * self.T * self.max_height / ((self.T / 2) ** 6)
+        A[2, 2] = -3 * self.T**2 * self.max_height / ((self.T / 2) ** 6)
+        A[3, 2] = self.T**3 * self.max_height / ((self.T / 2) ** 6)
+
+        self.A = A
+
+    def compute_position(self, j, t):
+        A = self.A.copy()
+
+        t_xy = t - self.vert_time
+        t_xy = max(0.0, t_xy)
+        t_xy = min(t_xy, self.T - 2 * self.vert_time)
+        self.position[:2, j] = (
+            A[5, :2]
+            + A[4, :2] * t_xy
+            + A[3, :2] * t_xy**2
+            + A[2, :2] * t_xy**3
+            + A[1, :2] * t_xy**4
+            + A[0, :2] * t_xy**5
+        )
+
+        self.position[2, j] = (
+            A[3, 2] * t**3 + A[2, 2] * t**4 + A[1, 2] * t**5 + A[0, 2] * t**6
+        )
+
+        return self.position[:, j]

python/quadruped_reactive_walking/WB_MPC_Wrapper.py

@@ -112,6 +112,7 @@ class MPC_Wrapper:
             self.last_available_result.xs = [x0 for _ in range(self.pd.T + 1)]
             p = Process(target=self.MPC_asynchronous)
             p.start()
+
         self.add_new_data(k, x0, footstep, gait, xs, us)
 
     def MPC_asynchronous(self):

python/quadruped_reactive_walking/main_solo12_control.py

@@ -161,7 +161,6 @@ def control_loop():
         put_on_the_floor(device, q_init)
 
     # CONTROL LOOP ***************************************************
-
     t = 0.0
     t_max = (params.N_SIMULATION - 1) * params.dt_wbc
 

python/quadruped_reactive_walking/tools/LoggerControl.py

@@ -100,7 +100,6 @@ class LoggerControl:
             self.mocapOrientationQuat[self.i] = device.baseState[1]
 
         # Controller timings: MPC time, ...
-        self.target[self.i] = controller.target.evaluate_circle(0)
         self.t_mpc[self.i] = controller.t_mpc
         self.t_send[self.i] = controller.t_send
         self.t_loop[self.i] = controller.t_loop
@@ -119,6 +118,13 @@ class LoggerControl:
         self.t_loop[self.i] = controller.t_loop
 
         self.t_ocp_ddp[self.i] = controller.mpc_result.solving_duration
+
+        if self.i == 0:
+            for i in range(self.pd.T * self.pd.mpc_wbc_ratio):
+                self.target[i] = controller.footsteps[i // self.pd.mpc_wbc_ratio][:, 1]
+        if self.i + self.pd.T * self.pd.mpc_wbc_ratio < self.log_size:
+            self.target[self.i + self.pd.T * self.pd.mpc_wbc_ratio] = controller.target_footstep[:, 1]
+
         if not self.params.enable_multiprocessing:
             self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
             self.t_ocp_warm_start[self.i] = controller.mpc.ocp.t_warm_start