Planner refactor

* Code cleaning and refactor
* Add Types, FootTrajectoryGenerator, Gait, FootstepPlanner classes
* Rename quadruped-reactive-walking qrw

Authored-by: Fanny Risbourg <frisbourg@laas.fr>

.gitignore

@@ -16,6 +16,7 @@ cprofile.txt
 # Distribution / packaging
 .Python
 build/
+cmake/
 develop-eggs/
 dist/
 downloads/
@@ -134,3 +135,7 @@ dmypy.json
 
 # Pyre type checker
 .pyre/
+
+.clang-format
+.vscode/
+scripts/solopython

CMakeLists.txt

@@ -44,22 +44,25 @@ SEARCH_FOR_BOOST()
 
 # Main Library
 set(${PROJECT_NAME}_HEADERS
-  include/${PROJECT_NAME}/gepadd.hpp
-  include/${PROJECT_NAME}/MPC.hpp
-  include/${PROJECT_NAME}/Planner.hpp
-  include/${PROJECT_NAME}/InvKin.hpp
-  include/${PROJECT_NAME}/QPWBC.hpp
+  include/qrw/gepadd.hpp
+  include/qrw/MPC.hpp
+  include/qrw/Planner.hpp
+  include/qrw/Gait.hpp
+  include/qrw/FootTrajectoryGenerator.hpp
+  include/qrw/FootstepPlanner.hpp
+  include/qrw/Types.h
+  include/qrw/InvKin.hpp
+  include/qrw/QPWBC.hpp
   include/other/st_to_cc.hpp
-  #include/osqp_folder/include/osqp_configure.h
-  #include/osqp_folder/include/osqp.h
-  #include/osqp_folder/include/cs.h
-  #include/osqp_folder/include/util.h
   )
 
 set(${PROJECT_NAME}_SOURCES
   src/st_to_cc.cpp
   src/gepadd.cpp
   src/MPC.cpp
+  src/Gait.cpp
+  src/FootTrajectoryGenerator.cpp
+  src/FootstepPlanner.cpp
   src/Planner.cpp
   src/InvKin.cpp
   src/QPWBC.cpp

README.md

-# quadruped-reactive-walking
+# qrw
 
 Implementation of a reactive walking controller for quadruped robots. Architecture mainly in Python with some parts in C++ with bindings to Python.
 
@@ -72,7 +72,7 @@ Implementation of a reactive walking controller for quadruped robots. Architectu
 
 * You can define a new gait in `src/Planner.cpp` using `create_trot` or `create_walk` as models. Create a new function (like `create_bounding` for a bounding gait) and call it inside the Planner constructor before `create_gait_f()`.
 
-* You can modify the swinging feet apex height in `include/quadruped-reactive-control/Planner.hpp` with `max_height_feet` or the lock time before touchdown with `t_lock_before_touchdown` (to lock the target location on the ground before touchdown).
+* You can modify the swinging feet apex height in `include/quadruped-reactive-control/Planner.hpp` with `maxHeight_` or the lock time before touchdown with `lockTime_` (to lock the target location on the ground before touchdown).
 
 * For the MPC QP problem you can tune weights of the Q and P matrices in the `create_weight_matrices` function of `src/MPC.cpp`.
 

cmake @ a61ae614

-Subproject commit a61ae61479a1d50d1ae3c988d1d9aaf20841173d

include/qrw/FootTrajectoryGenerator.hpp

+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for FootTrajectoryGenerator class
+///
+/// \details This class generates a reference trajectory for the swing foot, in position, velocity
+///           and acceleration
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef TRAJGEN_H_INCLUDED
+#define TRAJGEN_H_INCLUDED
+
+#include "qrw/Gait.hpp"
+#include "qrw/Types.h"
+
+class FootTrajectoryGenerator
+{
+public:
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Constructor
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    FootTrajectoryGenerator();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Initialize with given data
+    ///
+    /// \param[in] maxHeightIn Apex height of the swinging trajectory
+    /// \param[in] lockTimeIn Target lock before the touchdown
+    /// \param[in] target desired target location at the end of the swing phase
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void initialize(double const maxHeightIn,
+                    double const lockTimeIn,
+                    Matrix34 const& targetFootstepIn,
+                    Matrix34 const& initialFootPosition,
+                    double const& dt_tsid_in,
+                    int const& k_mpc_in,
+                    std::shared_ptr<Gait> gait);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Destructor.
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ~FootTrajectoryGenerator() {}  // Empty constructor
+
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief updates the nex foot position, velocity and acceleration, and the foot goal position
+    ///
+    /// \param[in] j foot id
+    /// \param[in] targetFootstep desired target location at the end of the swing phase
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void updateFootPosition(int const j, Vector3 const& targetFootstep);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Update the 3D desired position for feet in swing phase by using a 5-th order polynomial that lead them
+    ///        to the desired position on the ground (computed by the footstep planner)
+    ///
+    /// \param[in] k (int): number of time steps since the start of the simulation
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void update(int k, MatrixN const& targetFootstep);
+
+    MatrixN getTargetPosition() { return targetFootstep_; }  ///< Get the foot goal position
+    MatrixN getFootPosition() { return position_; }          ///< Get the next foot position
+    MatrixN getFootVelocity() { return velocity_; }          ///< Get the next foot velocity
+    MatrixN getFootAcceleration() { return acceleration_; }  ///< Get the next foot acceleration
+
+private:
+    double dt_tsid;     ///<
+    int k_mpc;          ///<
+    double maxHeight_;  ///< Apex height of the swinging trajectory
+    double lockTime_;   ///< Target lock before the touchdown
+    std::shared_ptr<Gait> gait_;         ///< Target lock before the touchdown
+
+    std::vector<int> feet;
+    Vector4 t0s;
+    Vector4 t_swing;
+
+    Matrix64 Ax;  ///< Coefficients for the X component
+    Matrix64 Ay;  ///< Coefficients for the Y component
+
+    Matrix34 targetFootstep_;  // Target for the X component
+
+    Matrix34 position_;      // position computed in updateFootPosition
+    Matrix34 velocity_;      // velocity computed in updateFootPosition
+    Matrix34 acceleration_;  // acceleration computed in updateFootPosition
+};
+#endif  // PLANNER_H_INCLUDED

include/qrw/FootstepPlanner.hpp

+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for FootstepPlanner class
+///
+/// \details Planner that outputs current and future locations of footsteps, the reference
+///          trajectory of the base based on the reference velocity given by the user and the current
+///          position/velocity of the base
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef FOOTSTEPPLANNER_H_INCLUDED
+#define FOOTSTEPPLANNER_H_INCLUDED
+
+#include "pinocchio/math/rpy.hpp"
+#include "qrw/Gait.hpp"
+#include "qrw/Types.h"
+
+#define N0_gait 20
+// Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is always at
+// least one empty line at the end of the gait matrix
+
+// Order of feet/legs: FL, FR, HL, HR
+
+class FootstepPlanner
+{
+public:
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Empty constructor
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    FootstepPlanner();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Default constructor
+    ///
+    /// \param[in] dt_in
+    /// \param[in] T_mpc_in
+    /// \param[in] h_ref_in
+    /// \param[in] shoulderIn
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void initialize(double dt_in,
+                    double k_mpc_in,
+                    double T_mpc_in,
+                    double h_ref_in,
+                    Matrix34 const& shouldersIn,
+                    std::shared_ptr<Gait> gaitIn);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Destructor.
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ~FootstepPlanner() {}
+
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    MatrixN computeTargetFootstep(int const k,
+                                  VectorN const& q,
+                                  Vector6 const& v,
+                                  Vector6 const& b_vref,
+                                  double const z_average);
+
+    MatrixN getXReference();
+    MatrixN getFootsteps();
+    MatrixN getTargetFootsteps();
+
+private:
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Compute a X by 13 matrix containing the remaining number of steps of each phase of the gait (first column)
+    ///        and the [x, y, z]^T desired position of each foot for each phase of the gait (12 other columns).
+    ///        For feet currently touching the ground the desired position is where they currently are.
+    ///
+    /// \param[in] q current position vector of the flying base in world frame(linear and angular stacked)
+    /// \param[in] v current velocity vector of sthe flying base in world frame(linear and angular stacked)
+    /// \param[in] vref desired velocity vector of the flying base in world frame(linear and angular stacked)
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void compute_footsteps(VectorN const& q, Vector6 const& v, Vector6 const& vref);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Compute the target location on the ground of a given foot for an upcoming stance phase
+    ///
+    /// \param[in] i considered phase (row of the gait matrix)
+    /// \param[in] j considered foot (col of the gait matrix)
+    ///
+    /// \retval Matrix with the next footstep positions
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void compute_next_footstep(int i, int j);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Compute the reference trajectory of the CoM for each time step of the
+    ///        predition horizon. The ouput is a matrix of size 12 by (N+1) with N the number
+    ///        of time steps in the gait cycle (T_gait/dt) and 12 the position, orientation,
+    ///        linear velocity and angular velocity vertically stacked. The first column contains
+    ///        the current state while the remaining N columns contains the desired future states.
+    ///
+    /// \param[in] q current position vector of the flying base in world frame (linear and angular stacked)
+    /// \param[in] v current velocity vector of the flying base in world frame (linear and angular stacked)
+    /// \param[in] vref desired velocity vector of the flying base in world frame (linear and angular stacked)
+    /// \param[in] z_average average height of feet currently in stance phase
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    int getRefStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Update desired location of footsteps using information coming from the footsteps planner
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void update_target_footsteps();
+
+    MatrixN vectorToMatrix(std::array<Matrix34, N0_gait> const& array);
+
+    std::shared_ptr<Gait> gait_;  // Gait object to hold the gait informations
+
+    double dt;      // Time step of the contact sequence (time step of the MPC)
+    double T_gait;  // Gait period
+    int k_mpc;      // Number of TSID iterations for one iteration of the MPC
+    double T_mpc;   // MPC period (prediction horizon)
+    double h_ref;   // Reference height for the trunk
+
+    // Predefined quantities
+    double k_feedback;  // Feedback gain for the feedback term of the planner
+    double g;           // Value of the gravity acceleartion
+    double L;           // Value of the maximum allowed deviation due to leg length
+
+    // Number of time steps in the prediction horizon
+    int n_steps;  // T_mpc / time step of the MPC
+
+    // Constant sized matrices
+    Matrix34 shoulders_;        // Position of shoulders in local frame
+    Matrix34 currentFootstep_;  // Feet matrix in world frame
+    Matrix34 nextFootstep_;     // Feet matrix in world frame
+    Matrix34 targetFootstep_;
+    std::array<Matrix34, N0_gait> footsteps_;
+
+    Matrix3 Rz;  // Predefined matrices for compute_footstep function
+    VectorN dt_cum;
+    VectorN yaws;
+    VectorN dx;
+    VectorN dy;
+
+    Vector3 q_tmp;
+    Vector3 q_dxdy;
+    Vector3 RPY;
+    Vector3 b_v;
+    Vector6 b_vref;
+
+    // Reference trajectory matrix of size 12 by (1 + N)  with the current state of
+    // the robot in column 0 and the N steps of the prediction horizon in the others
+    MatrixN xref;
+};
+
+#endif  // FOOTSTEPPLANNER_H_INCLUDED

include/qrw/Gait.hpp

+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Gait class
+///
+/// \details Planner that outputs current and future locations of footsteps, the reference
+///          trajectory of the base and the position, velocity, acceleration commands for feet in
+///          swing phase based on the reference velocity given by the user and the current
+///          position/velocity of the base
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef GAIT_H_INCLUDED
+#define GAIT_H_INCLUDED
+
+#include "qrw/Types.h"
+
+#define N0_gait 20
+// Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is always at
+// least one empty line at the end of the gait matrix
+
+// Order of feet/legs: FL, FR, HL, HR
+
+class Gait
+{
+public:
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Empty constructor
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    Gait();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Destructor.
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ~Gait() {}
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Initializer
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void initialize(double dt_in, double T_gait_in, double T_mpc_in);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief  Create a slow walking gait, raising and moving only one foot at a time
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    int create_walk();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Create a trot gait with diagonaly opposed legs moving at the same time
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    int create_trot();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Create a pacing gait with legs on the same side (left or right) moving at the same time
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    int create_pacing();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Create a bounding gait with legs on the same side (front or hind) moving at the same time
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    int create_bounding();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Create a static gait with all legs in stance phase
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    int create_static();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Initialize content of the gait matrix based on the desired gait, the gait period and
+    ///        the length of the prediciton horizon
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    int create_gait_f();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Compute the remaining and total duration of a swing phase or a stance phase based
+    ///        on the content of the gait matrix
+    ///
+    /// \param[in] i considered phase (row of the gait matrix)
+    /// \param[in] j considered foot (col of the gait matrix)
+    /// \param[in] value 0.0 for swing phase detection, 1.0 for stance phase detection
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    double getPhaseDuration(int i, int j, double value);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief  Move one step further in the gait cycle
+    ///
+    /// \details Decrease by 1 the number of remaining step for the current phase of the gait
+    ///           Transfer current gait phase into past gait matrix
+    ///           Insert future desired gait phase at the end of the gait matrix
+    ///
+    /// \param[in] k number of WBC iterations since the start of the simulation
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void roll(int k, Matrix34 const& footstep, Matrix34& currentFootstep);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Handle the joystick code to trigger events (change of gait for instance)
+    ///
+    /// \param[in] code integer to trigger events with the joystick
+    /// \param[in] q current position vector of the flying base in world frame (linear and angular stacked)
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    bool changeGait(int const code, VectorN const& q);
+
+    MatrixN getPastGait() { return pastGait_; }
+    MatrixN getCurrentGait() { return currentGait_; }
+    MatrixN getDesiredGait() { return desiredGait_; }
+    double getRemainingTime() { return remainingTime_; }
+    bool getIsStatic() { return is_static_; }
+    VectorN getQStatic() { return q_static_; }
+
+private:
+    MatrixN pastGait_;     // Past gait
+    MatrixN currentGait_;  // Current and future gait
+    MatrixN desiredGait_;  // Future desired gait
+
+    double dt_;      // Time step of the contact sequence (time step of the MPC)
+    double T_gait_;  // Gait period
+    double T_mpc_;   // MPC period (prediction horizon)
+
+    double remainingTime_;
+
+    bool is_static_;
+    VectorN q_static_;
+};
+
+#endif  // GAIT_H_INCLUDED

include/qrw/InvKin.hpp

+#ifndef INVKIN_H_INCLUDED
+#define INVKIN_H_INCLUDED
+
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/spatial/explog.hpp"
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <string>
+
+class InvKin
+{
+public:
+    InvKin();
+    InvKin(double dt_in);
+
+    Eigen::Matrix<double, 1, 3> cross3(Eigen::Matrix<double, 1, 3> left, Eigen::Matrix<double, 1, 3> right);
+
+    Eigen::MatrixXd refreshAndCompute(const Eigen::MatrixXd& x_cmd, const Eigen::MatrixXd& contacts,
+                                      const Eigen::MatrixXd& goals, const Eigen::MatrixXd& vgoals, const Eigen::MatrixXd& agoals,
+                                      const Eigen::MatrixXd& posf, const Eigen::MatrixXd& vf, const Eigen::MatrixXd& wf, const Eigen::MatrixXd& af,
+                                      const Eigen::MatrixXd& Jf, const Eigen::MatrixXd& posb, const Eigen::MatrixXd& rotb, const Eigen::MatrixXd& vb,
+                                      const Eigen::MatrixXd& ab, const Eigen::MatrixXd& Jb);
+    Eigen::MatrixXd computeInvKin(const Eigen::MatrixXd& posf, const Eigen::MatrixXd& vf, const Eigen::MatrixXd& wf, const Eigen::MatrixXd& af,
+                                  const Eigen::MatrixXd& Jf, const Eigen::MatrixXd& posb, const Eigen::MatrixXd& rotb, const Eigen::MatrixXd& vb, const Eigen::MatrixXd& ab,
+                                  const Eigen::MatrixXd& Jb);
+    Eigen::MatrixXd get_q_step();
+    Eigen::MatrixXd get_dq_cmd();
+
+private:
+    // Inputs of the constructor
+    double dt;  // Time step of the contact sequence (time step of the MPC)
+
+    // Matrices initialisation
+    Eigen::Matrix<double, 4, 3> feet_position_ref = Eigen::Matrix<double, 4, 3>::Zero();
+    Eigen::Matrix<double, 4, 3> feet_velocity_ref = Eigen::Matrix<double, 4, 3>::Zero();
+    Eigen::Matrix<double, 4, 3> feet_acceleration_ref = Eigen::Matrix<double, 4, 3>::Zero();
+    Eigen::Matrix<double, 1, 4> flag_in_contact = Eigen::Matrix<double, 1, 4>::Zero();
+    Eigen::Matrix<double, 3, 3> base_orientation_ref = Eigen::Matrix<double, 3, 3>::Zero();
+    Eigen::Matrix<double, 1, 3> base_angularvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
+    Eigen::Matrix<double, 1, 3> base_angularacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
+    Eigen::Matrix<double, 1, 3> base_position_ref = Eigen::Matrix<double, 1, 3>::Zero();
+    Eigen::Matrix<double, 1, 3> base_linearvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
+    Eigen::Matrix<double, 1, 3> base_linearacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
+    Eigen::Matrix<double, 6, 1> x_ref = Eigen::Matrix<double, 6, 1>::Zero();
+    Eigen::Matrix<double, 6, 1> x = Eigen::Matrix<double, 6, 1>::Zero();
+    Eigen::Matrix<double, 6, 1> dx_ref = Eigen::Matrix<double, 6, 1>::Zero();
+    Eigen::Matrix<double, 6, 1> dx = Eigen::Matrix<double, 6, 1>::Zero();
+    Eigen::Matrix<double, 18, 18> J = Eigen::Matrix<double, 18, 18>::Zero();
+    Eigen::Matrix<double, 18, 18> invJ = Eigen::Matrix<double, 18, 18>::Zero();
+    Eigen::Matrix<double, 1, 18> acc = Eigen::Matrix<double, 1, 18>::Zero();
+    Eigen::Matrix<double, 1, 18> x_err = Eigen::Matrix<double, 1, 18>::Zero();
+    Eigen::Matrix<double, 1, 18> dx_r = Eigen::Matrix<double, 1, 18>::Zero();
+
+    Eigen::Matrix<double, 4, 3> pfeet_err = Eigen::Matrix<double, 4, 3>::Zero();
+    Eigen::Matrix<double, 4, 3> vfeet_ref = Eigen::Matrix<double, 4, 3>::Zero();
+    Eigen::Matrix<double, 4, 3> afeet = Eigen::Matrix<double, 4, 3>::Zero();
+    Eigen::Matrix<double, 1, 3> e_basispos = Eigen::Matrix<double, 1, 3>::Zero();
+    Eigen::Matrix<double, 1, 3> abasis = Eigen::Matrix<double, 1, 3>::Zero();
+    Eigen::Matrix<double, 1, 3> e_basisrot = Eigen::Matrix<double, 1, 3>::Zero();
+    Eigen::Matrix<double, 1, 3> awbasis = Eigen::Matrix<double, 1, 3>::Zero();
+
+    Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(18, 1);
+    Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(18, 1);
+    Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(18, 1);
+
+    // Gains
+    double Kp_base_orientation = 100.0;
+    double Kd_base_orientation = 2.0 * std::sqrt(Kp_base_orientation);
+
+    double Kp_base_position = 100.0;
+    double Kd_base_position = 2.0 * std::sqrt(Kp_base_position);
+
+    double Kp_flyingfeet = 1000.0;
+    double Kd_flyingfeet = 5.0 * std::sqrt(Kp_flyingfeet);
+};
+
+template <typename _Matrix_Type_>
+_Matrix_Type_ pseudoInverse(const _Matrix_Type_& a, double epsilon = std::numeric_limits<double>::epsilon())
+{
+    Eigen::JacobiSVD<_Matrix_Type_> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
+    double tolerance = epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
+    return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
+}
+#endif  // INVKIN_H_INCLUDED

include/qrw/Planner.hpp

+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Planner class
+///
+/// \details Planner that outputs current and future locations of footsteps, the reference
+///          trajectory of the base and the position, velocity, acceleration commands for feet in
+///          swing phase based on the reference velocity given by the user and the current
+///          position/velocity of the base
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef PLANNER_H_INCLUDED
+#define PLANNER_H_INCLUDED
+
+#include "qrw/FootTrajectoryGenerator.hpp"
+#include "qrw/FootstepPlanner.hpp"
+#include "qrw/Gait.hpp"
+#include "qrw/Types.h"
+
+// Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is always at
+// least one empty line at the end of the gait matrix
+
+// Order of feet/legs: FL, FR, HL, HR
+
+class Planner
+{
+public:
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    /// \brief Empty constructor
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    Planner();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Default constructor
+    ///
+    /// \param[in] dt_in
+    /// \param[in] dt_tsid_in
+    /// \param[in] T_gait_in
+    /// \param[in] T_mpc_in
+    /// \param[in] k_mpc_in
+    /// \param[in] h_ref_in
+    /// \param[in] fsteps_in
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    Planner(double dt_in,
+            double dt_tsid_in,
+            double T_gait_in,
+            double T_mpc_in,
+            int k_mpc_in,
+            double h_ref_in,
+            Matrix34 const& intialFootsteps,
+            Matrix34 const& shouldersIn);
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    /// \brief Destructor.
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ~Planner() {}
+
+
+    void Print();
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    ///
+    /// \brief Run the planner for one iteration of the main control loop
+    ///
+    ///  \param[in] k  number of time steps since the start of the simulation
+    ///  \param[in] q  current position vector of the flying base in world frame (linear and angular stacked)
+    ///  \param[in] v  current velocity vector of the flying base in world frame (linear and angular stacked)
+    ///  \param[in] b_vref  desired velocity vector of the flying base in base frame (linear and angular stacked)
+    ///  \param[in] z_average  average height of feet currently in stance phase
+    ///  \param[in] joystick_code  integer to trigger events with the joystick
+    ///
+    ////////////////////////////////////////////////////////////////////////////////////////////////
+    void run_planner(int const k,
+                     VectorN const& q,
+                     Vector6 const& v,
+                     Vector6 const& b_vref,
+                     double const z_average,
+                     int const joystickCode);
+
+    // Accessors (to retrieve C data from Python)
+    MatrixN get_xref();
+    MatrixN get_fsteps();
+    MatrixN get_gait();
+    Matrix3N get_goals();
+    Matrix3N get_vgoals();
+    Matrix3N get_agoals();
+
+private:
+    std::shared_ptr<Gait> gait_;  // Gait object to hold the gait information
+    FootstepPlanner footstepPlanner_;
+    FootTrajectoryGenerator fooTrajectoryGenerator_;
+    Matrix34 targetFootstep_;
+};
+
+#endif  // PLANNER_H_INCLUDED

include/quadruped-reactive-walking/QPWBC.hpp → include/qrw/QPWBC.hpp

 #ifndef QPWBC_H_INCLUDED
 #define QPWBC_H_INCLUDED
 
-#include "quadruped-reactive-walking/InvKin.hpp" // For pseudoinverse
+#include "qrw/InvKin.hpp" // For pseudoinverse
 #include <iostream>
 #include <fstream>
 #include <string>

include/qrw/Types.h

+#ifndef TYPES_H_INCLUDED
+#define TYPES_H_INCLUDED
+
+#include <Eigen/Eigen>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <string>
+#include <memory>
+
+using Vector2 = Eigen::Matrix<double, 2, 1>;
+using Vector3 = Eigen::Matrix<double, 3, 1>;
+using Vector4 = Eigen::Matrix<double, 4, 1>;
+using Vector5 = Eigen::Matrix<double, 5, 1>;
+using Vector6 = Eigen::Matrix<double, 6, 1>;
+using Vector7 = Eigen::Matrix<double, 7, 1>;
+using Vector11 = Eigen::Matrix<double, 11, 1>;
+using Vector12 = Eigen::Matrix<double, 12, 1>;
+using Vector19 = Eigen::Matrix<double, 1, 1>;
+using VectorN = Eigen::Matrix<double, Eigen::Dynamic, 1>;
+
+using Matrix3 = Eigen::Matrix<double, 3, 3>;
+using Matrix4 = Eigen::Matrix<double, 4, 4>;
+using Matrix34 = Eigen::Matrix<double, 3, 4>;
+using Matrix64 = Eigen::Matrix<double, 6, 4>;
+using Matrix3N = Eigen::Matrix<double, 3, Eigen::Dynamic>;
+using Matrix6N = Eigen::Matrix<double, 6, Eigen::Dynamic>;
+using MatrixN = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
+
+#endif  // TYPES_H_INCLUDED

include/quadruped-reactive-walking/InvKin.hpp

-#ifndef INVKIN_H_INCLUDED
-#define INVKIN_H_INCLUDED
-
-#include <Eigen/Core>
-#include <Eigen/Dense>
-#include <cmath>
-#include <fstream>
-#include <iostream>
-#include <string>
-#include "pinocchio/math/rpy.hpp"
-#include "pinocchio/spatial/explog.hpp"
-
-class InvKin {
-  /* Planner that outputs current and future locations of footsteps, the
-     reference trajectory of the base and the position, velocity, acceleration
-     commands for feet in swing phase based on the reference velocity given by
-     the user and the current position/velocity of the base */
-
-  /*private:
-   // Inputs of the constructor
-   double dt;       // Time step of the contact sequence (time step of the MPC)
-   double dt_tsid;  // Time step of TSID
-   double T_gait;   // Gait period
-   double T_mpc;    // MPC period (prediction horizon)
-   double h_ref;    // Reference height for the trunk
-   int k_mpc;       // Number of TSID iterations for one iteration of the MPC
-   bool on_solo8;   //  Whether we are working on solo8 or not
-
-   // Predefined quantities
-   double k_feedback = 0.03;  // Feedback gain for the feedback term of the
-   planner double g = 9.81;           // Value of the gravity acceleartion
-   double L = 0.155;          // Value of the maximum allowed deviation due to
-   leg length bool is_static = false;    // Flag for static gait
-
-   // Number of time steps in the prediction horizon
-   int n_steps; // T_mpc / time step of the MPC
-
-   // Feet index vector
-   std::vector<int> feet;
-   std::vector<double> t0s;
-   double t_remaining = 0.0;
-   double t_swing[4] = {0.0, 0.0, 0.0, 0.0};
-
-   // Constant sized matrices
-   Eigen::MatrixXd fsteps = Eigen::MatrixXd::Zero(N0_gait, 13);
-   Eigen::Matrix<double, 3, 4> shoulders = Eigen::Matrix<double, 3, 4>::Zero();
-   // Position of shoulders in local frame Eigen::Matrix<double, 19, 1> q_static
-   = Eigen::Matrix<double, 19, 1>::Zero(); Eigen::Matrix<double, 3, 1>
-   RPY_static = Eigen::Matrix<double, 3, 1>::Zero(); Eigen::Matrix<double, 1,
-   12> o_feet_contact = Eigen::Matrix<double, 1, 12>::Zero();  // Feet matrix in
-   world frame Eigen::Matrix<double, 3, 4> next_footstep = Eigen::Matrix<double,
-   3, 4>::Zero();  // To store the result of the compute_next_footstep function
-   Eigen::Matrix<double, 3, 3> R =
-       Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for
-   compute_footstep function Eigen::Matrix<double, 3, 3> R_1 =
-       Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for
-   compute_next_footstep function Eigen::Matrix<double, 3, 3> R_2 =
-   Eigen::Matrix<double, 3, 3>::Zero(); Eigen::Matrix<double, N0_gait, 1> dt_cum
-   = Eigen::Matrix<double, N0_gait, 1>::Zero(); Eigen::Matrix<double, N0_gait,
-   1> angle = Eigen::Matrix<double, N0_gait, 1>::Zero(); Eigen::Matrix<double,
-   N0_gait, 1> dx = Eigen::Matrix<double, N0_gait, 1>::Zero();
-   Eigen::Matrix<double, N0_gait, 1> dy = Eigen::Matrix<double, N0_gait,
-   1>::Zero(); Eigen::Matrix<double, 3, 1> q_tmp = Eigen::Matrix<double, 3,
-   1>::Zero(); Eigen::Matrix<double, 3, 1> q_dxdy = Eigen::Matrix<double, 3,
-   1>::Zero(); Eigen::Matrix<double, 3, 1> RPY = Eigen::Matrix<double, 3,
-   1>::Zero(); Eigen::Matrix<double, 3, 1> b_v_cur = Eigen::Matrix<double, 3,
-   1>::Zero(); Eigen::Matrix<double, 6, 1> b_v_ref = Eigen::Matrix<double, 6,
-   1>::Zero(); Eigen::Matrix<double, 3, 1> cross = Eigen::Matrix<double, 3,
-   1>::Zero(); Eigen::Matrix<double, 6, 1> vref_in = Eigen::Matrix<double, 6,
-   1>::Zero();
-
-   Eigen::Matrix<double, N0_gait, 5> gait_p = Eigen::Matrix<double, N0_gait,
-   5>::Zero();  // Past gait Eigen::MatrixXd gait_f =
-   Eigen::MatrixXd::Zero(N0_gait, 5);                                // Current
-   and future gait Eigen::Matrix<double, N0_gait, 5> gait_f_des =
-   Eigen::Matrix<double, N0_gait, 5>::Zero();  // Future desired gait
-
-   // Time interval vector
-   Eigen::Matrix<double, 1, Eigen::Dynamic> dt_vector;
-
-   // Reference trajectory matrix of size 12 by (1 + N)  with the current state
-   of
-   // the robot in column 0 and the N steps of the prediction horizon in the
-   others Eigen::MatrixXd xref;
-
-   // Foot trajectory generator
-   double max_height_feet = 0.05;  // * (1000/312.5);  // height * correction
-   coefficient double t_lock_before_touchdown = 0.07; std::vector<TrajGen>
-   myTrajGen;
-
-   // Variables for foot trajectory generator
-   int i_end_gait = 0;
-   Eigen::Matrix<double, 1, 4> t_stance =
-       Eigen::Matrix<double, 1, 4>::Zero();  // Total duration of current stance
-   phase for each foot Eigen::Matrix<double, 2, 4> footsteps_target =
-   Eigen::Matrix<double, 2, 4>::Zero(); Eigen::MatrixXd goals =
-   Eigen::MatrixXd::Zero(3, 4);   // Store 3D target position for feet
-   Eigen::MatrixXd vgoals = Eigen::MatrixXd::Zero(3, 4);  // Store 3D target
-   velocity for feet Eigen::MatrixXd agoals = Eigen::MatrixXd::Zero(3, 4);  //
-   Store 3D target acceleration for feet Eigen::Matrix<double, 6, 4> mgoals =
-       Eigen::Matrix<double, 6, 4>::Zero();  // Storage variable for the
-   trajectory generator
-
-   Eigen::Matrix<double, 11, 4> res_gen = Eigen::Matrix<double, 11, 4>::Zero();
-   // Result of the generator
-   */
-
- private:
-  // Inputs of the constructor
-  double dt;  // Time step of the contact sequence (time step of the MPC)
-
-  // Matrices initialisation
-  Eigen::Matrix<double, 4, 3> feet_position_ref = Eigen::Matrix<double, 4, 3>::Zero();
-  Eigen::Matrix<double, 4, 3> feet_velocity_ref = Eigen::Matrix<double, 4, 3>::Zero();
-  Eigen::Matrix<double, 4, 3> feet_acceleration_ref = Eigen::Matrix<double, 4, 3>::Zero();
-  Eigen::Matrix<double, 1, 4> flag_in_contact = Eigen::Matrix<double, 1, 4>::Zero();
-  Eigen::Matrix<double, 3, 3> base_orientation_ref = Eigen::Matrix<double, 3, 3>::Zero();
-  Eigen::Matrix<double, 1, 3> base_angularvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
-  Eigen::Matrix<double, 1, 3> base_angularacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
-  Eigen::Matrix<double, 1, 3> base_position_ref = Eigen::Matrix<double, 1, 3>::Zero();
-  Eigen::Matrix<double, 1, 3> base_linearvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
-  Eigen::Matrix<double, 1, 3> base_linearacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
-  Eigen::Matrix<double, 6, 1> x_ref = Eigen::Matrix<double, 6, 1>::Zero();
-  Eigen::Matrix<double, 6, 1> x = Eigen::Matrix<double, 6, 1>::Zero();
-  Eigen::Matrix<double, 6, 1> dx_ref = Eigen::Matrix<double, 6, 1>::Zero();
-  Eigen::Matrix<double, 6, 1> dx = Eigen::Matrix<double, 6, 1>::Zero();
-  Eigen::Matrix<double, 18, 18> J = Eigen::Matrix<double, 18, 18>::Zero();
-  Eigen::Matrix<double, 18, 18> invJ = Eigen::Matrix<double, 18, 18>::Zero();
-  Eigen::Matrix<double, 1, 18> acc = Eigen::Matrix<double, 1, 18>::Zero();
-  Eigen::Matrix<double, 1, 18> x_err = Eigen::Matrix<double, 1, 18>::Zero();
-  Eigen::Matrix<double, 1, 18> dx_r = Eigen::Matrix<double, 1, 18>::Zero();
-
-  Eigen::Matrix<double, 4, 3> pfeet_err = Eigen::Matrix<double, 4, 3>::Zero();
-  Eigen::Matrix<double, 4, 3> vfeet_ref = Eigen::Matrix<double, 4, 3>::Zero();
-  Eigen::Matrix<double, 4, 3> afeet = Eigen::Matrix<double, 4, 3>::Zero();
-  Eigen::Matrix<double, 1, 3> e_basispos = Eigen::Matrix<double, 1, 3>::Zero();
-  Eigen::Matrix<double, 1, 3> abasis = Eigen::Matrix<double, 1, 3>::Zero();
-  Eigen::Matrix<double, 1, 3> e_basisrot = Eigen::Matrix<double, 1, 3>::Zero();
-  Eigen::Matrix<double, 1, 3> awbasis = Eigen::Matrix<double, 1, 3>::Zero();
-
-  Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(18, 1);
-  Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(18, 1);
-  Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(18, 1);
-
-  // Gains
-  double Kp_base_orientation = 100.0;
-  double Kd_base_orientation = 2.0 * std::sqrt(Kp_base_orientation);
-  
-  double Kp_base_position = 100.0;
-  double Kd_base_position = 2.0 * std::sqrt(Kp_base_position);
-
-  double Kp_flyingfeet = 1000.0;
-  double Kd_flyingfeet = 5.0 * std::sqrt(Kp_flyingfeet);
-
- public:
-  InvKin();
-  InvKin(double dt_in);
-  
-  Eigen::Matrix<double, 1, 3> cross3(Eigen::Matrix<double, 1, 3> left, Eigen::Matrix<double, 1, 3> right);
-
-  Eigen::MatrixXd refreshAndCompute(const Eigen::MatrixXd &x_cmd, const Eigen::MatrixXd &contacts,
-                                    const Eigen::MatrixXd &goals, const Eigen::MatrixXd &vgoals, const Eigen::MatrixXd &agoals,
-                                    const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
-                                    const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb,
-                                    const Eigen::MatrixXd &ab, const Eigen::MatrixXd &Jb);
-  Eigen::MatrixXd computeInvKin(const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
-                                const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb, const Eigen::MatrixXd &ab,
-                                const Eigen::MatrixXd &Jb);
-  Eigen::MatrixXd get_q_step();
-  Eigen::MatrixXd get_dq_cmd();
-  /*void Print();
-
-  int create_walk();
-  int create_trot();
-  int create_gait_f();
-  int roll(int k);
-  int compute_footsteps(Eigen::MatrixXd q_cur, Eigen::MatrixXd v_cur,
-  Eigen::MatrixXd v_ref); double get_stance_swing_duration(int i, int j, double
-  value); int compute_next_footstep(int i, int j); int
-  getRefStates(Eigen::MatrixXd q, Eigen::MatrixXd v, Eigen::MatrixXd vref,
-  double z_average); int update_target_footsteps(); int
-  update_trajectory_generator(int k, double h_estim); int run_planner(int k,
-  const Eigen::MatrixXd &q, const Eigen::MatrixXd &v, const Eigen::MatrixXd
-  &b_vref, double h_estim, double z_average);*/
-
-  // Accessors (to retrieve C data from Python)
-  /*Eigen::MatrixXd get_xref();
-  Eigen::MatrixXd get_fsteps();
-  Eigen::MatrixXd get_gait();
-  Eigen::MatrixXd get_goals();
-  Eigen::MatrixXd get_vgoals();
-  Eigen::MatrixXd get_agoals();*/
-};
-
-template<typename _Matrix_Type_>
-_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
-{
-	Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
-	double tolerance = epsilon * static_cast<double>(std::max(a.cols(), a.rows())) *svd.singularValues().array().abs()(0);
-	return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
-}
-
-
-#endif  // INVKIN_H_INCLUDED

include/quadruped-reactive-walking/Planner.hpp

-#ifndef PLANNER_H_INCLUDED
-#define PLANNER_H_INCLUDED
-
-#include <iostream>
-#include <fstream>
-#include <string>
-#include <cmath>
-#include <Eigen/Core>
-#include <Eigen/Dense>
-#include "pinocchio/math/rpy.hpp"
-
-#define N0_gait 20
-// Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is always at
-// least one empty line at the end of the gait matrix
-
-typedef Eigen::MatrixXd matXd;
-
-class TrajGen {
-  /* Class that generates a reference trajectory in position, velocity and acceleration that feet it swing phase
-     should follow */
-
- private:
-  double lastCoeffs_x[6];  // Coefficients for the X component
-  double lastCoeffs_y[6];  // Coefficients for the Y component
-  double h = 0.05;  // Apex height of the swinging trajectory
-  double time_adaptative_disabled = 0.2;  // Target lock before the touchdown
-  double x1 = 0.0;  // Target for the X component
-  double y1 = 0.0;  // Target for the Y component
-  Eigen::Matrix<double, 11, 1> result = Eigen::Matrix<double, 11, 1>::Zero();  // Output of the generator
-
-  // Coefficients
-  double Ax5 = 0.0, Ax4 = 0.0, Ax3 = 0.0, Ax2 = 0.0, Ax1 = 0.0, Ax0 = 0.0, Ay5 = 0.0, Ay4 = 0.0, Ay3 = 0.0, Ay2 = 0.0,
-         Ay1 = 0.0, Ay0 = 0.0, Az6 = 0.0, Az5 = 0.0, Az4 = 0.0, Az3 = 0.0;
-
- public:
-  TrajGen();  // Empty constructor
-  TrajGen(double h_in, double t_lock_in, double x_in, double y_in);  // Default constructor
-  Eigen::Matrix<double, 11, 1> get_next_foot(double x0, double dx0, double ddx0, double y0, double dy0, double ddy0,
-                                             double x1_in, double y1_in, double t0, double t1, double dt);
-};
-
-class Planner {
-  /* Planner that outputs current and future locations of footsteps, the reference trajectory of the base and
-     the position, velocity, acceleration commands for feet in swing phase based on the reference velocity given by
-     the user and the current position/velocity of the base */
-    
- private:
-  // Inputs of the constructor
-  double dt;       // Time step of the contact sequence (time step of the MPC)
-  double dt_tsid;  // Time step of TSID
-  double T_gait;   // Gait period
-  double T_mpc;    // MPC period (prediction horizon)
-  double h_ref;    // Reference height for the trunk
-  int k_mpc;       // Number of TSID iterations for one iteration of the MPC
-  bool on_solo8;   //  Whether we are working on solo8 or not
-
-  // Predefined quantities
-  double k_feedback = 0.03;  // Feedback gain for the feedback term of the planner
-  double g = 9.81;           // Value of the gravity acceleartion
-  double L = 0.155;          // Value of the maximum allowed deviation due to leg length
-  bool is_static = false;    // Flag for static gait
-
-  // Number of time steps in the prediction horizon
-  int n_steps; // T_mpc / time step of the MPC
-
-  // Feet index vector
-  std::vector<int> feet;
-  std::vector<double> t0s;
-  double t_remaining = 0.0;
-  double t_swing[4] = {0.0, 0.0, 0.0, 0.0};
-
-  // Constant sized matrices
-  Eigen::MatrixXd fsteps = Eigen::MatrixXd::Zero(N0_gait, 13);
-  Eigen::Matrix<double, 3, 4> shoulders = Eigen::Matrix<double, 3, 4>::Zero();  // Position of shoulders in local frame
-  Eigen::Matrix<double, 19, 1> q_static = Eigen::Matrix<double, 19, 1>::Zero();
-  Eigen::Matrix<double, 3, 1> RPY_static = Eigen::Matrix<double, 3, 1>::Zero();
-  Eigen::Matrix<double, 1, 12> o_feet_contact = Eigen::Matrix<double, 1, 12>::Zero();  // Feet matrix in world frame
-  Eigen::Matrix<double, 3, 4> next_footstep =
-      Eigen::Matrix<double, 3, 4>::Zero();  // To store the result of the compute_next_footstep function
-  Eigen::Matrix<double, 3, 3> R =
-      Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for compute_footstep function
-  Eigen::Matrix<double, 3, 3> R_1 =
-      Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for compute_next_footstep function
-  Eigen::Matrix<double, 3, 3> R_2 = Eigen::Matrix<double, 3, 3>::Zero();
-  Eigen::Matrix<double, N0_gait, 1> dt_cum = Eigen::Matrix<double, N0_gait, 1>::Zero();
-  Eigen::Matrix<double, N0_gait, 1> angle = Eigen::Matrix<double, N0_gait, 1>::Zero();
-  Eigen::Matrix<double, N0_gait, 1> dx = Eigen::Matrix<double, N0_gait, 1>::Zero();
-  Eigen::Matrix<double, N0_gait, 1> dy = Eigen::Matrix<double, N0_gait, 1>::Zero();
-  Eigen::Matrix<double, 3, 1> q_tmp = Eigen::Matrix<double, 3, 1>::Zero();
-  Eigen::Matrix<double, 3, 1> q_dxdy = Eigen::Matrix<double, 3, 1>::Zero();
-  Eigen::Matrix<double, 3, 1> RPY = Eigen::Matrix<double, 3, 1>::Zero();
-  Eigen::Matrix<double, 3, 1> b_v_cur = Eigen::Matrix<double, 3, 1>::Zero();
-  Eigen::Matrix<double, 6, 1> b_v_ref = Eigen::Matrix<double, 6, 1>::Zero();
-  Eigen::Matrix<double, 3, 1> cross = Eigen::Matrix<double, 3, 1>::Zero();
-  Eigen::Matrix<double, 6, 1> vref_in = Eigen::Matrix<double, 6, 1>::Zero();
-
-  Eigen::Matrix<double, N0_gait, 5> gait_p = Eigen::Matrix<double, N0_gait, 5>::Zero();  // Past gait
-  Eigen::MatrixXd gait_f = Eigen::MatrixXd::Zero(N0_gait, 5);                                // Current and future gait
-  Eigen::Matrix<double, N0_gait, 5> gait_f_des = Eigen::Matrix<double, N0_gait, 5>::Zero();  // Future desired gait
-
-  // Time interval vector
-  Eigen::Matrix<double, 1, Eigen::Dynamic> dt_vector;
-
-  // Reference trajectory matrix of size 12 by (1 + N)  with the current state of
-  // the robot in column 0 and the N steps of the prediction horizon in the others
-  Eigen::MatrixXd xref;
-
-  // Foot trajectory generator
-  double max_height_feet = 0.05;  // * (1000/312.5);  // height * correction coefficient
-  double t_lock_before_touchdown = 0.07;
-  std::vector<TrajGen> myTrajGen;
-
-  // Variables for foot trajectory generator
-  int i_end_gait = 0;
-  Eigen::Matrix<double, 1, 4> t_stance =
-      Eigen::Matrix<double, 1, 4>::Zero();  // Total duration of current stance phase for each foot
-  Eigen::Matrix<double, 2, 4> footsteps_target = Eigen::Matrix<double, 2, 4>::Zero();
-  Eigen::MatrixXd goals = Eigen::MatrixXd::Zero(3, 4);   // Store 3D target position for feet
-  Eigen::MatrixXd vgoals = Eigen::MatrixXd::Zero(3, 4);  // Store 3D target velocity for feet
-  Eigen::MatrixXd agoals = Eigen::MatrixXd::Zero(3, 4);  // Store 3D target acceleration for feet
-  Eigen::Matrix<double, 6, 4> mgoals =
-      Eigen::Matrix<double, 6, 4>::Zero();  // Storage variable for the trajectory generator
-
-  Eigen::Matrix<double, 11, 4> res_gen = Eigen::Matrix<double, 11, 4>::Zero(); // Result of the generator
-
- public:
-  Planner();
-  Planner(double dt_in, double dt_tsid_in, double T_gait_in, double T_mpc_in, int k_mpc_in, bool on_solo8_in,
-          double h_ref_in, const Eigen::MatrixXd &fsteps_in);
-
-  void Print();
-
-  int create_walk();
-  int create_trot();
-  int create_pacing();
-  int create_bounding();
-  int create_static();
-  int create_gait_f();
-  int roll(int k);
-  int handle_joystick(int code, const Eigen::MatrixXd &q);
-  int compute_footsteps(Eigen::MatrixXd q_cur, Eigen::MatrixXd v_cur, Eigen::MatrixXd v_ref);
-  double get_stance_swing_duration(int i, int j, double value);
-  int compute_next_footstep(int i, int j);
-  int getRefStates(Eigen::MatrixXd q, Eigen::MatrixXd v, Eigen::MatrixXd vref, double z_average);
-  int update_target_footsteps();
-  int update_trajectory_generator(int k, double h_estim);
-  int run_planner(int k, const Eigen::MatrixXd &q, const Eigen::MatrixXd &v, const Eigen::MatrixXd &b_vref,
-                  double h_estim, double z_average, int joystick_code);
-
-  // Accessors (to retrieve C data from Python)
-  Eigen::MatrixXd get_xref();
-  Eigen::MatrixXd get_fsteps();
-  Eigen::MatrixXd get_gait();
-  Eigen::MatrixXd get_goals();
-  Eigen::MatrixXd get_vgoals();
-  Eigen::MatrixXd get_agoals();
-};
-
-#endif  // PLANNER_H_INCLUDED

python/gepadd.cpp

-#include "quadruped-reactive-walking/gepadd.hpp"
-#include "quadruped-reactive-walking/MPC.hpp"
-#include "quadruped-reactive-walking/Planner.hpp"
-#include "quadruped-reactive-walking/InvKin.hpp"
-#include "quadruped-reactive-walking/QPWBC.hpp"
+#include "qrw/gepadd.hpp"
+#include "qrw/InvKin.hpp"
+#include "qrw/MPC.hpp"
+#include "qrw/Planner.hpp"
+#include "qrw/QPWBC.hpp"
 
-#include <eigenpy/eigenpy.hpp>
 #include <boost/python.hpp>
+#include <eigenpy/eigenpy.hpp>
 
 namespace bp = boost::python;
 
 template <typename MPC>
-struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC> > {
-  template <class PyClassMPC>
-  void visit(PyClassMPC& cl) const {
-    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
-        .def(bp::init<double, int, double>(bp::args("dt_in", "n_steps_in", "T_gait_in"),
-                                           "Constructor with parameters."))
-
-        // Run MPC from Python
-        .def("run", &MPC::run, bp::args("num_iter", "xref_in", "fsteps_in"), "Run MPC from Python.\n")
-        .def("get_latest_result", &MPC::get_latest_result,
-             "Get latest result (predicted trajectory + forces to apply).\n")
-        .def("get_gait", &MPC::get_gait, "Get gait matrix.\n")
-        .def("get_Sgait", &MPC::get_Sgait, "Get S_gait matrix.\n");
-  }
-
-  static void expose() {
-    bp::class_<MPC>("MPC", bp::no_init).def(MPCPythonVisitor<MPC>());
-
-    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
-  }
+struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC>>
+{
+    template <class PyClassMPC>
+    void visit(PyClassMPC& cl) const
+    {
+        cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+            .def(bp::init<double, int, double>(bp::args("dt_in", "n_steps_in", "T_gait_in"),
+                                               "Constructor with parameters."))
+
+            // Run MPC from Python
+            .def("run", &MPC::run, bp::args("num_iter", "xref_in", "fsteps_in"), "Run MPC from Python.\n")
+            .def("get_latest_result", &MPC::get_latest_result,
+                 "Get latest result (predicted trajectory  forces to apply).\n")
+            .def("get_gait", &MPC::get_gait, "Get gait matrix.\n")
+            .def("get_Sgait", &MPC::get_Sgait, "Get S_gait matrix.\n");
+    }
+
+    static void expose()
+    {
+        bp::class_<MPC>("MPC", bp::no_init).def(MPCPythonVisitor<MPC>());
+
+        ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+    }
 };
 
 void exposeMPC() { MPCPythonVisitor<MPC>::expose(); }
 
+
 template <typename Planner>
-struct PlannerPythonVisitor : public bp::def_visitor<PlannerPythonVisitor<Planner> > {
-  template <class PyClassPlanner>
-  void visit(PyClassPlanner& cl) const {
-    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
-        .def(bp::init<double, double, double, double, int, bool, double, const Eigen::MatrixXd&>(
-            bp::args("dt_in", "dt_tsid_in", "T_gait_in", "T_mpc_in", "k_mpc_in", "on_solo8_in", "h_ref_in",
-                     "fsteps_in"),
-            "Constructor with parameters."))
-
-        .def("get_xref", &Planner::get_xref, "Get xref matrix.\n")
-        .def("get_fsteps", &Planner::get_fsteps, "Get fsteps matrix.\n")
-        .def("get_gait", &Planner::get_gait, "Get gait matrix.\n")
-        .def("get_goals", &Planner::get_goals, "Get position goals matrix.\n")
-        .def("get_vgoals", &Planner::get_vgoals, "Get velocity goals matrix.\n")
-        .def("get_agoals", &Planner::get_agoals, "Get acceleration goals matrix.\n")
-        //.add_property("xref", &Planner::get_xref)
-
-        // Run Planner from Python
-        .def("run_planner", &Planner::run_planner, bp::args("k", "q", "v", "b_vref", "h_estim", "z_average", "joystick_code"),
-             "Run Planner from Python.\n");
-  }
-
-  static void expose() {
-    bp::class_<Planner>("Planner", bp::no_init).def(PlannerPythonVisitor<Planner>());
-
-    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
-  }
+struct PlannerPythonVisitor : public bp::def_visitor<PlannerPythonVisitor<Planner>>
+{
+    template <class PyClassPlanner>
+    void visit(PyClassPlanner& cl) const
+    {
+        cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+            .def(bp::init<double, double, double, double, int, double, const MatrixN&, const MatrixN&>(
+                bp::args("dt_in", "dt_tsid_in", "T_gait_in", "T_mpc_in", "k_mpc_in", "h_ref_in",
+                         "fsteps_in", "shoulders positions"),
+                "Constructor with parameters."))
+
+            .def("get_xref", &Planner::get_xref, "Get xref matrix.\n")
+            .def("get_fsteps", &Planner::get_fsteps, "Get fsteps matrix.\n")
+            .def("get_gait", &Planner::get_gait, "Get gait matrix.\n")
+            .def("get_goals", &Planner::get_goals, "Get position goals matrix.\n")
+            .def("get_vgoals", &Planner::get_vgoals, "Get velocity goals matrix.\n")
+            .def("get_agoals", &Planner::get_agoals, "Get acceleration goals matrix.\n")
+
+            // Run Planner from Python
+            .def("run_planner", &Planner::run_planner, bp::args("k", "q", "v", "b_vref", "z_average", "joystick_code"),
+                 "Run Planner from Python.\n");
+    }
+
+    static void expose()
+    {
+        bp::class_<Planner>("Planner", bp::no_init).def(PlannerPythonVisitor<Planner>());
+
+        ENABLE_SPECIFIC_MATRIX_TYPE(MatrixN);
+    }
 };
-
 void exposePlanner() { PlannerPythonVisitor<Planner>::expose(); }
 
+
 template <typename InvKin>
-struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin> > {
-  template <class PyClassInvKin>
-  void visit(PyClassInvKin& cl) const {
-    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
-        .def(bp::init<double>(bp::args("dt_in"), "Constructor with parameters."))
-
-        .def("get_q_step", &InvKin::get_q_step, "Get velocity goals matrix.\n")
-        .def("get_dq_cmd", &InvKin::get_dq_cmd, "Get acceleration goals matrix.\n")
-
-        // Run InvKin from Python
-        .def("refreshAndCompute", &InvKin::refreshAndCompute, 
-             bp::args("x_cmd", "contacts", "goals", "vgoals", "agoals", "posf", "vf", "wf", "af", "Jf",
-                      "posb", "rotb", "vb", "ab", "Jb"),
-             "Run InvKin from Python.\n");
-  }
-
-  static void expose() {
-    bp::class_<InvKin>("InvKin", bp::no_init).def(InvKinPythonVisitor<InvKin>());
-
-    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
-  }
+struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin>>
+{
+    template <class PyClassInvKin>
+    void visit(PyClassInvKin& cl) const
+    {
+        cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+            .def(bp::init<double>(bp::args("dt_in"), "Constructor with parameters."))
+
+            .def("get_q_step", &InvKin::get_q_step, "Get velocity goals matrix.\n")
+            .def("get_dq_cmd", &InvKin::get_dq_cmd, "Get acceleration goals matrix.\n")
+
+            // Run InvKin from Python
+            .def("refreshAndCompute", &InvKin::refreshAndCompute,
+                 bp::args("x_cmd", "contacts", "goals", "vgoals", "agoals", "posf", "vf", "wf", "af", "Jf",
+                          "posb", "rotb", "vb", "ab", "Jb"),
+                 "Run InvKin from Python.\n");
+    }
+
+    static void expose()
+    {
+        bp::class_<InvKin>("InvKin", bp::no_init).def(InvKinPythonVisitor<InvKin>());
+
+        ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+    }
 };
 
 void exposeInvKin() { InvKinPythonVisitor<InvKin>::expose(); }
 
 template <typename QPWBC>
-struct QPWBCPythonVisitor : public bp::def_visitor<QPWBCPythonVisitor<QPWBC> > {
-  template <class PyClassQPWBC>
-  void visit(PyClassQPWBC& cl) const {
-    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
-
-        .def("get_f_res", &QPWBC::get_f_res, "Get velocity goals matrix.\n")
-        .def("get_ddq_res", &QPWBC::get_ddq_res, "Get acceleration goals matrix.\n")
-        .def("get_H", &QPWBC::get_H, "Get H weight matrix.\n")
-
-        // Run QPWBC from Python
-        .def("run", &QPWBC::run, bp::args("M", "Jc", "f_cmd", "RNEA", "k_contacts"), "Run QPWBC from Python.\n");
-  }
-
-  static void expose() {
-    bp::class_<QPWBC>("QPWBC", bp::no_init).def(QPWBCPythonVisitor<QPWBC>());
-
-    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
-  }
+struct QPWBCPythonVisitor : public bp::def_visitor<QPWBCPythonVisitor<QPWBC>>
+{
+    template <class PyClassQPWBC>
+    void visit(PyClassQPWBC& cl) const
+    {
+        cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+            .def("get_f_res", &QPWBC::get_f_res, "Get velocity goals matrix.\n")
+            .def("get_ddq_res", &QPWBC::get_ddq_res, "Get acceleration goals matrix.\n")
+            .def("get_H", &QPWBC::get_H, "Get H weight matrix.\n")
+
+            // Run QPWBC from Python
+            .def("run", &QPWBC::run, bp::args("M", "Jc", "f_cmd", "RNEA", "k_contacts"), "Run QPWBC from Python.\n");
+    }
+
+    static void expose()
+    {
+        bp::class_<QPWBC>("QPWBC", bp::no_init).def(QPWBCPythonVisitor<QPWBC>());
+
+        ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+    }
 };
 
 void exposeQPWBC() { QPWBCPythonVisitor<QPWBC>::expose(); }
 
-BOOST_PYTHON_MODULE(libquadruped_reactive_walking) {
-  boost::python::def("add", gepetto::example::add);
-  boost::python::def("sub", gepetto::example::sub);
+BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
+{
+    boost::python::def("add", gepetto::example::add);
+    boost::python::def("sub", gepetto::example::sub);
 
-  eigenpy::enableEigenPy();
+    eigenpy::enableEigenPy();
 
-  exposeMPC();
-  exposePlanner();
-  exposeInvKin();
-  exposeQPWBC();
+    exposeMPC();
+    exposePlanner();
+    exposeInvKin();
+    exposeQPWBC();
 }
\ No newline at end of file

scripts/Controller.py

@@ -198,7 +198,7 @@ class Controller:
                                                    self.q, self.v_estim * self.myController.dt)
                 self.yaw_estim = (utils_mpc.quaternionToRPY(self.q_estim[3:7, 0]))[2, 0]
             else:
-                self.planner.q_static[:, 0] = pin.integrate(self.solo.model,
+                self.planner.q_static[:] = pin.integrate(self.solo.model,
                                                             self.planner.q_static, self.v_estim * self.myController.dt)
                 self.planner.RPY_static[:, 0:1] = utils_mpc.quaternionToRPY(self.planner.q_static[3:7, 0])
         else:

scripts/LoggerControl.py

@@ -118,7 +118,7 @@ class LoggerControl():
         self.loop_o_v[self.i] = loop.v_estim[:, 0]
 
         # Logging from the planner
-        self.planner_q_static[self.i] = planner.q_static[:, 0]
+        self.planner_q_static[self.i] = planner.q_static[:]
         self.planner_RPY_static[self.i] = planner.RPY_static[:, 0]
         self.planner_xref[self.i] = planner.xref
         self.planner_fsteps[self.i] = planner.fsteps