diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index cb5eea4de3165ab04481fdc0b4e3b841c5e8eb8d..219d0b85c2c742ad10f7f8bba2a1b36f72a6ba00 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -1,8 +1,8 @@
///////////////////////////////////////////////////////////////////////////////////////////////////
///
-/// \brief This is the header for Estimator class
+/// \brief This is the header for Estimator and ComplementaryFilter classes
///
-/// \details This class estimates the state of the robot based on sensor measurements
+/// \details These classes estimate the state of the robot based on sensor measurements
///
//////////////////////////////////////////////////////////////////////////////////////////////////
@@ -21,267 +21,260 @@
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <deque>
-class ComplementaryFilter
-{
-public:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Constructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ComplementaryFilter();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Destructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ~ComplementaryFilter() {} // Empty destructor
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Initialize
- ///
- /// \param[in] dt time step of the complementary filter
- /// \param[in] HP_x initial value for the high pass filter
- /// \param[in] LP_x initial value for the low pass filter
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void initialize(double dt, Vector3 HP_x, Vector3 LP_x);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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 getHpX() { return HP_x_; } ///< Get the high-passed internal quantity
- Vector3 getLpX() { return LP_x_; } ///< Get the low-passed internal quantity
- Vector3 getAlpha() {return alpha_; } ///< Get the alpha coefficient of the filter
- Vector3 getFiltX() { return filt_x_; } ///< Get the filtered output
-
-private:
-
- double dt_;
- Vector3 x_, dx_, HP_x_, LP_x_, alpha_, filt_x_;
-
+class ComplementaryFilter {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ComplementaryFilter();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~ComplementaryFilter() {} // Empty destructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize
+ ///
+ /// \param[in] dt Time step of the complementary filter
+ /// \param[in] HP_x Initial value for the high pass filter
+ /// \param[in] LP_x Initial value for the low pass filter
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(double dt, Vector3 HP_x, Vector3 LP_x);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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 getHpX() { return HP_x_; } // Get the high-passed internal quantity
+ Vector3 getLpX() { return LP_x_; } // Get the low-passed internal quantity
+ Vector3 getAlpha() { return alpha_; } // Get the alpha coefficient of the filter
+ Vector3 getFiltX() { return filt_x_; } // Get the filtered output
+
+ private:
+ double dt_;
+ Vector3 x_, dx_, HP_x_, LP_x_, alpha_, filt_x_;
};
-
-class Estimator
-{
-public:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Constructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- Estimator();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Initialize with given data
- ///
- /// \param[in] params Object that stores parameters
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void initialize(Params& params);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Destructor.
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ~Estimator() {} // Empty destructor
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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 get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity, Vector3 const& baseOrientation);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Retrieve and update position and velocity of the 12 actuators
- ///
- /// \param[in] q_mes position of the 12 actuators
- /// \param[in] v_mes velocity of the 12 actuators
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void get_data_joints(Vector12 const& q_mes, Vector12 const& v_mes);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Estimate base position and velocity using Forward Kinematics
- ///
- /// \param[in] feet_status contact status of the four feet
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void get_data_FK(Eigen::Matrix<double, 1, 4> const& feet_status);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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 get_xyz_feet(Eigen::Matrix<double, 1, 4> const& feet_status, Matrix34 const& goals);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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 point
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- Vector3 BaseVelocityFromKinAndIMU(int contactFrameId);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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] dummyPos position of the robot in PyBullet simulator (only for simulation)
- /// \param[in] b_baseVel velocity of the robot in PyBullet simulator (only for simulation)
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void run_filter(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& dummyPos, VectorN const& b_baseVel);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Security check to verify that measured positions, velocities and required torques
- /// are not above defined tresholds
- ///
- /// \param[in] tau_ff feedforward torques outputted by the whole body control for the PD+
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- int security_check(VectorN const& tau_ff);
-
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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
- /// \param[in] gait Gait object
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void updateState(VectorN const& joystick_v_ref, Gait& gait);
-
- VectorN getQFilt() { return q_filt_dyn_; }
- VectorN getVFilt() { return v_filt_dyn_; }
- VectorN getVSecu() { return v_secu_dyn_; }
- Vector3 getVFiltBis() { return v_filt_bis_; }
- Vector3 getRPY() { return IMU_RPY_; }
- MatrixN getFeetStatus() { return feet_status_;}
- MatrixN getFeetGoals() { return feet_goals_; }
- Vector3 getFKLinVel() { return FK_lin_vel_; }
- Vector3 getFKXYZ() { return FK_xyz_; }
- Vector3 getXYZMeanFeet() { return xyz_mean_feet_; }
- Vector3 getFiltLinVel() { return b_filt_lin_vel_; }
-
- Vector3 getFilterVelX() { return filter_xyz_vel_.getX(); }
- Vector3 getFilterVelDX() { return filter_xyz_vel_.getDX(); }
- Vector3 getFilterVelAlpha() { return filter_xyz_vel_.getAlpha(); }
- Vector3 getFilterVelFiltX() { return filter_xyz_vel_.getFiltX(); }
-
- Vector3 getFilterPosX() { return filter_xyz_pos_.getX(); }
- Vector3 getFilterPosDX() { return filter_xyz_pos_.getDX(); }
- Vector3 getFilterPosAlpha() { return filter_xyz_pos_.getAlpha(); }
- Vector3 getFilterPosFiltX() { return filter_xyz_pos_.getFiltX(); }
-
- VectorN getQUpdated() { return q_up_; }
- VectorN getVRef() { return v_ref_; }
- VectorN getHV() { return h_v_; }
- Vector3 getHVBis() { return h_v_bis_; }
- Matrix3 getoRh() { return oRh_; }
- Vector3 getoTh() { return oTh_; }
- double getYawEstim() { return yaw_estim_; }
-
-private:
-
- ComplementaryFilter filter_xyz_pos_; // Complementary filter for base position
- ComplementaryFilter filter_xyz_vel_; // Complementary filter for base velocity
-
- double dt_wbc; // Time step of the estimator
- double alpha_v_; // Low pass coefficient for the outputted filtered velocity
- double alpha_secu_; // Low pass coefficient for the outputted filtered velocity for security check
- double offset_yaw_IMU_; // Yaw orientation of the IMU at startup
- bool perfect_estimator; // Enable perfect estimator (directly from the PyBullet simulation)
- int N_SIMULATION; // Number of loops before the control ends
- int k_log_; // Number of time the estimator has been called
-
- Vector3 IMU_lin_acc_; // Linear acceleration of the IMU (gravity compensated)
- Vector3 IMU_ang_vel_; // Angular velocity of the IMU
- Vector3 IMU_RPY_; // Roll Pitch Yaw orientation of the IMU
- pinocchio::SE3::Quaternion IMU_ang_pos_; // Quaternion orientation of the IMU
- Vector12 actuators_pos_; // Measured positions of actuators
- Vector12 actuators_vel_; // Measured velocities of actuators
-
- Vector19 q_FK_; // Configuration vector for Forward Kinematics
- Vector18 v_FK_; // Velocity vector for Forward Kinematics
- MatrixN feet_status_; // Contact status of the four feet
- MatrixN feet_goals_; // Target positions of the four feet
- Vector3 FK_lin_vel_; // Base linear velocity estimated by Forward Kinematics
- Vector3 FK_xyz_; // Base position estimated by Forward Kinematics
- Vector3 b_filt_lin_vel_; // Filtered estimated velocity at center base (base frame)
-
- Vector3 xyz_mean_feet_; // Barycenter of feet in contact
-
- Eigen::Matrix<double, 1, 4> k_since_contact_; // Number of loops during which each foot has been in contact
- int feet_indexes_[4] = {10, 18, 26, 34}; // Frame indexes of the four feet
-
- pinocchio::Model model_, model_for_xyz_; // Pinocchio models for frame computations and forward kinematics
- pinocchio::Data data_, data_for_xyz_; // Pinocchio datas for frame computations and forward kinematics
-
- Vector19 q_filt_; // Filtered output configuration
- Vector18 v_filt_; // Filtered output velocity
- Vector12 v_secu_; // Filtered output velocity for security check
-
- VectorN q_filt_dyn_; // Dynamic size version of q_filt_
- VectorN v_filt_dyn_; // Dynamic size version of v_filt_
- VectorN v_secu_dyn_; // Dynamic size version of v_secu_
-
- pinocchio::SE3 _1Mi_; // Transform between the base frame and the IMU frame
-
- Vector12 q_security_; // Position limits for the actuators above which safety controller is triggered
-
- // For updateState function
- VectorN q_up_; // Configuration vector
- VectorN v_ref_; // Reference velocity vector
- VectorN h_v_; // Velocity vector in horizontal frame
- Matrix3 oRh_; // Rotation between horizontal and world frame
- Vector3 oTh_; // Translation between horizontal and world frame
- double yaw_estim_; // Yaw angle in perfect world
-
- int N_queue_;
- Vector3 v_filt_bis_;
- Vector3 h_v_bis_;
- std::deque<double> vx_queue_, vy_queue_, vz_queue_;
-
+class Estimator {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Estimator();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize with given data
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Estimator() {} // Empty destructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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 get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
+ Vector3 const& baseOrientation);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Retrieve and update position and velocity of the 12 actuators
+ ///
+ /// \param[in] q_mes Position of the 12 actuators
+ /// \param[in] v_mes Velocity of the 12 actuators
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void get_data_joints(Vector12 const& q_mes, Vector12 const& v_mes);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Estimate base position and velocity using Forward Kinematics
+ ///
+ /// \param[in] feet_status Contact status of the four feet
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void get_data_FK(Eigen::Matrix<double, 1, 4> const& feet_status);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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 get_xyz_feet(Eigen::Matrix<double, 1, 4> const& feet_status, Matrix34 const& goals);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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 point
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Vector3 BaseVelocityFromKinAndIMU(int contactFrameId);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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] dummyPos Position of the robot in PyBullet simulator (only for simulation)
+ /// \param[in] b_baseVel Velocity of the robot in PyBullet simulator (only for simulation)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void run_filter(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& dummyPos, VectorN const& b_baseVel);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Security check to verify that measured positions, velocities and required torques
+ /// are not above defined tresholds
+ ///
+ /// \param[in] tau_ff Feedforward torques outputted by the whole body control for the PD+
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int security_check(VectorN const& tau_ff);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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
+ /// \param[in] gait Gait object
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateState(VectorN const& joystick_v_ref, Gait& gait);
+
+ VectorN getQFilt() { return q_filt_dyn_; }
+ VectorN getVFilt() { return v_filt_dyn_; }
+ VectorN getVSecu() { return v_secu_dyn_; }
+ Vector3 getVFiltBis() { return v_filt_bis_; }
+ Vector3 getRPY() { return IMU_RPY_; }
+ MatrixN getFeetStatus() { return feet_status_; }
+ MatrixN getFeetGoals() { return feet_goals_; }
+ Vector3 getFKLinVel() { return FK_lin_vel_; }
+ Vector3 getFKXYZ() { return FK_xyz_; }
+ Vector3 getXYZMeanFeet() { return xyz_mean_feet_; }
+ Vector3 getFiltLinVel() { return b_filt_lin_vel_; }
+
+ Vector3 getFilterVelX() { return filter_xyz_vel_.getX(); }
+ Vector3 getFilterVelDX() { return filter_xyz_vel_.getDX(); }
+ Vector3 getFilterVelAlpha() { return filter_xyz_vel_.getAlpha(); }
+ Vector3 getFilterVelFiltX() { return filter_xyz_vel_.getFiltX(); }
+
+ Vector3 getFilterPosX() { return filter_xyz_pos_.getX(); }
+ Vector3 getFilterPosDX() { return filter_xyz_pos_.getDX(); }
+ Vector3 getFilterPosAlpha() { return filter_xyz_pos_.getAlpha(); }
+ Vector3 getFilterPosFiltX() { return filter_xyz_pos_.getFiltX(); }
+
+ VectorN getQUpdated() { return q_up_; }
+ VectorN getVRef() { return v_ref_; }
+ VectorN getHV() { return h_v_; }
+ Vector3 getHVBis() { return h_v_bis_; }
+ Matrix3 getoRh() { return oRh_; }
+ Vector3 getoTh() { return oTh_; }
+ double getYawEstim() { return yaw_estim_; }
+
+ private:
+ ComplementaryFilter filter_xyz_pos_; // Complementary filter for base position
+ ComplementaryFilter filter_xyz_vel_; // Complementary filter for base velocity
+
+ double dt_wbc; // Time step of the estimator
+ double alpha_v_; // Low pass coefficient for the outputted filtered velocity
+ double alpha_secu_; // Low pass coefficient for the outputted filtered velocity for security check
+ double offset_yaw_IMU_; // Yaw orientation of the IMU at startup
+ bool perfect_estimator; // Enable perfect estimator (directly from the PyBullet simulation)
+ int N_SIMULATION; // Number of loops before the control ends
+ int k_log_; // Number of time the estimator has been called
+
+ Vector3 IMU_lin_acc_; // Linear acceleration of the IMU (gravity compensated)
+ Vector3 IMU_ang_vel_; // Angular velocity of the IMU
+ Vector3 IMU_RPY_; // Roll Pitch Yaw orientation of the IMU
+ pinocchio::SE3::Quaternion IMU_ang_pos_; // Quaternion orientation of the IMU
+ Vector12 actuators_pos_; // Measured positions of actuators
+ Vector12 actuators_vel_; // Measured velocities of actuators
+
+ Vector19 q_FK_; // Configuration vector for Forward Kinematics
+ Vector18 v_FK_; // Velocity vector for Forward Kinematics
+ MatrixN feet_status_; // Contact status of the four feet
+ MatrixN feet_goals_; // Target positions of the four feet
+ Vector3 FK_lin_vel_; // Base linear velocity estimated by Forward Kinematics
+ Vector3 FK_xyz_; // Base position estimated by Forward Kinematics
+ Vector3 b_filt_lin_vel_; // Filtered estimated velocity at center base (base frame)
+
+ Vector3 xyz_mean_feet_; // Barycenter of feet in contact
+
+ Eigen::Matrix<double, 1, 4> k_since_contact_; // Number of loops during which each foot has been in contact
+ int feet_indexes_[4] = {10, 18, 26, 34}; // Frame indexes of the four feet
+
+ pinocchio::Model model_, model_for_xyz_; // Pinocchio models for frame computations and forward kinematics
+ pinocchio::Data data_, data_for_xyz_; // Pinocchio datas for frame computations and forward kinematics
+
+ Vector19 q_filt_; // Filtered output configuration
+ Vector18 v_filt_; // Filtered output velocity
+ Vector12 v_secu_; // Filtered output velocity for security check
+
+ VectorN q_filt_dyn_; // Dynamic size version of q_filt_
+ VectorN v_filt_dyn_; // Dynamic size version of v_filt_
+ VectorN v_secu_dyn_; // Dynamic size version of v_secu_
+
+ pinocchio::SE3 _1Mi_; // Transform between the base frame and the IMU frame
+
+ Vector12 q_security_; // Position limits for the actuators above which safety controller is triggered
+
+ // For updateState function
+ VectorN q_up_; // Configuration vector
+ VectorN v_ref_; // Reference velocity vector
+ VectorN h_v_; // Velocity vector in horizontal frame
+ Matrix3 oRh_; // Rotation between horizontal and world frame
+ Vector3 oTh_; // Translation between horizontal and world frame
+ double yaw_estim_; // Yaw angle in perfect world
+
+ int N_queue_;
+ Vector3 v_filt_bis_;
+ Vector3 h_v_bis_;
+ std::deque<double> vx_queue_, vy_queue_, vz_queue_;
};
#endif // ESTIMATOR_H_INCLUDED
diff --git a/include/qrw/Filter.hpp b/include/qrw/Filter.hpp
index c87fb7983fb1a1f4be9854c276d8426db3524b4b..661f862a537241a6e82a64fb6b3c49dbfa6f5ffe 100644
--- a/include/qrw/Filter.hpp
+++ b/include/qrw/Filter.hpp
@@ -1,3 +1,12 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \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
@@ -10,67 +19,64 @@
#include "qrw/Params.hpp"
#include "pinocchio/math/rpy.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
+class Filter {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Filter();
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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 Initialize with given data
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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);
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Filter() {} // Empty destructor
- VectorN getFilt() { return y_; }
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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);
-private:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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);
- Vector1 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)
+ VectorN getFilt() { return y_; }
- 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
+ private:
+ Vector1 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)
- bool init_; // Initialisation flag
+ 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
diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 536ef25a0e72fab96d1b0a0809f9fa6b30e9759b..c3256a1c2ce7d4d20c7a60becf58c675858cef82 100644
--- a/include/qrw/FootTrajectoryGenerator.hpp
+++ b/include/qrw/FootTrajectoryGenerator.hpp
@@ -3,7 +3,7 @@
/// \brief This is the header for FootTrajectoryGenerator class
///
/// \details This class generates a reference trajectory for the swing foot, in position, velocity
-/// and acceleration
+/// and acceleration
///
//////////////////////////////////////////////////////////////////////////////////////////////////
@@ -14,87 +14,87 @@
#include "qrw/Params.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(Params& params, 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);
-
- Eigen::MatrixXd getFootPositionBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &T);
- Eigen::MatrixXd getFootVelocityBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &v_ref, const Eigen::Matrix<double, 3, 1> &w_ref);
- Eigen::MatrixXd getFootAccelerationBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &w_ref);
-
- 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:
- Gait* gait_; ///< Target lock before the touchdown
- double dt_wbc; ///<
- int k_mpc; ///<
- double maxHeight_; ///< Apex height of the swinging trajectory
- double lockTime_; ///< Target lock before the touchdown
- double vertTime_; ///< Duration during which feet move only along Z when taking off and landing
-
- std::vector<int> feet;
- Vector4 t0s;
- Vector4 t_swing;
-
- Matrix34 targetFootstep_; // Target for the X component
-
- Matrix64 Ax; ///< Coefficients for the X component
- Matrix64 Ay; ///< Coefficients for the Y component
-
- Matrix34 position_; // position computed in updateFootPosition
- Matrix34 velocity_; // velocity computed in updateFootPosition
- Matrix34 acceleration_; // acceleration computed in updateFootPosition
-
- Matrix34 position_base_; // position computed in updateFootPosition in base frame
- Matrix34 velocity_base_; // velocity computed in updateFootPosition in base frame
- Matrix34 acceleration_base_; // acceleration computed in updateFootPosition in base frame
+class FootTrajectoryGenerator {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ FootTrajectoryGenerator();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize with given data
+ ///
+ /// \param[in] params Object that stores parameters
+ /// \param[in] gait Gait object to hold the gait informations
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params ¶ms, Gait &gait);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~FootTrajectoryGenerator() {} // Empty constructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the next 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 Number of time steps since the start of the simulation
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void update(int k, MatrixN const &targetFootstep);
+
+ Eigen::MatrixXd getFootPositionBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &T);
+ Eigen::MatrixXd getFootVelocityBaseFrame(const Eigen::Matrix<double, 3, 3> &R,
+ const Eigen::Matrix<double, 3, 1> &v_ref,
+ const Eigen::Matrix<double, 3, 1> &w_ref);
+ Eigen::MatrixXd getFootAccelerationBaseFrame(const Eigen::Matrix<double, 3, 3> &R,
+ const Eigen::Matrix<double, 3, 1> &w_ref);
+
+ 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:
+ Gait *gait_; // Target lock before the touchdown
+ double dt_wbc; // Time step of the whole body control
+ int k_mpc; // Number of wbc time steps for each MPC time step
+ double maxHeight_; // Apex height of the swinging trajectory
+ double lockTime_; // Target lock before the touchdown
+ double vertTime_; // Duration during which feet move only along Z when taking off and landing
+
+ std::vector<int> feet; // Column indexes of feet currently in swing phase
+ Vector4 t0s; // Elapsed time since the start of the swing phase movement
+ Vector4 t_swing; // Swing phase duration for each foot
+
+ Matrix34 targetFootstep_; // Target for the X component
+
+ Matrix64 Ax; // Coefficients for the X component
+ Matrix64 Ay; // Coefficients for the Y component
+
+ Matrix34 position_; // Position computed in updateFootPosition
+ Matrix34 velocity_; // Velocity computed in updateFootPosition
+ Matrix34 acceleration_; // Acceleration computed in updateFootPosition
+
+ Matrix34 position_base_; // Position computed in updateFootPosition in base frame
+ Matrix34 velocity_base_; // Velocity computed in updateFootPosition in base frame
+ Matrix34 acceleration_base_; // Acceleration computed in updateFootPosition in base frame
};
#endif // TRAJGEN_H_INCLUDED
diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index c4db4f8cc5f3343843ffa199c4bc153b7185b84d..966d02c7dee46ce679a43f43356650138248788f 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -24,152 +24,159 @@
// Order of feet/legs: FL, FR, HL, HR
-class FootstepPlanner
-{
-public:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Empty constructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- FootstepPlanner();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Default constructor
- ///
- /// \param[in] dt_in Time step of the contact sequence (time step of the MPC)
- /// \param[in] dt_wbc_in Time step of whole body control
- /// \param[in] T_mpc_in MPC period (prediction horizon)
- /// \param[in] h_ref_in Reference height for the trunk
- /// \param[in] shoulderIn Position of shoulders in local frame
- /// \param[in] gaitIn Gait object to hold the gait informations
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void initialize(Params& params, Gait& gaitIn);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Destructor.
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ~FootstepPlanner() {}
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Refresh footsteps locations (computation and update of relevant matrices)
- ///
- /// \param[in] refresh true if we move one step further in the gait
- /// \param[in] k number of remaining wbc time step for the current mpc time step (wbc frequency is higher so there are inter-steps)
- /// \param[in] q current position vector of the flying base in horizontal frame (linear and angular stacked) + actuators
- /// \param[in] b_v current velocity vector of the flying base in horizontal frame (linear and angular stacked)
- /// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- MatrixN updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref);
-
- MatrixN getFootsteps();
- MatrixN getTargetFootsteps();
- MatrixN getRz();
-
-private:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Compute the desired location of footsteps and update relevant matrices
- ///
- /// \param[in] k number of remaining wbc time step for the current mpc time step (wbc frequency is higher so there are inter-steps)
- /// \param[in] q current position vector of the flying base in horizontal frame (linear and angular stacked)
- /// \param[in] b_v current velocity vector of the flying base in horizontal frame (linear and angular stacked)
- /// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- MatrixN computeTargetFootstep(int k, Vector6 const& q, Vector6 const& b_v, Vector6 const& b_vref);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Refresh feet position when entering a new contact phase
- ///
- /// \param[in] q current configuration vector
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void updateNewContact(Vector18 const& q);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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] b_v current velocity vector of sthe flying base in horizontal frame (linear and angular stacked)
- /// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void computeFootsteps(int k, Vector6 const& b_v, Vector6 const& b_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)
- /// \param[in] b_v current velocity vector of sthe flying base in horizontal frame (linear and angular stacked)
- /// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
- ///
- /// \retval Matrix with the next footstep positions
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void computeNextFootstep(int i, int j, Vector6 const& b_v, Vector6 const& b_vref);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Update desired location of footsteps using information coming from the footsteps planner
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void updateTargetFootsteps();
-
- MatrixN vectorToMatrix(std::vector<Matrix34> const& array);
-
- Params* params_; // Params object to store parameters
- Gait* gait_; // Gait object to hold the gait informations
-
- double dt; // Time step of the contact sequence (time step of the MPC)
- double dt_wbc; // Time step of the whole body control
- double T_gait; // Gait period
- double T_mpc; // MPC period (prediction horizon)
- double h_ref; // Reference height for the trunk
-
- // Predefined quantities
- 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 footsteps_under_shoulders_; // Positions of footsteps to be "under the shoulder"
- Matrix34 currentFootstep_; // Feet matrix
- Matrix34 nextFootstep_; // Temporary matrix to perform computations
- Matrix34 targetFootstep_; // In horizontal frame
- Matrix34 o_targetFootstep_; // targetFootstep_ in world frame
- std::vector<Matrix34> footsteps_;
-
- MatrixN Rz; // Rotation matrix along z axis
- VectorN dt_cum;
- VectorN yaws;
- VectorN dx;
- VectorN dy;
-
- Vector3 q_tmp;
- Vector3 q_dxdy;
- Vector3 RPY_;
- Eigen::Quaterniond quat_;
-
- pinocchio::Model model_; // Pinocchio model for forward kinematics
- pinocchio::Data data_; // Pinocchio datas for forward kinematics
- int foot_ids_[4] = {0, 0, 0, 0}; // Indexes of feet frames
- Matrix34 pos_feet_; // Estimated feet positions based on measurements
- Vector19 q_FK_;
-
+class FootstepPlanner {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ FootstepPlanner();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Default constructor
+ ///
+ /// \param[in] params Object that stores parameters
+ /// \param[in] gaitIn Gait object to hold the gait informations
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params, Gait& gaitIn);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~FootstepPlanner() {}
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Refresh footsteps locations (computation and update of relevant matrices)
+ ///
+ /// \param[in] refresh True if we move one step further in the gait
+ /// \param[in] k Number of remaining wbc time step for the current mpc time step (wbc frequency is higher so there
+ /// are inter-steps)
+ /// \param[in] q Current position vector of the flying base in horizontal frame (linear and angular stacked) +
+ /// actuators
+ /// \param[in] b_v Current velocity vector of the flying base in horizontal frame (linear and angular
+ /// stacked)
+ /// \param[in] b_vref Desired velocity vector of the flying base in horizontal frame (linear and angular
+ /// stacked)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ MatrixN updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref);
+
+ MatrixN getFootsteps();
+ MatrixN getTargetFootsteps();
+ MatrixN getRz();
+
+ private:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute the desired location of footsteps and update relevant matrices
+ ///
+ /// \param[in] k Number of remaining wbc time step for the current mpc time step (wbc frequency is higher so there
+ /// are inter-steps)
+ /// \param[in] q Current position vector of the flying base in horizontal frame (linear and
+ /// angular stacked)
+ /// \param[in] b_v Current velocity vector of the flying base in horizontal frame (linear and
+ /// angular stacked)
+ /// \param[in] b_vref Desired velocity vector of the flying base in horizontal frame (linear and
+ /// angular stacked)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ MatrixN computeTargetFootstep(int k, Vector6 const& q, Vector6 const& b_v, Vector6 const& b_vref);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Refresh feet position when entering a new contact phase
+ ///
+ /// \param[in] q Current configuration vector
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateNewContact(Vector18 const& q);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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] k Number of remaining wbc time step for the current mpc time step (wbc frequency is higher so there
+ /// are inter-steps)
+ /// \param[in] b_v Current velocity vector of sthe flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] b_vref Desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void computeFootsteps(int k, Vector6 const& b_v, Vector6 const& b_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)
+ /// \param[in] b_v Current velocity vector of sthe flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] b_vref Desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
+ ///
+ /// \retval Matrix with the next footstep positions
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void computeNextFootstep(int i, int j, Vector6 const& b_v, Vector6 const& b_vref);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update desired location of footsteps using information coming from the footsteps planner
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateTargetFootsteps();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Transform a std::vector of N 3x4 matrices into a single Nx12 matrix
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ MatrixN vectorToMatrix(std::vector<Matrix34> const& array);
+
+ Params* params_; // Params object to store parameters
+ Gait* gait_; // Gait object to hold the gait informations
+
+ double dt; // Time step of the contact sequence (time step of the MPC)
+ double dt_wbc; // Time step of the whole body control
+ double T_gait; // Gait period
+ double T_mpc; // MPC period (prediction horizon)
+ double h_ref; // Reference height for the trunk
+
+ // Predefined quantities
+ 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 footsteps_under_shoulders_; // Positions of footsteps to be "under the shoulder"
+ Matrix34 currentFootstep_; // Feet matrix
+ Matrix34 nextFootstep_; // Temporary matrix to perform computations
+ Matrix34 targetFootstep_; // In horizontal frame
+ Matrix34 o_targetFootstep_; // targetFootstep_ in world frame
+ std::vector<Matrix34> footsteps_; // Desired footsteps locations for each step of the horizon
+
+ MatrixN Rz; // Rotation matrix along z axis
+ VectorN dt_cum; // Cumulated time vector
+ VectorN yaws; // Predicted yaw variation for each cumulated time
+ VectorN dx; // Predicted x displacement for each cumulated time
+ VectorN dy; // Predicted y displacement for each cumulated time
+
+ Vector3 q_dxdy; // Temporary storage variable for offset to the future position
+ Vector3 RPY_; // Temporary storage variable for roll pitch yaw orientation
+
+ pinocchio::Model model_; // Pinocchio model for forward kinematics
+ pinocchio::Data data_; // Pinocchio datas for forward kinematics
+ int foot_ids_[4] = {0, 0, 0, 0}; // Indexes of feet frames
+ Matrix34 pos_feet_; // Estimated feet positions based on measurements
+ Vector19 q_FK_;
};
#endif // FOOTSTEPPLANNER_H_INCLUDED
diff --git a/include/qrw/Gait.hpp b/include/qrw/Gait.hpp
index 0fe2a48599ffb9704d13401e61cf764dc950e9d4..6fe55322fa931787593f22863b6bdba1ab9e353e 100644
--- a/include/qrw/Gait.hpp
+++ b/include/qrw/Gait.hpp
@@ -17,152 +17,153 @@
// Order of feet/legs: FL, FR, HL, HR
-class Gait
-{
-public:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Empty constructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- Gait();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Destructor.
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ~Gait() {}
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Initializer
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void initialize(Params& params);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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 Handle the joystick code to trigger events (change of gait for instance)
- ///
- /// \param[in] k numero of the current loop
- /// \param[in] k_mpc number of loop per mpc time step
- /// \param[in] code integer to trigger events with the joystick
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- bool changeGait(int const k, int const k_mpc, int const code);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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 numero of the current loop
- /// \param[in] k_mpc number of loop per mpc time step
- /// \param[in] joystickCode integer to trigger events with the joystick
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void updateGait(int const k, int const k_mpc, int const joystickCode);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Move one step further into the gait
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void rollGait();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Update the gait matrix externally (directly set the gait matrix)
- ///
- /// \param[in] gaitMatrix gait matrix that should be used for the incoming timesteps
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- bool setGait(MatrixN const& gaitMatrix);
-
- MatrixN getPastGait() { return pastGait_; }
- MatrixN getCurrentGait() { return currentGait_; }
- double getCurrentGaitCoeff(int i, int j) { return currentGait_(i, j); }
- MatrixN getDesiredGait() { return desiredGait_; }
- double getRemainingTime() { return remainingTime_; }
- bool getIsStatic() { return is_static_; }
- VectorN getQStatic() { return q_static_; }
- bool isNewPhase() { return newPhase_; }
-
-private:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a slow walking gait, raising and moving only one foot at a time
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void create_walk();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a trot gait with diagonaly opposed legs moving at the same time
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void create_trot();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a pacing gait with legs on the same side (left or right) moving at the same time
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void create_pacing();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a bounding gait with legs on the same side (front or hind) moving at the same time
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void create_bounding();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a static gait with all legs in stance phase
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void create_static();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Initialize content of the gait matrix based on the desired gait, the gait period and
- /// the length of the prediciton horizon
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void create_gait_f();
-
- 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)
- int n_steps_; // Number of time steps in the prediction horizon
-
- double remainingTime_;
-
- bool newPhase_;
- bool is_static_;
- int switch_to_gait_;
- VectorN q_static_;
+class Gait {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Gait();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Gait() {}
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initializer
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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 Handle the joystick code to trigger events (change of gait for instance)
+ ///
+ /// \param[in] k Numero of the current loop
+ /// \param[in] k_mpc Number of loop per mpc time step
+ /// \param[in] code Integer to trigger events with the joystick
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ bool changeGait(int const k, int const k_mpc, int const code);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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 Numero of the current loop
+ /// \param[in] k_mpc Number of loop per mpc time step
+ /// \param[in] joystickCode Integer to trigger events with the joystick
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateGait(int const k, int const k_mpc, int const joystickCode);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Move one step further into the gait
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void rollGait();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the gait matrix externally (directly set the gait matrix)
+ ///
+ /// \param[in] gaitMatrix Gait matrix that should be used for the incoming timesteps
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ bool setGait(MatrixN const& gaitMatrix);
+
+ MatrixN getPastGait() { return pastGait_; }
+ MatrixN getCurrentGait() { return currentGait_; }
+ double getCurrentGaitCoeff(int i, int j) { return currentGait_(i, j); }
+ MatrixN getDesiredGait() { return desiredGait_; }
+ double getRemainingTime() { return remainingTime_; }
+ bool getIsStatic() { return is_static_; }
+ VectorN getQStatic() { return q_static_; }
+ bool isNewPhase() { return newPhase_; }
+
+ private:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a slow walking gait, raising and moving only one foot at a time
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_walk();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a trot gait with diagonaly opposed legs moving at the same time
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_trot();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a pacing gait with legs on the same side (left or right) moving at the same time
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_pacing();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a bounding gait with legs on the same side (front or hind) moving at the same time
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_bounding();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a static gait with all legs in stance phase
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_static();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize content of the gait matrix based on the desired gait, the gait period and
+ /// the length of the prediciton horizon
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_gait_f();
+
+ 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)
+ int n_steps_; // Number of time steps in the prediction horizon
+
+ double remainingTime_; // Remaining time till the end of the current stance/swing phase
+
+ bool newPhase_; // Flag to indicate that the contact status has changed
+ bool is_static_; // Flag to indicate that all feet are in an infinite stance phase
+ int switch_to_gait_; // Memory to store joystick code if the user wants to change the gait pattern
+ VectorN q_static_; // Configuration vector used during static phases (4 feet in stance)
};
#endif // GAIT_H_INCLUDED
diff --git a/include/qrw/InvKin.hpp b/include/qrw/InvKin.hpp
index 76b63417a07a3ec9a20c385beae26a5ee34696ad..435622f2e2416cdc3f3ca996f3b84f2054a86088 100644
--- a/include/qrw/InvKin.hpp
+++ b/include/qrw/InvKin.hpp
@@ -1,3 +1,12 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for InvKin class
+///
+/// \details Perform inverse kinematics to output command positions, velocities and accelerations for the actuators
+/// based on contact status and desired position, velocity and acceleration of the feet
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
#ifndef INVKIN_H_INCLUDED
#define INVKIN_H_INCLUDED
@@ -18,72 +27,99 @@
#include "qrw/Params.hpp"
#include "qrw/Types.h"
-class InvKin
-{
-public:
- InvKin();
- void initialize(Params& params);
-
- void refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals, Matrix43 const& vgoals, Matrix43 const& agoals);
-
- void run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals, MatrixN const& vgoals, MatrixN const& agoals);
-
- Eigen::MatrixXd get_q_step() { return q_step_; }
- Eigen::MatrixXd get_dq_cmd() { return dq_cmd_; }
- VectorN get_q_cmd() { return q_cmd_; }
- VectorN get_ddq_cmd() { return ddq_cmd_; }
- Matrix12 get_Jf() { return Jf_; }
- int get_foot_id(int i) { return foot_ids_[i];}
- Matrix43 get_posf() { return posf_; }
- Matrix43 get_vf() { return vf_; }
-
-private:
- // Inputs of the constructor
- Params* params_;
-
- // Matrices initialisation
-
- Matrix12 invJ;
- Matrix112 acc;
- Matrix112 x_err;
- Matrix112 dx_r;
-
- Matrix43 pfeet_err;
- Matrix43 vfeet_ref;
- Matrix43 afeet;
- Matrix13 e_basispos;
- Matrix13 abasis;
- Matrix13 e_basisrot;
- Matrix13 awbasis;
-
- int foot_ids_[4] = {0, 0, 0, 0};
-
- Matrix43 posf_;
- Matrix43 vf_;
- Matrix43 wf_;
- Matrix43 af_;
- Matrix12 Jf_;
- Eigen::Matrix<double, 6, 12> Jf_tmp_;
-
- Vector12 ddq_cmd_;
- Vector12 dq_cmd_;
- Vector12 q_cmd_;
- Vector12 q_step_;
-
- pinocchio::Model model_; // Pinocchio model for frame computations and inverse kinematics
- pinocchio::Data data_; // Pinocchio datas for frame computations and inverse kinematics
-
- 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();
+class InvKin {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ InvKin();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize with given data
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~InvKin() {} // Empty destructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Solve the inverse kinematics problem by inverting the joint Jacobian
+ ///
+ /// \param[in] contacts Contact status of the four feet
+ /// \param[in] pgoals Desired positions of the four feet in base frame
+ /// \param[in] vgoals Desired velocities of the four feet in base frame
+ /// \param[in] agoals Desired accelerations of the four feet in base frame
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals, Matrix43 const& vgoals,
+ Matrix43 const& agoals);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Solve the inverse kinematics problem by inverting the feet Jacobian
+ ///
+ /// \param[in] q Estimated positions of the 12 actuators
+ /// \param[in] dq Estimated velocities of the 12 actuators
+ /// \param[in] contacts Contact status of the four feet
+ /// \param[in] pgoals Desired positions of the four feet in base frame
+ /// \param[in] vgoals Desired velocities of the four feet in base frame
+ /// \param[in] agoals Desired accelerations of the four feet in base frame
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
+ MatrixN const& vgoals, MatrixN const& agoals);
+
+ Eigen::MatrixXd get_q_step() { return q_step_; }
+ Eigen::MatrixXd get_dq_cmd() { return dq_cmd_; }
+ VectorN get_q_cmd() { return q_cmd_; }
+ VectorN get_ddq_cmd() { return ddq_cmd_; }
+ Matrix12 get_Jf() { return Jf_; }
+ int get_foot_id(int i) { return foot_ids_[i]; }
+ Matrix43 get_posf() { return posf_; }
+ Matrix43 get_vf() { return vf_; }
+
+ private:
+ Params* params_; // Params object to store parameters
+
+ // Matrices initialisation
+
+ Matrix12 invJ; // Inverse of the feet Jacobian
+ Matrix112 acc; // Reshaped feet acceleration references to get command accelerations for actuators
+ Matrix112 x_err; // Reshaped feet position errors to get command position step for actuators
+ Matrix112 dx_r; // Reshaped feet velocity references to get command velocities for actuators
+
+ Matrix43 pfeet_err; // Feet position errors to get command position step for actuators
+ Matrix43 vfeet_ref; // Feet velocity references to get command velocities for actuators
+ Matrix43 afeet; // Feet acceleration references to get command accelerations for actuators
+
+ int foot_ids_[4] = {0, 0, 0, 0}; // Feet frame IDs
+
+ Matrix43 posf_; // Current feet positions
+ Matrix43 vf_; // Current feet linear velocities
+ Matrix43 wf_; // Current feet angular velocities
+ Matrix43 af_; // Current feet linear accelerations
+ Matrix12 Jf_; // Current feet Jacobian (only linear part)
+ Eigen::Matrix<double, 6, 12> Jf_tmp_; // Temporary storage variable to only retrieve the linear part of the Jacobian
+ // and discard the angular part
+
+ Vector12 ddq_cmd_; // Actuator command accelerations
+ Vector12 dq_cmd_; // Actuator command velocities
+ Vector12 q_cmd_; // Actuator command positions
+ Vector12 q_step_; // Actuator command position steps
+
+ pinocchio::Model model_; // Pinocchio model for frame computations and inverse kinematics
+ pinocchio::Data data_; // Pinocchio datas for frame computations and inverse kinematics
};
-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
diff --git a/include/qrw/Joystick.hpp b/include/qrw/Joystick.hpp
index c87a393f68997cc77048b19a4574a172ef3e2bc8..d6a7868dd31b1d1d944ffb60084ccccd02870a12 100644
--- a/include/qrw/Joystick.hpp
+++ b/include/qrw/Joystick.hpp
@@ -1,51 +1,53 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Joystick class
+///
+/// \details This class handles computations related to the reference velocity of the robot
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
#ifndef JOYSTICK_H_INCLUDED
#define JOYSTICK_H_INCLUDED
-#include "qrw/Types.h"
#include <iostream>
#include <fstream>
-#include <string>
#include <cmath>
-#include <limits>
-#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
+#include "qrw/Types.h"
-
-class Joystick
-{
-public:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Empty constructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- Joystick();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Destructor.
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ~Joystick() {}
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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] k numero of the current loop
- /// \param[in] k_switch information about the position of key frames
- /// \param[in] v_switch information about the desired velocity for each key frame
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- VectorN handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch);
-
-private:
-
- Vector6 A3_;
- Vector6 A2_;
- Vector6 v_ref_;
+class Joystick {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Joystick();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Joystick() {}
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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] k Numero of the current loop
+ /// \param[in] k_switch Information about the position of key frames
+ /// \param[in] v_switch Information about the desired velocity for each key frame
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ VectorN handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch);
+
+ private:
+ Vector6 A3_; // Third order coefficient of the polynomial that generates the velocity profile
+ Vector6 A2_; // Second order coefficient of the polynomial that generates the velocity profile
+ Vector6 v_ref_; // Reference velocity resulting of the polynomial interpolation
};
#endif // JOYSTICK_H_INCLUDED
diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp
index 476bb1c0a5abf8df8df0980c458ba34641a66c9c..f003cddf1218f175ae29ee2306b4f0be62879f81 100644
--- a/include/qrw/MPC.hpp
+++ b/include/qrw/MPC.hpp
@@ -16,36 +16,232 @@
typedef Eigen::MatrixXd matXd;
class MPC {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ MPC();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor with parameters
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ MPC(Params ¶ms);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~MPC() {} // Empty destructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Run one iteration of the whole MPC by calling all the necessary functions (data retrieval, update
+ /// of constraint matrices, update of the solver, running the solver, retrieving result)
+ ///
+ /// \param[in] num_iter Number of the current iteration of the MPC
+ /// \param[in] xref_in Reference state trajectory over the prediction horizon
+ /// \param[in] fsteps_in Footsteps location over the prediction horizon stored in a Nx12 matrix
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in);
+
+ // Getters
+ Eigen::MatrixXd get_latest_result(); // Return the latest desired contact forces that have been computed
+ Eigen::MatrixXd get_gait(); // Return the gait matrix
+ Eigen::MatrixXd get_Sgait(); // Return the S_gait matrix
+ double *get_x_next(); // Return the next predicted state of the base
+
private:
- Params* params_;
- double dt, mass, mu, T_gait;
- int n_steps, cpt_ML, cpt_P;
-
- Eigen::Matrix<double, 3, 3> gI;
- Eigen::Matrix<double, 3, 4> footholds = Eigen::Matrix<double, 3, 4>::Zero();
- Eigen::Matrix<double, 1, 12> footholds_tmp = Eigen::Matrix<double, 12, 1>::Zero();
- Eigen::Matrix<double, 3, 4> lever_arms = Eigen::Matrix<double, 3, 4>::Zero();
- Eigen::Matrix<int, Eigen::Dynamic, 4> gait;
- Eigen::Matrix<int, Eigen::Dynamic, 4> inv_gait;
- Eigen::Matrix<double, 12, 1> g = Eigen::Matrix<double, 12, 1>::Zero();
- Eigen::Matrix<double, 3, 1> offset_CoM = Eigen::Matrix<double, 3, 1>::Zero();
-
- Eigen::Matrix<double, 12, 12> A = Eigen::Matrix<double, 12, 12>::Identity();
- Eigen::Matrix<double, 12, 12> B = Eigen::Matrix<double, 12, 12>::Zero();
- Eigen::Matrix<double, 12, 1> x0 = Eigen::Matrix<double, 12, 1>::Zero();
- double x_next[12] = {};
- Eigen::MatrixXd x_f_applied;
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Print positions and value of coefficients in a csc matrix
+ ///
+ /// \param[in] M (csc*): pointer to the csc matrix you want to print
+ /// \param[in] name (char*): name that should be displayed for the matrix (one char)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void my_print_csc_matrix(csc *M, const char *name);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Save positions and value of coefficients of a csc matrix in a csc file
+ ///
+ /// \param[in] M (csc*): pointer to the csc matrix you want to save
+ /// \param[in] filename (string): name of the generated csv file
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void save_csc_matrix(csc *M, std::string filename);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Save positions and value of coefficients of a dense matrix in a csc file
+ ///
+ /// \param[in] M (double*): pointer to the dense matrix you want to save
+ /// \param[in] size (int): size of the dense matrix
+ /// \param[in] filename (string): name of the generated csv file
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void save_dns_matrix(double *M, int size, std::string filename);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Add a new non-zero coefficient to the sparse ML matrix by filling the triplet r_ML / c_ML / v_ML
+ ///
+ /// \param[in] i Row index of the new entry
+ /// \param[in] j Column index of the new entry
+ /// \param[in] v Value of the new entry
+ /// \param[in] r_ML Pointer to the table that contains row indexes
+ /// \param[in] c_ML Pointer to the table that contains column indexes
+ /// \param[in] v_ML Pointer to the table that contains values
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ inline void add_to_ML(int i, int j, double v, int *r_ML, int *c_ML, double *v_ML);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Add a new non-zero coefficient to the P matrix by filling the triplet r_P / c_P / v_P
+ ///
+ /// \param[in] i Row index of the new entry
+ /// \param[in] j Column index of the new entry
+ /// \param[in] v Value of the new entry
+ /// \param[in] r_P Pointer to the table that contains row indexes
+ /// \param[in] c_P Pointer to the table that contains column indexes
+ /// \param[in] v_P Pointer to the table that contains values
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ inline void add_to_P(int i, int j, double v, int *r_P, int *c_P, double *v_P);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create the constraint matrices (M.X = N and L.X <= K)
+ /// Create the weight matrices P and Q (cost 1/2 x^T * P * X + X^T * Q)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int create_matrices();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create the M and L matrices involved in the constraint equations
+ /// the solution has to respect: M.X = N and L.X <= K
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int create_ML();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create the N and K matrices involved in the MPC constraint equations
+ /// the solution has to respect: M.X = N and L.X <= K
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int create_NK();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create the weight matrices P and Q in the cost function
+ /// 1/2 x^T.P.x + x^T.q of the QP problem
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int create_weight_matrices();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the M, N, L and K constraint matrices depending on what happened
+ ///
+ /// \param[in] fsteps Footsteps location over the prediction horizon stored in a Nx12 matrix
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int update_matrices(Eigen::MatrixXd fsteps);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the M and L constaint matrices depending on the current state of the gait
+ ///
+ /// \param[in] fsteps Footsteps location over the prediction horizon stored in a Nx12 matrix
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int update_ML(Eigen::MatrixXd fsteps);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the N and K matrices involved in the MPC constraint equations M.X = N and L.X <= K
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int update_NK();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize the solver (first iteration) or update it (next iterations) then call the
+ /// OSQP solver to solve the QP problem
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int call_solver(int);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Extract relevant information from the output of the QP solver
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int retrieve_result();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Returns the skew matrix of a 3 by 1 column vector
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Matrix3 getSkew(Vector3 v);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Construct an array of size 12*N that contains information about the contact state of feet.
+ /// This matrix is used to enable/disable contact forces in the QP problem.
+ /// N is the number of time step in the prediction horizon.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int construct_S();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Reconstruct the gait matrix based on the fsteps matrix since only the last one is received by the MPC
+ ///
+ /// \param[in] fsteps Footsteps location over the prediction horizon stored in a Nx12 matrix
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int construct_gait(Eigen::MatrixXd fsteps_in);
+
+ Params *params_; // Object that stores parameters
+ double dt; // Time step
+ double mass; // Total mass
+ double mu; // Friction coefficient
+ double T_gait; // Period of the gait
+ int n_steps; // Number of time steps in the prediction horizon
+ int cpt_ML; // Counter of ML coefficients
+ int cpt_P; // Counter of P coefficients
+
+ Matrix3 gI; // Inertia matrix
+ Matrix34 footholds = Matrix34::Zero(); // Initial position of footsteps
+ Matrix112 footholds_tmp = Matrix112::Zero(); // Updated position of footsteps
+ Matrix34 lever_arms = Matrix34::Zero(); // Lever arms of footsteps with the center of mass
+ Eigen::Matrix<int, Eigen::Dynamic, 4> gait; // Contact status over the prediction horizon
+ Eigen::Matrix<int, Eigen::Dynamic, 4> inv_gait; // Inversed contact status over the prediction horizon
+ Vector12 g = Vector12::Zero(); // Gravity vector
+ Vector3 offset_CoM = Vector3::Zero(); // Offset of the CoM position compared to center of base
+
+ Matrix12 A = Matrix12::Identity(); // Of evolution X+ = A*X + B*f + C
+ Matrix12 B = Matrix12::Zero(); // Of evolution X+ = A*X + B*f + C
+ Vector12 x0 = Vector12::Zero(); // Current state of the robot
+ double x_next[12] = {}; // Next state of the robot (difference with reference state)
+ Eigen::MatrixXd x_f_applied; // Next predicted state of the robot + Desired contact forces to reach it
// Matrix ML
const static int size_nz_ML = 5000;
- // int r_ML [size_nz_ML] = {}; // row indexes of non-zero values in matrix ML
- // int c_ML [size_nz_ML] = {}; // col indexes of non-zero values in matrix ML
- // double v_ML [size_nz_ML] = {}; // non-zero values in matrix ML
- // csc* ML_triplet; // Compressed Sparse Column matrix (triplet format)
csc *ML; // Compressed Sparse Column matrix
- inline void add_to_ML(int i, int j, double v, int *r_ML, int *c_ML,
- double *v_ML); // function to fill the triplet r/c/v
- inline void add_to_P(int i, int j, double v, int *r_P, int *c_P, double *v_P); // function to fill the triplet r/c/v
// Indices that are used to udpate ML
int i_x_B[12 * 4] = {};
@@ -61,9 +257,6 @@ class MPC {
// Matrix P
const static int size_nz_P = 5000;
- // c_int r_P [size_nz_P] = {}; // row indexes of non-zero values in matrix ML
- // c_int c_P [size_nz_P] = {}; // col indexes of non-zero values in matrix ML
- // c_float v_P [size_nz_P] = {}; // non-zero values in matrix ML
csc *P; // Compressed Sparse Column matrix
// Matrix Q
@@ -76,55 +269,16 @@ class MPC {
OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
// Matrices whose size depends on the arguments sent to the constructor function
- Eigen::Matrix<double, 12, Eigen::Dynamic> xref;
- Eigen::Matrix<double, Eigen::Dynamic, 1> x;
- Eigen::Matrix<int, Eigen::Dynamic, 1> S_gait;
- Eigen::Matrix<double, Eigen::Dynamic, 1> warmxf;
- Eigen::Matrix<double, Eigen::Dynamic, 1> NK_up;
- Eigen::Matrix<double, Eigen::Dynamic, 1> NK_low;
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> D;
- Eigen::Matrix<int, Eigen::Dynamic, 1> i_off;
-
- public:
- MPC();
- MPC(Params& params);
-
- int create_matrices();
- int create_ML();
- int create_NK();
- int create_weight_matrices();
- int update_matrices(Eigen::MatrixXd fsteps);
- int update_ML(Eigen::MatrixXd fsteps);
- int update_NK();
- int call_solver(int);
- int retrieve_result();
- double *get_x_next();
- int run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in);
-
- Eigen::Matrix<double, 3, 3> getSkew(Eigen::Matrix<double, 3, 1> v);
- int construct_S();
- int construct_gait(Eigen::MatrixXd fsteps_in);
-
- // Getters
- Eigen::MatrixXd get_latest_result();
- Eigen::MatrixXd get_gait();
- Eigen::MatrixXd get_Sgait();
-
-
- // Utils
- void my_print_csc_matrix(csc *M, const char *name);
- void save_csc_matrix(csc *M, std::string filename);
- void save_dns_matrix(double *M, int size, std::string filename);
-
- // Bindings
- void run_python(const matXd &xref_py, const matXd &fsteps_py);
-
- // Eigen::Matrix<double, 12, 12> getA() { return A; }
- // Eigen::MatrixXf getML() { return ML; }
- /*void setDate(int year, int month, int day);
- int getYear();
- int getMonth();
- int getDay();*/
+ Eigen::Matrix<double, 12, Eigen::Dynamic> xref; // Reference state trajectory over the prediction horizon
+ VectorN x; // State vector
+ Eigen::Matrix<int, Eigen::Dynamic, 1> S_gait; // Matrix used to enable/disable feet in the solver
+ VectorN warmxf; // Vector to store the solver warm start
+ VectorN NK_up; // Upper constraint limit
+ VectorN NK_low; // Lower constraint limit
+ // There is no M.X = N and L.X <= K, it's actually NK_low <= ML.X <= NK_up for the solver
+ MatrixN D; // Matrix used to create NK matrix
+ Eigen::Matrix<int, Eigen::Dynamic, 1> i_off; // Coefficient offsets to directly update the data field
+ // Sparse matrix coefficents are all stored in M->x so if we know the indexes we can update them directly
};
#endif // MPC_H_INCLUDED
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index 4f147d7f1e52fe0399a6ebbffbebcec17fcf7c90..47a79e7329d04ace5bf240c76f9be4aa50e16672 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -2,8 +2,9 @@
///
/// \brief This is the header for Params class
///
-/// \details Planner that outputs the reference trajectory of the base based on the reference
-/// velocity given by the user and the current position/velocity of the base
+/// \details This class retrieves and stores all parameters written in the main .yaml so that the user can easily
+/// change their value without digging into the code. It also stores some model parameters whose values depends on what
+/// is in the yaml
///
//////////////////////////////////////////////////////////////////////////////////////////////////
@@ -13,116 +14,124 @@
#include "qrw/Types.h"
#include <yaml-cpp/yaml.h>
-class Params
-{
-public:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Empty constructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- Params();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Destructor.
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ~Params() {}
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Initializer
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void initialize(const std::string& file_path);
-
-
- // See .yaml file for meaning of parameters
- std::string interface;
- bool SIMULATION;
- bool LOGGING;
- bool PLOTTING;
- int envID;
- bool use_flat_plane;
- bool predefined_vel;
- int velID;
- int N_SIMULATION;
- bool enable_pyb_GUI;
- bool enable_corba_viewer;
- bool enable_multiprocessing;
- bool perfect_estimator;
-
- std::vector<double> q_init;
- double dt_wbc;
- int N_gait;
- double dt_mpc;
- double T_gait;
- double T_mpc;
- int type_MPC;
- bool kf_enabled;
- double Kp_main;
- double Kd_main;
- double Kff_main;
-
- double fc_v_esti;
-
- double k_feedback;
-
- double max_height;
- double lock_time;
- double vert_time;
-
- std::vector<double> osqp_w_states;
- std::vector<double> osqp_w_forces;
- double osqp_Nz_lim;
-
- double Kp_flyingfeet;
- double Kd_flyingfeet;
-
- double Q1;
- double Q2;
- double Fz_max;
- double Fz_min;
-
-
- // Not defined in yaml
- double mass; // Mass of the robot
- std::vector<double> I_mat; // Inertia matrix
- double h_ref; // Reference height for the base
- std::vector<double> shoulders; // Position of shoulders in base frame
- std::vector<double> footsteps_init; // Initial 3D position of footsteps in base frame
- std::vector<double> footsteps_under_shoulders; // // Positions of footsteps to be "under the shoulder"
-
+class Params {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Params();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Params() {}
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initializer
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(const std::string& file_path);
+
+ // See .yaml file for meaning of parameters
+ // General parameters
+ std::string interface; // Name of the communication interface (check with ifconfig)
+ bool SIMULATION; // Enable/disable PyBullet simulation or running on real robot
+ bool LOGGING; // Enable/disable logging during the experiment
+ bool PLOTTING; // Enable/disable automatic plotting at the end of the experiment
+ int envID; // Identifier of the environment to choose in which one the simulation will happen
+ bool use_flat_plane; // If True the ground is flat, otherwise it has bumps
+ bool predefined_vel; // If we are using a predefined reference velocity (True) or a joystick (False)
+ int velID; // Identifier of the reference velocity profile to choose which one will be sent to the robot
+ int N_SIMULATION; // Number of simulated wbc time steps
+ bool enable_pyb_GUI; // Enable/disable PyBullet GUI
+ bool enable_corba_viewer; // Enable/disable Corba Viewer
+ bool enable_multiprocessing; // Enable/disable running the MPC in another process in parallel of the main loop
+ bool perfect_estimator; // Enable/disable perfect estimator by using data directly from PyBullet
+
+ // General control parameters
+ std::vector<double> q_init; // Initial articular positions
+ double dt_wbc; // Time step of the whole body control
+ int N_gait; // 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
+ double dt_mpc; // Time step of the model predictive control
+ double T_gait; // Duration of one gait period
+ double T_mpc; // Duration of the prediction horizon
+ int type_MPC; // Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
+ bool kf_enabled; // Use complementary filter (False) or kalman filter (True) for the estimator
+ double Kp_main; // Proportional gains for the PD+
+ double Kd_main; // Derivative gains for the PD+
+ double Kff_main; // Feedforward torques multiplier for the PD+
+
+ // Parameters of Estimator
+ double fc_v_esti; // Cut frequency for the low pass that filters the estimated base velocity
+
+ // Parameters of FootstepPlanner
+ double k_feedback; // Value of the gain for the feedback heuristic
+
+ // Parameters of FootTrajectoryGenerator
+ double max_height; // Apex height of the swinging trajectory [m]
+ double lock_time; // Target lock before the touchdown [s]
+ double vert_time; // Duration during which feet move only along Z when taking off and landing
+
+ // Parameters of MPC with OSQP
+ std::vector<double> osqp_w_states; // Weights for state tracking error
+ std::vector<double> osqp_w_forces; // Weights for force regularisation
+ double osqp_Nz_lim; // Maximum vertical force that can be applied at contact points
+
+ // Parameters of InvKin
+ double Kp_flyingfeet; // Proportional gain for feet position tasks
+ double Kd_flyingfeet; // Derivative gain for feet position tasks
+
+ // Parameters of WBC QP problem
+ double Q1; // Weights for the "delta articular accelerations" optimization variables
+ double Q2; // Weights for the "delta contact forces" optimization variables
+ double Fz_max; // Maximum vertical contact force [N]
+ double Fz_min; // Minimal vertical contact force [N]
+
+ // Not defined in yaml
+ double mass; // Mass of the robot
+ std::vector<double> I_mat; // Inertia matrix
+ double h_ref; // Reference height for the base
+ std::vector<double> shoulders; // Position of shoulders in base frame
+ std::vector<double> footsteps_init; // Initial 3D position of footsteps in base frame
+ std::vector<double> footsteps_under_shoulders; // Positions of footsteps to be "under the shoulder"
};
-namespace yaml_control_interface
-{
-#define assert_yaml_parsing(yaml_node, parent_node_name, child_node_name) \
- if (!yaml_node[child_node_name]) \
- { \
- std::ostringstream oss; \
- oss << "Error: Wrong parsing of the YAML file from src file: [" \
- << __FILE__ << "], in function: [" << __FUNCTION__ << "], line: [" \
- << __LINE__ << ". Node [" << child_node_name \
- << "] does not exists under the node [" << parent_node_name \
- << "]."; \
- throw std::runtime_error(oss.str()); \
- } \
- assert(true)
-
-#define assert_file_exists(filename) \
- std::ifstream f(filename.c_str()); \
- if (!f.good()) \
- { \
- std::ostringstream oss; \
- oss << "Error: Problem opening the file [" << filename \
- << "], from src file: [" << __FILE__ << "], in function: [" \
- << __FUNCTION__ << "], line: [" << __LINE__ \
- << ". The file may not exists."; \
- throw std::runtime_error(oss.str()); \
- } \
- assert(true)
-} // end of yaml_control_interface namespace
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Check if a parameter exists in a given yaml file (bofore we try retrieving its value)
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
+namespace yaml_control_interface {
+#define assert_yaml_parsing(yaml_node, parent_node_name, child_node_name) \
+ if (!yaml_node[child_node_name]) { \
+ std::ostringstream oss; \
+ oss << "Error: Wrong parsing of the YAML file from src file: [" << __FILE__ << "], in function: [" \
+ << __FUNCTION__ << "], line: [" << __LINE__ << ". Node [" << child_node_name \
+ << "] does not exists under the node [" << parent_node_name << "]."; \
+ throw std::runtime_error(oss.str()); \
+ } \
+ assert(true)
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Check if a file exists (before we try loading it)
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
+#define assert_file_exists(filename) \
+ std::ifstream f(filename.c_str()); \
+ if (!f.good()) { \
+ std::ostringstream oss; \
+ oss << "Error: Problem opening the file [" << filename << "], from src file: [" << __FILE__ \
+ << "], in function: [" << __FUNCTION__ << "], line: [" << __LINE__ << ". The file may not exists."; \
+ throw std::runtime_error(oss.str()); \
+ } \
+ assert(true)
+} // namespace yaml_control_interface
#endif // PARAMS_H_INCLUDED
diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index ec900b965a5d5ada8120697939116c3498c199be..b3296c368301eaf9feaaa879afc3a3c54911ed10 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -1,8 +1,16 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for QPWBC and WbcWrapper classes
+///
+/// \details WbcWrapper provides an interface for the user to solve the whole body control problem
+/// Internally it calls first the InvKin class to solve an inverse kinematics problem then calls the QPWBC
+/// class to solve a box QP problem based on result from the inverse kinematic and desired ground forces
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
#ifndef QPWBC_H_INCLUDED
#define QPWBC_H_INCLUDED
-#include "qrw/InvKin.hpp" // For pseudoinverse
-#include "qrw/Params.hpp"
#include <iostream>
#include <fstream>
#include <string>
@@ -12,6 +20,8 @@
#include <Eigen/Core>
#include <Eigen/Dense>
#include "osqp.h"
+#include "qrw/InvKin.hpp"
+#include "qrw/Params.hpp"
#include "other/st_to_cc.hpp"
// For wrapper
@@ -25,12 +35,169 @@
#include "pinocchio/algorithm/crba.hpp"
class QPWBC {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ QPWBC();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize with given data
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params ¶ms);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~QPWBC() {} // Empty destructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Run one iteration of the whole WBC QP problem by calling all the necessary functions (data retrieval,
+ /// update of constraint matrices, update of the solver, running the solver, retrieving result)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd,
+ const Eigen::MatrixXd &RNEA, const Eigen::MatrixXd &k_contact);
+
+ // Getters
+ Eigen::MatrixXd get_f_res(); // Return the f_res matrix
+ Eigen::MatrixXd get_ddq_res(); // Return the ddq_res matrix
+ Eigen::MatrixXd get_H(); // Return the H matrix
+
private:
-
- Params* params_; // Object that stores parameters
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Print positions and value of coefficients in a csc matrix
+ ///
+ /// \param[in] M (csc*): pointer to the csc matrix you want to print
+ /// \param[in] name (char*): name that should be displayed for the matrix (one char)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void my_print_csc_matrix(csc *M, const char *name);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Save positions and value of coefficients of a csc matrix in a csc file
+ ///
+ /// \param[in] M (csc*): pointer to the csc matrix you want to save
+ /// \param[in] filename (string): name of the generated csv file
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void save_csc_matrix(csc *M, std::string filename);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Save positions and value of coefficients of a dense matrix in a csc file
+ ///
+ /// \param[in] M (double*): pointer to the dense matrix you want to save
+ /// \param[in] size (int): size of the dense matrix
+ /// \param[in] filename (string): name of the generated csv file
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void save_dns_matrix(double *M, int size, std::string filename);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Add a new non-zero coefficient to the sparse ML matrix by filling the triplet r_ML / c_ML / v_ML
+ ///
+ /// \param[in] i Row index of the new entry
+ /// \param[in] j Column index of the new entry
+ /// \param[in] v Value of the new entry
+ /// \param[in] r_ML Pointer to the table that contains row indexes
+ /// \param[in] c_ML Pointer to the table that contains column indexes
+ /// \param[in] v_ML Pointer to the table that contains values
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ inline void add_to_ML(int i, int j, double v, int *r_ML, int *c_ML, double *v_ML);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Add a new non-zero coefficient to the P matrix by filling the triplet r_P / c_P / v_P
+ ///
+ /// \param[in] i Row index of the new entry
+ /// \param[in] j Column index of the new entry
+ /// \param[in] v Value of the new entry
+ /// \param[in] r_P Pointer to the table that contains row indexes
+ /// \param[in] c_P Pointer to the table that contains column indexes
+ /// \param[in] v_P Pointer to the table that contains values
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ inline void add_to_P(int i, int j, double v, int *r_P, int *c_P, double *v_P);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create the constraint matrices (M.X = N and L.X <= K)
+ /// Create the weight matrices P and Q (cost 1/2 x^T * P * X + X^T * Q)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int create_matrices();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create the M and L matrices involved in the constraint equations
+ /// the solution has to respect: M.X = N and L.X <= K
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int create_ML();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create the weight matrices P and Q in the cost function
+ /// 1/2 x^T.P.x + x^T.q of the QP problem
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int create_weight_matrices();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute all matrices of the Box QP problem
+ ///
+ /// \param[in] M joint space inertia matrix computed with crba
+ /// \param[in] Jc Jacobian of contact points
+ /// \param[in] f_cmd reference contact forces coming from the MPC
+ /// \param[in] RNEA joint torques according to the current state of the system and the desired joint accelerations
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd,
+ const Eigen::MatrixXd &RNEA);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update P and Q matrices in the cost function xT P x + 2 xT Q
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void update_PQ();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize the solver (first iteration) or update it (next iterations) then call the OSQP solver to solve
+ /// the QP problem
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int call_solver();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Extract relevant information from the output of the QP solver
+ ///
+ /// \param[in] f_cmd Reference contact forces received from the MPC
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int retrieve_result(const Eigen::MatrixXd &f_cmd);
- int cpt_ML = 0;
- int cpt_P = 0;
+ Params *params_; // Object that stores parameters
+
+ int cpt_ML = 0; // Counter of ML coefficients
+ int cpt_P = 0; // Counter of P coefficients
// Set to True after the creation of the QP problem during the first call of the solver
bool initialized = false;
@@ -51,17 +218,17 @@ class QPWBC {
Eigen::Matrix<double, 6, 6> Yinv = Eigen::Matrix<double, 6, 6>::Zero();
Eigen::Matrix<double, 6, 12> A = Eigen::Matrix<double, 6, 12>::Zero();
Eigen::Matrix<double, 6, 1> gamma = Eigen::Matrix<double, 6, 1>::Zero();
- Eigen::Matrix<double, 12, 12> H = Eigen::Matrix<double, 12, 12>::Zero();
+ Eigen::Matrix<double, 12, 12> H = Eigen::Matrix<double, 12, 12>::Zero();
Eigen::Matrix<double, 12, 1> g = Eigen::Matrix<double, 12, 1>::Zero();
// Results
// Eigen::Matrix<double, 12, 1> lambdas = Eigen::Matrix<double, 12, 1>::Zero();
Eigen::MatrixXd f_res = Eigen::MatrixXd::Zero(12, 1);
Eigen::MatrixXd ddq_res = Eigen::MatrixXd::Zero(12, 1);
-
+
// Matrix ML
- const static int size_nz_ML = 20*12; //4 * (4 * 2 + 1);
- csc *ML; // Compressed Sparse Column matrix
+ const static int size_nz_ML = 20 * 12; // 4 * (4 * 2 + 1);
+ csc *ML; // Compressed Sparse Column matrix
// Matrix NK
const static int size_nz_NK = 20;
@@ -70,8 +237,8 @@ class QPWBC {
double v_warmxf[size_nz_NK] = {}; // matrix NK (lower bound)
// Matrix P
- const static int size_nz_P = 6*13; // 6*13; // 12*13/2;
- csc *P; // Compressed Sparse Column matrix
+ const static int size_nz_P = 6 * 13; // 6*13; // 12*13/2;
+ csc *P; // Compressed Sparse Column matrix
// Matrix Q
const static int size_nz_Q = 12;
@@ -81,113 +248,104 @@ class QPWBC {
OSQPWorkspace *workspce = new OSQPWorkspace();
OSQPData *data;
OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
+};
+class WbcWrapper {
public:
-
- QPWBC(); // Constructor
- void initialize(Params& params);
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ WbcWrapper();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~WbcWrapper() {}
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initializer
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params ¶ms);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Run and solve one iteration of the whole body control (matrix update, invkin, QP)
+ ///
+ /// \param[in] q Estimated positions of the 12 actuators
+ /// \param[in] dq Estimated velocities of the 12 actuators
+ /// \param[in] f_cmd Reference contact forces received from the MPC
+ /// \param[in] contacts Contact status of the four feet
+ /// \param[in] pgoals Desired positions of the four feet in base frame
+ /// \param[in] vgoals Desired velocities of the four feet in base frame
+ /// \param[in] agoals Desired accelerations of the four feet in base frame
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void compute(VectorN const &q, VectorN const &dq, MatrixN const &f_cmd, MatrixN const &contacts,
+ MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals);
+
+ VectorN get_qdes() { return qdes_; }
+ VectorN get_vdes() { return vdes_; }
+ VectorN get_tau_ff() { return tau_ff_; }
+ VectorN get_f_with_delta() { return f_with_delta_; }
+ MatrixN get_feet_pos() { return invkin_->get_posf().transpose(); }
+ MatrixN get_feet_err() { return log_feet_pos_target - invkin_->get_posf().transpose(); }
+ MatrixN get_feet_vel() { return invkin_->get_vf().transpose(); }
+ MatrixN get_feet_pos_target() { return log_feet_pos_target; }
+ MatrixN get_feet_vel_target() { return log_feet_vel_target; }
+ MatrixN get_feet_acc_target() { return log_feet_acc_target; }
- // Functions
- inline void add_to_ML(int i, int j, double v, int *r_ML, int *c_ML, double *v_ML); // function to fill the triplet r/c/v
- inline void add_to_P(int i, int j, double v, int *r_P, int *c_P, double *v_P); // function to fill the triplet r/c/v
- int create_matrices();
- int create_ML();
- int create_weight_matrices();
- void compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA);
- void update_PQ();
- int call_solver();
- int retrieve_result(const Eigen::MatrixXd &f_cmd);
- int run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA, const Eigen::MatrixXd &k_contact);
+ private:
+ Params *params_; // Object that stores parameters
+ QPWBC *box_qp_; // QP problem solver for the whole body control
+ InvKin *invkin_; // Inverse Kinematics solver for the whole body control
- // Getters
- Eigen::MatrixXd get_f_res();
- Eigen::MatrixXd get_ddq_res();
- Eigen::MatrixXd get_H();
+ pinocchio::Model model_; // Pinocchio model for frame computations
+ pinocchio::Data data_; // Pinocchio datas for frame computations
- // Utils
- void my_print_csc_matrix(csc *M, const char *name);
- void save_csc_matrix(csc *M, std::string filename);
- void save_dns_matrix(double *M, int size, std::string filename);
+ Eigen::Matrix<double, 18, 18> M_; // Mass matrix
+ Eigen::Matrix<double, 12, 6> Jc_; // Jacobian matrix
+ Matrix14 k_since_contact_; // Number of time step during which feet have been in the current stance phase
+ Vector12 qdes_; // Desired actuator positions
+ Vector12 vdes_; // Desired actuator velocities
+ Vector12 tau_ff_; // Desired actuator torques (feedforward)
-};
+ Vector18 ddq_cmd_; // Actuator accelerations computed by Inverse Kinematics
+ Vector19 q_default_; // Default configuration vector to compute the mass matrix
+ Vector12 f_with_delta_; // Contact forces with deltas found by QP solver
+ Vector18 ddq_with_delta_; // Actuator accelerations with deltas found by QP solver
+
+ Matrix43 posf_tmp_; // Temporary matrix to store posf_ from invkin_
-class WbcWrapper
-{
-public:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Empty constructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- WbcWrapper();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Destructor.
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ~WbcWrapper() {}
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Initializer
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void initialize(Params& params);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void compute(VectorN const& q, VectorN const& dq, MatrixN const& f_cmd, MatrixN const& contacts,
- MatrixN const& pgoals, MatrixN const& vgoals, MatrixN const& agoals);
-
- VectorN get_qdes() { return qdes_; }
- VectorN get_vdes() { return vdes_; }
- VectorN get_tau_ff() { return tau_ff_; }
- VectorN get_f_with_delta() { return f_with_delta_; }
- MatrixN get_feet_pos() { return invkin_->get_posf().transpose(); }
- MatrixN get_feet_err() { return log_feet_pos_target - invkin_->get_posf().transpose(); }
- MatrixN get_feet_vel() { return invkin_->get_vf().transpose(); }
- MatrixN get_feet_pos_target() { return log_feet_pos_target; }
- MatrixN get_feet_vel_target() { return log_feet_vel_target; }
- MatrixN get_feet_acc_target() { return log_feet_acc_target; }
-
-private:
-
- Params* params_; // Object that stores parameters
- QPWBC* box_qp_; // QP problem solver for the whole body control
- InvKin* invkin_; // Inverse Kinematics solver for the whole body control
-
- pinocchio::Model model_; // Pinocchio model for frame computations
- pinocchio::Data data_; // Pinocchio datas for frame computations
-
- Eigen::Matrix<double, 18, 18> M_; // Mass matrix
- Eigen::Matrix<double, 12, 6> Jc_; // Jacobian matrix
- Eigen::Matrix<double, 1, 4> k_since_contact_;
- Vector12 qdes_; // Desired actuator positions
- Vector12 vdes_; // Desired actuator velocities
- Vector12 tau_ff_; // Desired actuator torques (feedforward)
-
- Vector18 ddq_cmd_; // Actuator accelerations computed by Inverse Kinematics
- Vector19 q_default_; // Default configuration vector to compute the mass matrix
- Vector12 f_with_delta_; // Contact forces with deltas found by QP solver
- Vector18 ddq_with_delta_; // Actuator accelerations with deltas found by QP solver
-
- Matrix43 posf_tmp_; // Temporary matrix to store posf_ from invkin_
-
- Matrix34 log_feet_pos_target; // Store the target feet positions
- Matrix34 log_feet_vel_target; // Store the target feet velocities
- Matrix34 log_feet_acc_target; // Store the target feet accelerations
-
- int k_log_; // Counter for logging purpose
- int indexes_[4] = {10, 18, 26, 34}; // Indexes of feet frames in this order: [FL, FR, HL, HR]
+ Matrix34 log_feet_pos_target; // Store the target feet positions
+ Matrix34 log_feet_vel_target; // Store the target feet velocities
+ Matrix34 log_feet_acc_target; // Store the target feet accelerations
+
+ int k_log_; // Counter for logging purpose
+ int indexes_[4] = {10, 18, 26, 34}; // Indexes of feet frames in this order: [FL, FR, HL, HR]
};
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Compute the pseudo inverse of a matrix using the Jacobi SVD formula
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
+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 // QPWBC_H_INCLUDED
diff --git a/include/qrw/StatePlanner.hpp b/include/qrw/StatePlanner.hpp
index a213ca792ea52cdb4f589ff2b5d46474bce66e9a..a1cb084c28068b16513747f2916cd9d843317284 100644
--- a/include/qrw/StatePlanner.hpp
+++ b/include/qrw/StatePlanner.hpp
@@ -2,7 +2,7 @@
///
/// \brief This is the header for StatePlanner class
///
-/// \details Planner that outputs the reference trajectory of the base based on the reference
+/// \details Planner that outputs the reference trajectory of the base based on the reference
/// velocity given by the user and the current position/velocity of the base
///
//////////////////////////////////////////////////////////////////////////////////////////////////
@@ -14,63 +14,62 @@
#include "qrw/Params.hpp"
#include "qrw/Types.h"
+class StatePlanner {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ StatePlanner();
-class StatePlanner
-{
-public:
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Empty constructor
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- StatePlanner();
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initializer
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Destructor.
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ~StatePlanner() {}
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~StatePlanner() {}
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Initializer
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void initialize(Params& params);
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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 horizontal frame (linear and angular stacked)
+ /// \param[in] v current velocity vector of the flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] z_average average height of feet currently in stance phase
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average);
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \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 horizontal frame (linear and angular stacked)
- /// \param[in] v current velocity vector of the flying base in horizontal frame (linear and angular stacked)
- /// \param[in] vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
- /// \param[in] z_average average height of feet currently in stance phase
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average);
+ MatrixN getReferenceStates() { return referenceStates_; }
+ int getNSteps() { return n_steps_; }
- MatrixN getReferenceStates() { return referenceStates_; }
- int getNSteps() { return n_steps_; }
+ private:
+ double dt_; // Time step of the contact sequence (time step of the MPC)
+ double h_ref_; // Reference height for the trunk
+ int n_steps_; // Number of time steps in the prediction horizon
-private:
- double dt_; // Time step of the contact sequence (time step of the MPC)
- double h_ref_; // Reference height for the trunk
- int n_steps_; // Number of time steps in the prediction horizon
+ Vector3 RPY_; // To store roll, pitch and yaw angles
- Vector3 RPY_; // To store roll, pitch and yaw angles
-
- // 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 referenceStates_;
-
- VectorN dt_vector_; // Vector containing all time steps in the prediction horizon
+ // 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 referenceStates_;
+ VectorN dt_vector_; // Vector containing all time steps in the prediction horizon
};
#endif // STATEPLANNER_H_INCLUDED
diff --git a/include/qrw/Types.h b/include/qrw/Types.h
index 553d6fd853c7f0f9bcf4e79a01b0476605615af5..8a536853704d08da24da9022d70dbe7d36219e86 100644
--- a/include/qrw/Types.h
+++ b/include/qrw/Types.h
@@ -1,7 +1,6 @@
#ifndef TYPES_H_INCLUDED
#define TYPES_H_INCLUDED
-#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index 48b397b4bd187c5730e9646c8b612c727cd1dfa0..6030d8c346d249d9d43b6e30cfa32644689f5bfb 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -11,7 +11,6 @@ FootstepPlanner::FootstepPlanner()
, yaws()
, dx()
, dy()
- , q_tmp(Vector3::Zero())
, q_dxdy(Vector3::Zero())
, RPY_(Vector3::Zero())
, pos_feet_(Matrix34::Zero())
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index d3164d51c64d64204b0cc16de7c0c4c6ed39b62e..699624067a8169fd171fc9fc62821c1169793a43 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -8,10 +8,6 @@ InvKin::InvKin()
, pfeet_err(Matrix43::Zero())
, vfeet_ref(Matrix43::Zero())
, afeet(Matrix43::Zero())
- , e_basispos(Matrix13::Zero())
- , abasis(Matrix13::Zero())
- , e_basisrot(Matrix13::Zero())
- , awbasis(Matrix13::Zero())
, posf_(Matrix43::Zero())
, vf_(Matrix43::Zero())
, wf_(Matrix43::Zero())
diff --git a/src/MPC.cpp b/src/MPC.cpp
index 83b274d7d3b1cdb7c41d4a5cdc16b2ed65e7d155..a37b6b4761145d49d275300a23b8ed0c3d4f0f1a 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -39,10 +39,6 @@ MPC::MPC(Params& params) {
MPC::MPC() { }
-/*
-Create the constraint matrices of the MPC (M.X = N and L.X <= K)
-Create the weight matrices P and Q of the MPC solver (cost 1/2 x^T * P * X + X^T * Q)
-*/
int MPC::create_matrices() {
// Create the constraint matrices
create_ML();
@@ -54,9 +50,6 @@ int MPC::create_matrices() {
return 0;
}
-/*
-Add a new non-zero coefficient to the ML matrix by filling the triplet r_ML / c_ML / v_ML
-*/
inline void MPC::add_to_ML(int i, int j, double v, int *r_ML, int *c_ML, double *v_ML) {
r_ML[cpt_ML] = i; // row index
c_ML[cpt_ML] = j; // column index
@@ -64,9 +57,6 @@ inline void MPC::add_to_ML(int i, int j, double v, int *r_ML, int *c_ML, double
cpt_ML++; // increment the counter
}
-/*
-Add a new non-zero coefficient to the P matrix by filling the triplet r_P / c_P / v_P
-*/
inline void MPC::add_to_P(int i, int j, double v, int *r_P, int *c_P, double *v_P) {
r_P[cpt_P] = i; // row index
c_P[cpt_P] = j; // column index
@@ -74,9 +64,6 @@ inline void MPC::add_to_P(int i, int j, double v, int *r_P, int *c_P, double *v_
cpt_P++; // increment the counter
}
-/*
-Create the M and L matrices involved in the MPC constraint equations M.X = N and L.X <= K
-*/
int MPC::create_ML() {
int *r_ML = new int[size_nz_ML]; // row indexes of non-zero values in matrix ML
int *c_ML = new int[size_nz_ML]; // col indexes of non-zero values in matrix ML
@@ -261,9 +248,6 @@ int MPC::create_ML() {
return 0;
}
-/*
-Create the N and K matrices involved in the MPC constraint equations M.X = N and L.X <= K
-*/
int MPC::create_NK() {
// Create NK matrix (upper and lower bounds)
NK_up = Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(12 * n_steps * 2 + 20 * n_steps, 1);
@@ -317,9 +301,6 @@ int MPC::create_NK() {
return 0;
}
-/*
-Create the weight matrices P and q in the cost function x^T.P.x + x^T.q of the QP problem
-*/
int MPC::create_weight_matrices() {
int *r_P = new int[size_nz_P]; // row indexes of non-zero values in matrix P
int *c_P = new int[size_nz_P]; // col indexes of non-zero values in matrix P
@@ -396,9 +377,6 @@ int MPC::create_weight_matrices() {
return 0;
}
-/*
-Update the M, N, L and K constraint matrices depending on what happened
-*/
int MPC::update_matrices(Eigen::MatrixXd fsteps) {
/* M need to be updated between each iteration:
- lever_arms changes since the robot moves
@@ -417,10 +395,6 @@ int MPC::update_matrices(Eigen::MatrixXd fsteps) {
return 0;
}
-/*
-Update the M and L constaint matrices depending on the current state of the gait
-
-*/
int MPC::update_ML(Eigen::MatrixXd fsteps) {
int j = 0;
int k_cum = 0;
@@ -469,9 +443,6 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
return 0;
}
-/*
-Update the N and K matrices involved in the MPC constraint equations M.X = N and L.X <= K
-*/
int MPC::update_NK() {
// Matrix g is already created and not changed
@@ -501,9 +472,6 @@ int MPC::update_NK() {
return 0;
}
-/*
-Create an initial guess and call the solver to solve the QP problem
-*/
int MPC::call_solver(int k) {
// Initial guess for forces (mass evenly supported by all legs in contact)
warmxf.block(0, 0, 12 * (n_steps - 1), 1) = x.block(12, 0, 12 * (n_steps - 1), 1);
@@ -569,9 +537,6 @@ int MPC::call_solver(int k) {
return 0;
}
-/*
-Extract relevant information from the output of the QP solver
-*/
int MPC::retrieve_result() {
// Retrieve the "contact forces" part of the solution of the QP problem
for (int i = 0; i < (n_steps); i++) {
@@ -614,21 +579,6 @@ Return the next predicted state of the base
*/
double *MPC::get_x_next() { return x_next; }
-/*
-Run function with arrays as input for compatibility between Python and C++
-*/
-void MPC::run_python(const matXd &xref_py, const matXd &fsteps_py) {
- printf("Trigger bindings \n");
- printf("xref: %f %f %f \n", xref_py(0, 0), xref_py(1, 0), xref_py(2, 0));
- printf("fsteps: %f %f %f \n", fsteps_py(0, 0), fsteps_py(1, 0), fsteps_py(2, 0));
-
- return;
-}
-
-/*
-Run one iteration of the whole MPC by calling all the necessary functions (data retrieval,
-update of constraint matrices, update of the solver, running the solver, retrieving result)
-*/
int MPC::run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in) {
// Recontruct the gait based on the computed footsteps
construct_gait(fsteps_in);
@@ -654,20 +604,12 @@ int MPC::run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd
return 0;
}
-/*
-Returns the skew matrix of a 3 by 1 column vector
-*/
-Eigen::Matrix<double, 3, 3> MPC::getSkew(Eigen::Matrix<double, 3, 1> v) {
+Matrix3 MPC::getSkew(Vector3 v) {
Eigen::Matrix<double, 3, 3> result;
result << 0.0, -v(2, 0), v(1, 0), v(2, 0), 0.0, -v(0, 0), -v(1, 0), v(0, 0), 0.0;
return result;
}
-/*
-Construct an array of size 12*N that contains information about the contact state of feet.
-This matrix is used to enable/disable contact forces in the QP problem.
-N is the number of time step in the prediction horizon.
-*/
int MPC::construct_S() {
int i = 0;
@@ -686,9 +628,6 @@ int MPC::construct_S() {
return 0;
}
-/*
-Reconstruct the gait matrix based on the fsteps matrix since only the last one is received by the MPC
-*/
int MPC::construct_gait(Eigen::MatrixXd fsteps_in) {
int k = 0;
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 563aeb2461d1fd26115ce71912fbca21cb343b0c..b2ab70a0a72ba143290c85767e1f01de0dba296d 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -36,10 +36,6 @@ void QPWBC::initialize(Params& params) {
}
int QPWBC::create_matrices() {
- /*
- Create the constraint matrices (M.X = N and L.X <= K)
- Create the weight matrices P and Q (cost 1/2 x^T * P * X + X^T * Q)
- */
// Create the constraint matrices
create_ML();
@@ -51,18 +47,6 @@ int QPWBC::create_matrices() {
}
inline void QPWBC::add_to_ML(int i, int j, double v, int *r_ML, int *c_ML, double *v_ML) {
- /*
- Add a new non-zero coefficient to the ML matrix by filling the triplet r_ML / c_ML / v_ML
-
- Args:
- - i (int): row index of the new entry
- - j (int): column index of the new entry
- - v (double): value of the new entry
- - r_ML (int*): table that contains row indexes
- - c_ML (int*): table that contains column indexes
- - v_ML (double*): table that contains values
- */
-
r_ML[cpt_ML] = i; // row index
c_ML[cpt_ML] = j; // column index
v_ML[cpt_ML] = v; // value of coefficient
@@ -70,18 +54,6 @@ inline void QPWBC::add_to_ML(int i, int j, double v, int *r_ML, int *c_ML, doubl
}
inline void QPWBC::add_to_P(int i, int j, double v, int *r_P, int *c_P, double *v_P) {
- /*
- Add a new non-zero coefficient to the P matrix by filling the triplet r_P / c_P / v_P
-
- Args:
- - i (int): row index of the new entry
- - j (int): column index of the new entry
- - v (double): value of the new entry
- - r_P (int*): table that contains row indexes
- - c_P (int*): table that contains column indexes
- - v_P (double*): table that contains values
- */
-
r_P[cpt_P] = i; // row index
c_P[cpt_P] = j; // column index
v_P[cpt_P] = v; // value of coefficient
@@ -89,10 +61,6 @@ inline void QPWBC::add_to_P(int i, int j, double v, int *r_P, int *c_P, double *
}
int QPWBC::create_ML() {
- /*
- Create the M and L matrices involved in the constraint equations
- the solution has to respect: M.X = N and L.X <= K
- */
int *r_ML = new int[size_nz_ML]; // row indexes of non-zero values in matrix ML
int *c_ML = new int[size_nz_ML]; // col indexes of non-zero values in matrix ML
@@ -155,10 +123,6 @@ int QPWBC::create_ML() {
int QPWBC::create_weight_matrices() {
- /*
- Create the weight matrices P and Q in the cost function
- 1/2 x^T.P.x + x^T.q of the QP problem
- */
int *r_P = new int[size_nz_P]; // row indexes of non-zero values in matrix P
int *c_P = new int[size_nz_P]; // col indexes of non-zero values in matrix P
@@ -217,10 +181,6 @@ int QPWBC::create_weight_matrices() {
}
int QPWBC::call_solver() {
- /*
- Initialize the solver (first iteration) or update it (next iterations)
- then call the OSQP solver to solve the QP problem
- */
// Setup the solver (first iteration) then just update it
if (not initialized) // Setup the solver with the matrices
@@ -281,12 +241,6 @@ int QPWBC::call_solver() {
}
int QPWBC::retrieve_result(const Eigen::MatrixXd &f_cmd) {
- /*
- Extract relevant information from the output of the QP solver
-
- Args:
- - f_cmd (Eigen::MatrixXd): reference contact forces received from the MPC
- */
// Retrieve the solution of the QP problem
for (int k = 0; k < 12; k++) {
@@ -315,17 +269,6 @@ Eigen::MatrixXd QPWBC::get_H() {
int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA,
const Eigen::MatrixXd &k_contact) {
- /*
- Run one iteration of the whole WBC QP problem by calling all the necessary functions (data retrieval,
- update of constraint matrices, update of the solver, running the solver, retrieving result)
-
- Args:
- - M (Eigen::MatrixXd): joint space inertia matrix computed with crba
- - Jc (Eigen::MatrixXd): Jacobian of contact points
- - f_cmd (Eigen::MatrixXd): reference contact forces coming from the MPC
- - RNEA (Eigen::MatrixXd): joint torques according to the current state of the system and the desired joint accelerations
- - k_contact (Eigen::MatrixXd): nb of iterations since contact has been enabled for each foot
- */
// Create the constraint and weight matrices used by the QP solver
// Minimize x^T.P.x + 2 x^T.Q with constraints M.X == N and L.X <= K
@@ -395,13 +338,6 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
}
void QPWBC::my_print_csc_matrix(csc *M, const char *name) {
- /*
- Print positions and value of coefficients in a csc matrix
-
- Args:
- - M (csc*): pointer to the csc matrix you want to print
- - name (char*): name that should be displayed for the matrix (one char)
- */
c_int j, i, row_start, row_stop;
c_int k = 0;
@@ -428,13 +364,6 @@ void QPWBC::my_print_csc_matrix(csc *M, const char *name) {
}
void QPWBC::save_csc_matrix(csc *M, std::string filename) {
- /*
- Save positions and value of coefficients of a csc matrix in a csc file
-
- Args:
- - M (csc*): pointer to the csc matrix you want to save
- - filename (string): name of the generated csv file
- */
c_int j, i, row_start, row_stop;
c_int k = 0;
@@ -462,14 +391,6 @@ void QPWBC::save_csc_matrix(csc *M, std::string filename) {
}
void QPWBC::save_dns_matrix(double *M, int size, std::string filename) {
- /*
- Save positions and value of coefficients of a dense matrix in a csc file
-
- Args:
- - M (double*): pointer to the dense matrix you want to save
- - size (int): size of the dense matrix
- - filename (string): name of the generated csv file
- */
// Open file
std::ofstream myfile;
@@ -484,15 +405,6 @@ void QPWBC::save_dns_matrix(double *M, int size, std::string filename) {
void QPWBC::compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA) {
- /*
- Compute all matrices of the Box QP problem
-
- Args:
- - M (Eigen::MatrixXd): joint space inertia matrix computed with crba
- - Jc (Eigen::MatrixXd): Jacobian of contact points
- - f_cmd (Eigen::MatrixXd): reference contact forces coming from the MPC
- - RNEA (Eigen::MatrixXd): joint torques according to the current state of the system and the desired joint accelerations
- */
Y = M.block(0, 0, 6, 6);
X = Jc.block(0, 0, 12, 6).transpose();
@@ -523,9 +435,6 @@ void QPWBC::compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc
}
void QPWBC::update_PQ() {
- /*
- Update P and Q matrices in the cost function xT P x + 2 xT Q
- */
// Update P matrix of min xT P x + 2 xT Q
int cpt = 0;
diff --git a/src/config_solo12.yaml b/src/config_solo12.yaml
index 6b97790723444b05b7ea1ec5c65df0c1e2643275..1a8abaeddb19001b225a6eeb9d6d439305eee13a 100644
--- a/src/config_solo12.yaml
+++ b/src/config_solo12.yaml
@@ -1,6 +1,6 @@
robot:
# General parameters
- interface: enp2s0
+ interface: enp2s0 # Name of the communication interface (check with ifconfig)
SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
LOGGING: false # Enable/disable logging during the experiment
PLOTTING: true # Enable/disable automatic plotting at the end of the experiment