From cad1e0537d77ab890ec77bd48c73058d79dbf625 Mon Sep 17 00:00:00 2001
From: thomascbrs <thomas.corberes@laposte.net>
Date: Mon, 25 Oct 2021 01:26:57 +0100
Subject: [PATCH] solo3D integration
---
CMakeLists.txt | 19 +-
include/qrw/Estimator.hpp | 3 +-
include/qrw/FootTrajectoryGeneratorBezier.hpp | 188 +++++
include/qrw/FootstepPlannerQP.hpp | 272 ++++++++
include/qrw/Heightmap.hpp | 131 ++++
include/qrw/Params.hpp | 6 +
include/qrw/StatePlanner3D.hpp | 97 +++
include/qrw/Surface.hpp | 67 ++
include/qrw/Types.h | 1 +
python/gepadd.cpp | 192 +++++-
scripts/Controller.py | 267 +++++++-
scripts/main_solo12_control.py | 14 +-
scripts/solo3D/StatePlanner.py | 172 +++++
scripts/solo3D/SurfacePlanner.py | 270 ++++++++
scripts/solo3D/SurfacePlannerWrapper.py | 328 +++++++++
scripts/solo3D/tools/Surface.py | 279 ++++++++
scripts/solo3D/tools/heightmap_generator.py | 67 ++
scripts/solo3D/tools/heightmap_tools.py | 173 +++++
scripts/solo3D/tools/pyb_environment_3D.py | 208 ++++++
scripts/solo3D/tools/readme.txt | 13 +
scripts/solo3D/tools/utils.py | 74 ++
src/Estimator.cpp | 28 +-
src/FootTrajectoryGeneratorBezier.cpp | 648 ++++++++++++++++++
src/FootstepPlannerQP.cpp | 587 ++++++++++++++++
src/Heightmap.cpp | 95 +++
src/Params.cpp | 12 +
src/StatePlanner3D.cpp | 130 ++++
src/Surface.cpp | 37 +
src/walk_parameters.yaml | 15 +-
29 files changed, 4316 insertions(+), 77 deletions(-)
create mode 100644 include/qrw/FootTrajectoryGeneratorBezier.hpp
create mode 100644 include/qrw/FootstepPlannerQP.hpp
create mode 100644 include/qrw/Heightmap.hpp
create mode 100644 include/qrw/StatePlanner3D.hpp
create mode 100644 include/qrw/Surface.hpp
create mode 100644 scripts/solo3D/StatePlanner.py
create mode 100644 scripts/solo3D/SurfacePlanner.py
create mode 100644 scripts/solo3D/SurfacePlannerWrapper.py
create mode 100644 scripts/solo3D/tools/Surface.py
create mode 100644 scripts/solo3D/tools/heightmap_generator.py
create mode 100644 scripts/solo3D/tools/heightmap_tools.py
create mode 100644 scripts/solo3D/tools/pyb_environment_3D.py
create mode 100644 scripts/solo3D/tools/readme.txt
create mode 100644 scripts/solo3D/tools/utils.py
create mode 100644 src/FootTrajectoryGeneratorBezier.cpp
create mode 100644 src/FootstepPlannerQP.cpp
create mode 100644 src/Heightmap.cpp
create mode 100644 src/StatePlanner3D.cpp
create mode 100644 src/Surface.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9104b5d0..4b8f3529 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,11 +48,15 @@ SEARCH_FOR_BOOST()
# Main Library
set(${PROJECT_NAME}_HEADERS
include/qrw/gepadd.hpp
+ include/qrw/Heightmap.hpp
include/qrw/MPC.hpp
include/qrw/Gait.hpp
include/qrw/FootTrajectoryGenerator.hpp
+ include/qrw/FootTrajectoryGeneratorBezier.hpp
include/qrw/FootstepPlanner.hpp
+ include/qrw/FootstepPlannerQP.hpp
include/qrw/StatePlanner.hpp
+ include/qrw/StatePlanner3D.hpp
include/qrw/Types.h
include/qrw/InvKin.hpp
include/qrw/QPWBC.hpp
@@ -64,16 +68,23 @@ set(${PROJECT_NAME}_HEADERS
include/qrw/MpcWrapper.hpp
include/qrw/FakeRobot.hpp
include/other/st_to_cc.hpp
+ include/qrw/FootTrajectoryGeneratorBezier.hpp
+ include/qrw/FootstepPlannerQP.hpp
+ include/qrw/Surface.hpp
)
set(${PROJECT_NAME}_SOURCES
src/st_to_cc.cpp
src/gepadd.cpp
+ src/Heightmap.cpp
src/MPC.cpp
src/Gait.cpp
src/FootTrajectoryGenerator.cpp
+ src/FootTrajectoryGeneratorBezier.cpp
src/FootstepPlanner.cpp
+ src/FootstepPlannerQP.cpp
src/StatePlanner.cpp
+ src/StatePlanner3D.cpp
src/InvKin.cpp
src/QPWBC.cpp
src/Params.cpp
@@ -82,10 +93,14 @@ set(${PROJECT_NAME}_SOURCES
src/Filter.cpp
src/Controller.cpp
src/MpcWrapper.cpp
+ src/FootTrajectoryGeneratorBezier.cpp
+ src/FootstepPlannerQP.cpp
+ src/Surface.cpp
)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
target_include_directories(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
+target_include_directories(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include/qrw>)
#Â Include Eigen3 directories
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
@@ -105,10 +120,10 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC odri_control_interface::odri_contro
TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC master_board_sdk::master_board_sdk)
# Find parametric curves library and headers
-# add_project_dependency(curves REQUIRED)
+add_project_dependency(ndcurves REQUIRED)
# Link parametric curves library
-# target_link_libraries(${PROJECT_NAME} PUBLIC curves::curves)
+target_link_libraries(${PROJECT_NAME} PUBLIC ndcurves::ndcurves)
# Find eiquadprog library and headers
add_project_dependency(eiquadprog REQUIRED)
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 718e2e87..74bed7a9 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -106,7 +106,7 @@ class Estimator {
///
////////////////////////////////////////////////////////////////////////////////////////////////
void get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
- Vector3 const& baseOrientation);
+ Vector3 const& baseOrientation, VectorN const& dummyPos);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -229,6 +229,7 @@ class Estimator {
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)
+ bool solo3D; // Perfect estimator including yaw angle
int N_SIMULATION; // Number of loops before the control ends
int k_log_; // Number of time the estimator has been called
diff --git a/include/qrw/FootTrajectoryGeneratorBezier.hpp b/include/qrw/FootTrajectoryGeneratorBezier.hpp
new file mode 100644
index 00000000..ad5b1a9b
--- /dev/null
+++ b/include/qrw/FootTrajectoryGeneratorBezier.hpp
@@ -0,0 +1,188 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for FootTrajectoryGenerator class
+///
+/// \details This class generates a reference trajectory for the swing foot, in position, velocity
+/// and acceleration
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef TRAJGEN_BEZIER_H_INCLUDED
+#define TRAJGEN_BEZIER_H_INCLUDED
+
+#include "qrw/Gait.hpp"
+#include "qrw/Surface.hpp"
+#include "qrw/Types.h"
+#include "qrw/Params.hpp"
+
+#include "ndcurves/fwd.h"
+#include "ndcurves/bezier_curve.h"
+#include "ndcurves/optimization/details.h"
+#include <Eigen/Dense>
+#include <vector>
+
+#include "eiquadprog/eiquadprog-fast.hpp"
+
+using namespace ndcurves;
+using namespace eiquadprog::solvers;
+
+typedef std::vector<Surface> SurfaceVector;
+
+class FootTrajectoryGeneratorBezier {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ FootTrajectoryGeneratorBezier();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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, Surface initialSurface_in, double x_margin_max_in, double t_margin_in,
+ double z_margin_in, int N_samples_in, int N_samples_ineq_in, int degree_in);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~FootTrajectoryGeneratorBezier() {} // 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& k, int const& i_foot, 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, SurfaceVector const& surfacesSelected,
+ MatrixN const& currentPosition);
+
+ void updatePolyCoeff_XY(int const& i_foot, Vector3 const& x_init, Vector3 const& v_init, Vector3 const& a_init,
+ Vector3 const& x_target, double const& t0, double const& t1);
+ void updatePolyCoeff_Z(int const& i_foot, Vector3 const& x_init, Vector3 const& x_target, double const& t1,
+ double const& h);
+ Vector3 evaluatePoly(int const& i_foot, int const& indice, double const& t);
+ Vector3 evaluateBezier(int const& i_foot, int const& indice, double const& t);
+
+ 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,
+ const Eigen::Matrix<double, 3, 1>& a_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
+ MatrixN getFootJerk() { return jerk_; } // Get the next foot jerk
+ Vector4 get_t0s() { return t0s; }
+ Vector4 get_t_swing() { return t_swing; }
+
+ 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
+
+ std::vector<int> feet;
+ Vector4 t0s;
+ Vector4 t0_bezier;
+ Vector4 t_stop;
+ Vector4 t_swing;
+
+ Matrix34 targetFootstep_; // Target for the X component
+
+ Matrix64 Ax; ///< Coefficients for the X component
+ Matrix64 Ay; ///< Coefficients for the Y component
+ Matrix74 Az; ///< Coefficients for the Z component
+
+ Matrix34 position_; // position computed in updateFootPosition
+ Matrix34 velocity_; // velocity computed in updateFootPosition
+ Matrix34 acceleration_; // acceleration computed in updateFootPosition
+ Matrix34 jerk_; // Jerk 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
+
+ Vector2 intersectionPoint_; // tmp point of intersection
+
+ SurfaceVector newSurface_;
+ SurfaceVector pastSurface_;
+
+ typedef optimization::problem_definition<pointX_t, double> problem_definition_t;
+ typedef optimization::problem_data<pointX_t, double> problem_data_t;
+ static const bool safe = true;
+
+ // Number of points in the least square problem
+ int N_samples;
+ int N_samples_ineq;
+ // dimension of our problem (here 3 as our curve is 3D)
+ int dim = 3;
+ // Degree of the Bezier curve to match the polys
+ int degree;
+ // Size of the optimised vector in the least square (6 = nb of initial/final conditions)
+ // = dim*(degree + 1 - 6)
+ int res_size;
+
+ // Vector of Problem Definition, containing Initial/Final conditions and flag for conditions.
+ std::vector<problem_definition_t> pDefs;
+
+ // Vector of Bezier curves, containing the curves optimised for each foot
+ std::vector<bezier_t> fitBeziers;
+
+ // QP matrix
+ MatrixN P_;
+ VectorN q_;
+
+ MatrixN G_;
+ VectorN h_;
+
+ MatrixN C_;
+ VectorN d_;
+
+ VectorN x;
+
+ std::vector<Vector3> ineq_;
+ Vector4 ineq_vector_;
+ Vector4 x_margin_;
+ double x_margin_max_; // margin around the obstacle
+ double t_margin_; // % of the curve after critical point
+ double z_margin_; // % of the height of the obstacle around the critical point
+
+ // QP solver
+ EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
+ EiquadprogFast_status status;
+
+ EiquadprogFast qp;
+
+ // Methods to compute intersection point
+ bool doIntersect_segment(Vector2 const& p1, Vector2 const& q1, Vector2 const& p2, Vector2 const& q2);
+ bool onSegment(Vector2 const& p, Vector2 const& q, Vector2 const& r);
+ int orientation(Vector2 const& p, Vector2 const& q, Vector2 const& r);
+ void get_intersect_segment(Vector2 a1, Vector2 a2, Vector2 b1, Vector2 b2);
+};
+#endif // TRAJGEN_H_INCLUDED
diff --git a/include/qrw/FootstepPlannerQP.hpp b/include/qrw/FootstepPlannerQP.hpp
new file mode 100644
index 00000000..b3e53904
--- /dev/null
+++ b/include/qrw/FootstepPlannerQP.hpp
@@ -0,0 +1,272 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for FootstepPlanner class
+///
+/// \details Planner that outputs current and future locations of footsteps, the reference
+/// trajectory of the base based on the reference velocity given by the user and the current
+/// position/velocity of the base
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef FOOTSTEPPLANNERQP_H_INCLUDED
+#define FOOTSTEPPLANNERQP_H_INCLUDED
+
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+#include "qrw/Gait.hpp"
+#include "qrw/Params.hpp"
+#include "qrw/Surface.hpp"
+#include "qrw/Types.h"
+#include <vector>
+#include "eiquadprog/eiquadprog-fast.hpp"
+
+// Order of feet/legs: FL, FR, HL, HR
+
+using namespace eiquadprog::solvers;
+typedef std::vector<Surface> SurfaceVector;
+typedef std::vector<std::vector<Surface>> SurfaceVectorVector;
+
+struct optimData
+{
+ int phase;
+ int foot;
+ Surface surface;
+ Vector3 next_pos;
+};
+
+class FootstepPlannerQP {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ FootstepPlannerQP();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Default constructor
+ ///
+ /// \param[in] params Object that stores parameters
+ /// \param[in] gaitIn Gait object to hold the gait informations
+ /// \param[in] initialSurface_in Surface of the floor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params, Gait& gaitIn, Surface initialSurface_in);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~FootstepPlannerQP() {}
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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,
+ SurfaceVectorVector const& potentialSurfaces, SurfaceVector const& surfaces,
+ bool const surfaceStatus, int const surfaceIteration);
+
+ 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,
+ SurfaceVectorVector const& potentialSurfaces, SurfaceVector const& surfaces,
+ bool const surfaceStatus, int const surfaceIteration);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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)
+ /// \param[in] footstep Vector x3 to update with the next foostep position of foot j, gait row i
+ /// \param[in] feedback_term Boolean if the feedback term is taken into account
+ ///
+ /// \retval Vector of the next footstep position for foot j, gait index i
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void computeNextFootstep(int i, int j, Vector6 const& b_v, Vector6 const& b_vref, Vector3& footstep,
+ bool feedback_term);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update desired location of footsteps using information coming from the footsteps planner
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateTargetFootsteps();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Select a surface from a point in 2D inside potential surfaces
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Surface selectSurfaceFromPoint(Vector3 const& point, int phase, int moving_foot_index);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the remaining timing of the flying phase
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void update_remaining_time(int k);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the QP problem with the surface object
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int surfaceInequalities(int i_start, Surface const& surface, Vector3 const& next_ft, int id_foot);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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 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 footsteps_offset_;
+ Matrix34 currentFootstep_; // Feet matrix
+ Vector3 next_footstep_; // Temporary vector to perform computations
+ Vector3 next_footstep_qp_; // Temporary vector 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
+ std::vector<Matrix34> b_footsteps_; // Desired footsteps locations for each step of the horizon in base frame
+
+ MatrixN Rz; // Rotation matrix along z axis
+ MatrixN Rz_tmp; // Temporary rotation matrix along z axis
+ VectorN dt_cum; // Cumulated time vector
+ VectorN yaws; // Predicted yaw variation for each cumulated time
+ VectorN yaws_b; // Predicted yaw variation for each cumulated time in base frame
+ 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 q_tmp; // Temporary storage variable for position of the base in the world
+ 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_;
+
+ int k_mpc;
+ // QP problem :
+ const int N = 14; // Number of variables vx, vy, p1, p2, p3
+ const int M = 6 * 4; // Number of constraints inequalities
+
+ std::vector<int> feet_;
+ Vector4 t0s;
+ Vector4 t_swing;
+
+ VectorN weights_;
+ Vector3 voptim_;
+ Vector6 b_voptim_;
+
+ // min. 1/2 * x' C_ x + q_' x
+ // s.t. C_ x + d_ = 0
+ // G_ x + h_ >= 0
+
+ // Weight Matrix
+ MatrixN P_;
+ VectorN q_;
+
+ MatrixN G_;
+ VectorN h_;
+
+ MatrixN C_;
+ VectorN d_;
+
+
+ // qp solver
+ EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
+ EiquadprogFast_status status;
+ VectorN x;
+ EiquadprogFast qp;
+
+ bool surfaceStatus_;
+ int surfaceIteration_;
+ SurfaceVector surfaces_;
+ SurfaceVectorVector potentialSurfaces_;
+ Surface initialSurface_;
+
+ std::vector<optimData> optimVector_;
+ SurfaceVector selectedSurfaces_;
+};
+
+#endif // FOOTSTEPPLANNERQP_H_INCLUDED
diff --git a/include/qrw/Heightmap.hpp b/include/qrw/Heightmap.hpp
new file mode 100644
index 00000000..96ab9032
--- /dev/null
+++ b/include/qrw/Heightmap.hpp
@@ -0,0 +1,131 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Heightmap class
+///
+/// \details This class loads a binary file, return the height and compute surface of the heightmap
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef HEIGHTMAP_H_INCLUDED
+#define HEIGHTMAP_H_INCLUDED
+
+#include <stdio.h>
+#include <fstream>
+#include <iostream>
+#include "qrw/Types.h"
+#include "qrw/Params.hpp"
+#include <Eigen/Dense>
+#include "eiquadprog/eiquadprog-fast.hpp"
+
+using namespace std;
+
+struct Header {
+ int size_x;
+ int size_y;
+ double x_init;
+ double x_end;
+ double y_init;
+ double y_end;
+};
+
+class Heightmap {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Heightmap();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize with given data
+ ///
+ /// \param[in] file_name name of the binary file containing the data of the heightmap
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(const std::string& file_name);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Heightmap() {} // Empty constructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Get the i indice of a given x-axis position in the heightmap
+ ///
+ /// \param[in] x x-axis position
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int map_x(double x);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Get the j indice of a given y-axis position in the heightmap
+ ///
+ /// \param[in] y y-axis position
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ int map_y(double y);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Get height given a 2D position
+ ///
+ /// \param[in] x x-axis position
+ /// \param[in] y y-axis position
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ double get_height(double x, double y);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the surface equation to fit the heightmap, [a,b,c] such as ax + by -z +c = 0
+ /// for a given 2D position
+ ///
+ /// \param[in] x x-axis position
+ /// \param[in] y y-axis position
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void update_mean_surface(double x, double y);
+
+ MatrixN z_;
+ MatrixN x_;
+ MatrixN y_;
+ VectorN surface_eq = VectorN::Zero(3); // [a,b,c], such as ax + by -z + c = 0
+
+ private:
+ Header header_; // Contain the size and parameters of the heightmap
+
+ double dx_; // interval size x-axis
+ double dy_; // interval size y-axis
+
+ double FIT_SIZE_X; // size around x-axis the robot to detect the surface
+ double FIT_SIZE_Y; // size around x-axis the robot to detect the surface
+ int FIT_NX; // Number of point for the QP, to compute the surface, x-axis
+ int FIT_NY; // Number of point for the QP, to compute the surface, y-axis
+
+ // min. 1/2 * x' C_ x + q_' x
+ // s.t. C_ x + d_ = 0
+ // G_ x + h_ >= 0
+ MatrixN P_= MatrixN::Zero(3, 3);
+ VectorN q_= VectorN::Zero(3);
+
+ MatrixN G_= MatrixN::Zero(3, 3);
+ VectorN h_= VectorN::Zero(3);
+
+ MatrixN C_= MatrixN::Zero(3, 3);
+ VectorN d_= VectorN::Zero(3);
+
+ // qp solver
+ eiquadprog::solvers::EiquadprogFast_status expected = eiquadprog::solvers::EIQUADPROG_FAST_OPTIMAL;
+ eiquadprog::solvers::EiquadprogFast_status status;
+ eiquadprog::solvers::EiquadprogFast qp;
+
+ MatrixN A;
+ VectorN b;
+};
+#endif // HEIGHTMAP_H_INCLUDED
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index c542053d..c9366e95 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -115,6 +115,12 @@ class Params {
double Fz_max; // Maximum vertical contact force [N]
double Fz_min; // Minimal vertical contact force [N]
+ // Parameters of MIP
+ bool solo3D; // Enable the 3D environment with corresponding planner blocks
+ bool enable_multiprocessing_mip; // Enable/disable running the MIP in another process in parallel of the main loop
+ std::string environment_URDF; // URDF path for the 3D environment
+ std::string environment_heightmap; // path to the heightmap
+
// Not defined in yaml
Eigen::MatrixXd gait; // Initial gait matrix (Eigen)
double T_gait; // Period of the gait
diff --git a/include/qrw/StatePlanner3D.hpp b/include/qrw/StatePlanner3D.hpp
new file mode 100644
index 00000000..f1c1775d
--- /dev/null
+++ b/include/qrw/StatePlanner3D.hpp
@@ -0,0 +1,97 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for StatePlanner 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
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef STATEPLANNER3D_H_INCLUDED
+#define STATEPLANNER3D_H_INCLUDED
+
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "qrw/Params.hpp"
+#include "qrw/Types.h"
+#include "qrw/Heightmap.hpp"
+#include <vector>
+
+class StatePlanner3D {
+ public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ StatePlanner3D();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initializer
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~StatePlanner3D() {}
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \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, int is_new_step);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute references configuration for MIP planner
+ ///
+ /// \param[in] q current position 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)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void compute_configurations(VectorN const& q, Vector6 const& vref);
+
+ MatrixN getReferenceStates() { return referenceStates_; }
+ int getNSteps() { return n_steps_; }
+ MatrixN get_configurations() { return configs; }
+
+ 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
+ double T_step_; // Period of a step
+
+ 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
+ Heightmap heightmap_;
+ Vector3 rpy_map = Vector3::Zero(3);
+
+ double v_max = 0.3; // rad.s-1
+ double v_max_z = 0.1; // rad.s-1
+ int n_surface_configs = 3;
+ MatrixN configs;
+};
+
+#endif // STATEPLANNER3D_H_INCLUDED
diff --git a/include/qrw/Surface.hpp b/include/qrw/Surface.hpp
new file mode 100644
index 00000000..d3ac1988
--- /dev/null
+++ b/include/qrw/Surface.hpp
@@ -0,0 +1,67 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief This is the header for Surface class
+///
+/// \details Surface data structure
+///
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifndef SURFACE_H_INCLUDED
+#define SURFACE_H_INCLUDED
+
+#include "qrw/Types.h"
+
+class Surface {
+ public:
+ // Constructor
+ Surface();
+ Surface(MatrixN const& A_in, VectorN const& b_in, MatrixN const& vertices_in);
+
+ bool operator==(const Surface& other) const {
+ return A_ == other.A_ && b_ == other.b_ && vertices_ == other.vertices_;
+ }
+
+ bool operator!=(const Surface& other) const {
+ return A_ != other.A_ or b_ != other.b_ or vertices_ != other.vertices_;
+ }
+
+ // Destructor
+ ~Surface() {}
+
+ // Usefull for python binding
+ MatrixN getA() const;
+ void setA(MatrixN const& A_in);
+
+ VectorN getb() const;
+ void setb(VectorN const& b_in);
+
+ MatrixN getVertices() const;
+ void setVertices(MatrixN const& vertices_in);
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief For a given X,Y point that belongs to the surface, return the height
+ /// d/c -a/c*x -b/c*y
+ ///
+ /// \param[in] point Vecto3 [x, y, z]
+ ///
+ //////////////////////////////////////////////////////////////////////////////////////////////////
+ double getHeight(Vector2 const& point) const;
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief For a given X,Y point return true if the point is in the surface
+ ///
+ /// \param[in] point Vecto3 [x, y]
+ ///
+ //////////////////////////////////////////////////////////////////////////////////////////////////
+ bool hasPoint(Vector2 const& point) const;
+
+ MatrixN vertices_;
+
+ private:
+ MatrixN A_;
+ VectorN b_;
+};
+
+#endif // SURFACE_H_INCLUDED
diff --git a/include/qrw/Types.h b/include/qrw/Types.h
index cda71ad7..cd028747 100644
--- a/include/qrw/Types.h
+++ b/include/qrw/Types.h
@@ -36,6 +36,7 @@ using Matrix118 = Eigen::Matrix<double, 1, 18>;
using Matrix34 = Eigen::Matrix<double, 3, 4>;
using Matrix43 = Eigen::Matrix<double, 4, 3>;
using Matrix64 = Eigen::Matrix<double, 6, 4>;
+using Matrix74 = Eigen::Matrix<double, 7, 4>;
using MatrixN4 = Eigen::Matrix<double, Eigen::Dynamic, 4>;
using Matrix3N = Eigen::Matrix<double, 3, Eigen::Dynamic>;
using Matrix6N = Eigen::Matrix<double, 6, Eigen::Dynamic>;
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index e155a27b..1e18f6ec 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -2,6 +2,7 @@
#include "qrw/InvKin.hpp"
#include "qrw/MPC.hpp"
#include "qrw/StatePlanner.hpp"
+#include "qrw/StatePlanner3D.hpp"
#include "qrw/Gait.hpp"
#include "qrw/FootstepPlanner.hpp"
#include "qrw/FootTrajectoryGenerator.hpp"
@@ -10,9 +11,14 @@
#include "qrw/Joystick.hpp"
#include "qrw/Filter.hpp"
#include "qrw/Params.hpp"
+#include "qrw/FootstepPlannerQP.hpp"
+#include "qrw/Surface.hpp"
+#include "qrw/FootTrajectoryGeneratorBezier.hpp"
#include <boost/python.hpp>
#include <eigenpy/eigenpy.hpp>
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+
namespace bp = boost::python;
@@ -491,8 +497,11 @@ struct ParamsPythonVisitor : public bp::def_visitor<ParamsPythonVisitor<Params>>
.def_readwrite("lock_time", &Params::lock_time)
.def_readwrite("vert_time", &Params::vert_time)
.def_readwrite("footsteps_init", &Params::footsteps_init)
- .def_readwrite("footsteps_under_shoulders", &Params::footsteps_under_shoulders);
-
+ .def_readwrite("footsteps_under_shoulders", &Params::footsteps_under_shoulders)
+ .def_readwrite("solo3D", &Params::solo3D)
+ .def_readwrite("enable_multiprocessing_mip", &Params::enable_multiprocessing_mip)
+ .def_readwrite("environment_URDF", &Params::environment_URDF)
+ .def_readwrite("environment_heightmap", &Params::environment_heightmap);
}
static void expose()
@@ -504,26 +513,167 @@ struct ParamsPythonVisitor : public bp::def_visitor<ParamsPythonVisitor<Params>>
};
void exposeParams() { ParamsPythonVisitor<Params>::expose(); }
+/////////////////////////////////
+/// Binding FootTrajectoryGeneratorBezier class
+/////////////////////////////////
+template <typename FootTrajectoryGeneratorBezier>
+struct FootTrajectoryGeneratorBezierPythonVisitor
+ : public bp::def_visitor<FootTrajectoryGeneratorBezierPythonVisitor<FootTrajectoryGeneratorBezier>> {
+ template <class PyClassFootTrajectoryGeneratorBezier>
+ void visit(PyClassFootTrajectoryGeneratorBezier& cl) const {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+ .def("getFootPosition", &FootTrajectoryGeneratorBezier::getFootPosition, "Get position_ matrix.\n")
+ .def("getFootVelocity", &FootTrajectoryGeneratorBezier::getFootVelocity, "Get velocity_ matrix.\n")
+ .def("getFootAcceleration", &FootTrajectoryGeneratorBezier::getFootAcceleration, "Get acceleration_ matrix.\n")
+ .def("getFootJerk", &FootTrajectoryGeneratorBezier::getFootJerk, "Get jerk_ matrix.\n")
+ .def("evaluateBezier", &FootTrajectoryGeneratorBezier::evaluateBezier, "Evaluate Bezier curve by foot.\n")
+ .def("evaluatePoly", &FootTrajectoryGeneratorBezier::evaluatePoly, "Evaluate Bezier curve by foot.\n")
+ .def("getFootPositionBaseFrame", &FootTrajectoryGeneratorBezier::getFootPositionBaseFrame, bp::args("R", "T"),
+ "Get position_ matrix in base frame.\n")
+ .def("getFootVelocityBaseFrame", &FootTrajectoryGeneratorBezier::getFootVelocityBaseFrame,
+ bp::args("R", "v_ref", "w_ref"), "Get velocity_ matrix in base frame.\n")
+ .def("getFootAccelerationBaseFrame", &FootTrajectoryGeneratorBezier::getFootAccelerationBaseFrame,
+ bp::args("R", "w_ref", "a_ref"), "Get acceleration_ matrix in base frame.\n")
+
+ .def("initialize", &FootTrajectoryGeneratorBezier::initialize, bp::args("params", "gaitIn"),
+ "Initialize FootTrajectoryGeneratorBezier from Python.\n")
+
+ .add_property("t0s", bp::make_function(&FootTrajectoryGeneratorBezier::get_t0s,
+ bp::return_value_policy<bp::return_by_value>()))
+ .add_property("t_swing", bp::make_function(&FootTrajectoryGeneratorBezier::get_t_swing,
+ bp::return_value_policy<bp::return_by_value>()))
+
+ // Compute target location of footsteps from Python
+ .def("update", &FootTrajectoryGeneratorBezier::update, bp::args("k", "targetFootstep"),
+ "Compute target location of footsteps from Python.\n");
+ }
+
+ static void expose() {
+ bp::class_<FootTrajectoryGeneratorBezier>("FootTrajectoryGeneratorBezier", bp::no_init)
+ .def(FootTrajectoryGeneratorBezierPythonVisitor<FootTrajectoryGeneratorBezier>());
+
+ ENABLE_SPECIFIC_MATRIX_TYPE(MatrixN);
+ }
+};
+void exposeFootTrajectoryGeneratorBezier() {
+ FootTrajectoryGeneratorBezierPythonVisitor<FootTrajectoryGeneratorBezier>::expose();
+}
+
+/////////////////////////////////
+/// Binding Surface class
+/////////////////////////////////
+template <typename Surface>
+struct SurfacePythonVisitor : public bp::def_visitor<SurfacePythonVisitor<Surface>> {
+ template <class PyClassSurface>
+ void visit(PyClassSurface& cl) const {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+ .def(bp::init<MatrixN, VectorN, MatrixN>(bp::args("A", "b", "vertices"), "Constructor with parameters."))
+
+ .def("get_vertices", &Surface::getVertices, "get the vertices of the surface.\n")
+ .def("get_A", &Surface::getA, "get A vector of inequalities.\n")
+ .def("get_b", &Surface::getb, "get b vector of inequalities.\n")
+
+ .add_property("A", bp::make_function(&Surface::getA, bp::return_value_policy<bp::return_by_value>()))
+ .add_property("b", bp::make_function(&Surface::getb, bp::return_value_policy<bp::return_by_value>()))
+ .add_property("vertices",
+ bp::make_function(&Surface::getVertices, bp::return_value_policy<bp::return_by_value>()))
+
+ .def("get_height", &Surface::getHeight, bp::args("point"), "get the height of a point of the surface.\n")
+ .def("has_point", &Surface::hasPoint, bp::args("point"), "return true if the point is in the surface.\n");
+ }
+
+ static void expose() {
+ bp::class_<Surface>("Surface", bp::no_init).def(SurfacePythonVisitor<Surface>());
+
+ ENABLE_SPECIFIC_MATRIX_TYPE(MatrixN);
+ }
+};
+void exposeSurface() { SurfacePythonVisitor<Surface>::expose(); }
+
+/////////////////////////////////
+/// Binding FootstepPlannerQP class
+/////////////////////////////////
+template <typename FootstepPlannerQP>
+struct FootstepPlannerQPPythonVisitor : public bp::def_visitor<FootstepPlannerQPPythonVisitor<FootstepPlannerQP>> {
+ template <class PyClassFootstepPlannerQP>
+ void visit(PyClassFootstepPlannerQP& cl) const {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+ .def("getFootsteps", &FootstepPlannerQP::getFootsteps, "Get footsteps_ matrix.\n")
+ .def("getTargetFootsteps", &FootstepPlannerQP::getTargetFootsteps, "Get footsteps_ matrix.\n")
+ .def("getRz", &FootstepPlannerQP::getRz, "Get rotation along z matrix.\n")
+
+ .def("initialize", &FootstepPlannerQP::initialize, bp::args("params", "gaitIn"),
+ "Initialize FootstepPlanner from Python.\n")
+
+ // Compute target location of footsteps from Python
+ .def("updateFootsteps", &FootstepPlannerQP::updateFootsteps, bp::args("refresh", "k", "q", "b_v", "b_vref"),
+ "Update and compute location of footsteps from Python.\n");
+ }
+
+ static void expose() {
+ bp::class_<SurfaceVector>("SurfaceVector").def(bp::vector_indexing_suite<SurfaceVector>());
+ bp::class_<SurfaceVectorVector>("SurfaceVectorVector").def(bp::vector_indexing_suite<SurfaceVectorVector>());
+ bp::class_<FootstepPlannerQP>("FootstepPlannerQP", bp::no_init)
+ .def(FootstepPlannerQPPythonVisitor<FootstepPlannerQP>());
+
+ ENABLE_SPECIFIC_MATRIX_TYPE(MatrixN);
+ }
+};
+void exposeFootstepPlannerQP() { FootstepPlannerQPPythonVisitor<FootstepPlannerQP>::expose(); }
+
+/////////////////////////////////
+/// Binding StatePlanner3D class
+/////////////////////////////////
+template <typename StatePlanner3D>
+struct StatePlanner3DPythonVisitor : public bp::def_visitor<StatePlanner3DPythonVisitor<StatePlanner3D>> {
+ template <class PyClassStatePlanner3D>
+ void visit(PyClassStatePlanner3D& cl) const {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+ .def("getReferenceStates", &StatePlanner3D::getReferenceStates, "Get xref matrix.\n")
+ .def("getNSteps", &StatePlanner3D::getNSteps, "Get number of steps in prediction horizon.\n")
+ .def("get_configurations", &StatePlanner3D::get_configurations, "Get conf vector.\n")
+
+ .def("initialize", &StatePlanner3D::initialize, bp::args("params"), "Initialize StatePlanner3D from Python.\n")
+
+ // Run StatePlanner3D from Python
+ .def("computeReferenceStates", &StatePlanner3D::computeReferenceStates,
+ bp::args("q", "v", "b_vref", "is_new_step"), "Run StatePlanner from Python.\n");
+ }
+
+ static void expose() {
+ bp::class_<StatePlanner3D>("StatePlanner3D", bp::no_init).def(StatePlanner3DPythonVisitor<StatePlanner3D>());
+
+ ENABLE_SPECIFIC_MATRIX_TYPE(MatrixN);
+ }
+};
+void exposeStatePlanner3D() { StatePlanner3DPythonVisitor<StatePlanner3D>::expose(); }
+
/////////////////////////////////
/// Exposing classes
/////////////////////////////////
-BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
-{
- boost::python::def("add", gepetto::example::add);
- boost::python::def("sub", gepetto::example::sub);
-
- eigenpy::enableEigenPy();
-
- exposeMPC();
- exposeFilter();
- exposeStatePlanner();
- exposeGait();
- exposeFootstepPlanner();
- exposeFootTrajectoryGenerator();
- exposeInvKin();
- exposeQPWBC();
- exposeWbcWrapper();
- exposeEstimator();
- exposeJoystick();
- exposeParams();
+BOOST_PYTHON_MODULE(libquadruped_reactive_walking) {
+ boost::python::def("add", gepetto::example::add);
+ boost::python::def("sub", gepetto::example::sub);
+
+ eigenpy::enableEigenPy();
+
+ exposeMPC();
+ exposeFilter();
+ exposeStatePlanner();
+ exposeGait();
+ exposeFootstepPlanner();
+ exposeFootTrajectoryGenerator();
+ exposeInvKin();
+ exposeQPWBC();
+ exposeWbcWrapper();
+ exposeEstimator();
+ exposeJoystick();
+ exposeParams();
+ exposeSurface();
+ exposeFootTrajectoryGeneratorBezier();
+ exposeFootstepPlannerQP();
+ exposeStatePlanner3D();
}
\ No newline at end of file
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 6ec5ac05..6079fad9 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -1,6 +1,8 @@
# coding: utf8
import numpy as np
+from numpy.lib.function_base import sinc
+from IPython import embed_kernel
import utils_mpc
import time
import math
@@ -13,6 +15,10 @@ from solopython.utils.viewerClient import viewerClient, NonBlockingViewerFromRob
import libquadruped_reactive_walking as lqrw
from example_robot_data.robots_loader import Solo12Loader
+from solo3D.SurfacePlannerWrapper import SurfacePlanner_Wrapper
+from solo3D.tools.pyb_environment_3D import PybEnvironment3D
+from solo3D.tools.utils import quaternionToRPY
+from example_robot_data import load
class Result:
"""Object to store the result of the control loop
@@ -68,6 +74,9 @@ class dummyDevice:
self.hardware = dummyHardware()
self.imu = dummyIMU()
self.joints = dummyJoints()
+ self.dummyPos = np.zeros(3)
+ self.dummyPos[2] = 0.24
+ self.b_baseVel = np.zeros(3)
class Controller:
@@ -121,18 +130,9 @@ class Controller:
self.xgoals = np.zeros((12, 1))
self.xgoals[2, 0] = self.h_ref
- self.statePlanner = lqrw.StatePlanner()
- self.statePlanner.initialize(params)
-
self.gait = lqrw.Gait()
self.gait.initialize(params)
- self.footstepPlanner = lqrw.FootstepPlanner()
- self.footstepPlanner.initialize(params, self.gait)
-
- self.footTrajectoryGenerator = lqrw.FootTrajectoryGenerator()
- self.footTrajectoryGenerator.initialize(params, self.gait)
-
self.estimator = lqrw.Estimator()
self.estimator.initialize(params)
@@ -144,6 +144,66 @@ class Controller:
self.o_targetFootstep = np.zeros((3, 4)) # Store result for MPC_planner
self.DEMONSTRATION = params.DEMONSTRATION
+ self.solo3D = params.solo3D
+ self.enable_multiprocessing_mip = params.enable_multiprocessing_mip
+ self.offset_perfect_estimator = 0.
+ if self.solo3D:
+ self.surfacePlanner = SurfacePlanner_Wrapper(params) # MIP Wrapper
+
+ self.statePlanner = lqrw.StatePlanner3D()
+ self.statePlanner.initialize(params)
+
+ self.footstepPlanner = lqrw.FootstepPlannerQP()
+ self.footstepPlanner.initialize(params, self.gait, self.surfacePlanner.floor_surface)
+
+ self.footstepPlanner_ref = lqrw.FootstepPlanner()
+ self.footstepPlanner_ref.initialize(params, self.gait)
+
+ # Trajectory Generator Bezier
+ x_margin_max_ = 0.05 # 4cm margin
+ t_margin_ = 0.3 # 15% of the curve around critical point
+ z_margin_ = 0.01 # 1% of the curve after the critical point
+ N_sample = 8 # Number of sample in the least square optimisation for Bezier coeffs
+ N_sample_ineq = 10 # Number of sample while browsing the curve
+ degree = 7 # Degree of the Bezier curve
+
+ # pinocchio model and data, CoM and Inertia estimation for MPC
+ robot = load('solo12')
+ self.data = robot.data.copy() # for velocity estimation (forward kinematics)
+ self.model = robot.model.copy() # for velocity estimation (forward kinematics)
+ self.q_neutral = pin.neutral(self.model).reshape((19, 1)) # column vector
+
+ self.footTrajectoryGenerator = lqrw.FootTrajectoryGeneratorBezier()
+ self.footTrajectoryGenerator.initialize(params, self.gait, self.surfacePlanner.floor_surface,
+ x_margin_max_, t_margin_, z_margin_, N_sample, N_sample_ineq,
+ degree)
+
+ self.pybEnvironment3D = PybEnvironment3D(params, self.gait, self.statePlanner, self.footstepPlanner,
+ self.footTrajectoryGenerator)
+
+ self.q_mes_3d = np.zeros((18,1))
+ self.v_mes_3d = np.zeros((18,1))
+ self.q_filt_3d = np.zeros((18,1))
+ self.v_filt_3d = np.zeros((18,1))
+ self.filter_q_3d = lqrw.Filter()
+ self.filter_q_3d.initialize(params)
+ self.filter_v_3d = lqrw.Filter()
+ self.filter_v_3d.initialize(params)
+
+ else:
+ self.statePlanner = lqrw.StatePlanner()
+ self.statePlanner.initialize(params)
+
+ self.footstepPlanner = lqrw.FootstepPlanner()
+ self.footstepPlanner.initialize(params, self.gait)
+
+ self.footTrajectoryGenerator = lqrw.FootTrajectoryGenerator()
+ self.footTrajectoryGenerator.initialize(params, self.gait)
+
+ # ForceMonitor to display contact forces in PyBullet with red lines
+ # import ForceMonitor
+ # myForceMonitor = ForceMonitor.ForceMonitor(pyb_sim.robotId, pyb_sim.planeId)
+
self.envID = params.envID
self.velID = params.velID
self.dt_wbc = params.dt_wbc
@@ -204,7 +264,7 @@ class Controller:
dDevice.joints.positions = q_init
self.compute(dDevice)
- def compute(self, device):
+ def compute(self, device, qc=None):
"""Run one iteration of the main control loop
Args:
@@ -216,6 +276,19 @@ class Controller:
# Update the reference velocity coming from the gamepad
self.joystick.update_v_ref(self.k, self.velID, self.gait.getIsStatic())
+ # dummyPos replaced by dummy_state to give Yaw estimated by motion capture to the estimator
+ dummy_state = np.zeros((6,1)) # state = [x,y,z,roll,pitch,yaw]
+ b_baseVel = np.zeros((3,1))
+ if self.solo3D and qc == None:
+ dummy_state[:3,0] = device.dummyPos
+ dummy_state[3:,0] = device.imu.attitude_euler # Yaw only used for solo3D
+ b_baseVel[:,0] = device.b_baseVel
+ elif self.solo3D and qc != None:
+ # motion capture data
+ dummy_state[:3,0] = qc.getPosition()
+ dummy_state[3:] = quaternionToRPY(qc.getOrientationQuat())
+ b_baseVel[:,0] = (self.qc.getOrientationMat9().reshape((3,3)).transpose() @ self.qc.getVelocity().reshape((3, 1))).ravel()
+
# Process state estimator
self.estimator.run_filter(self.gait.getCurrentGait(),
self.footTrajectoryGenerator.getFootPosition(),
@@ -224,8 +297,8 @@ class Controller:
device.imu.attitude_euler.reshape((-1, 1)),
device.joints.positions.reshape((-1, 1)),
device.joints.velocities.reshape((-1, 1)),
- np.zeros((3, 1)), # device.dummyPos.reshape((-1, 1)), #Â TODO: Case of real device
- np.zeros((3, 1))) # device.b_baseVel.reshape((-1, 1)))
+ dummy_state,
+ b_baseVel)
# Update state vectors of the robot (q and v) + transformation matrices between world and horizontal frames
self.estimator.updateState(self.joystick.getVRef(), self.gait)
@@ -243,6 +316,21 @@ class Controller:
# TODO: Understand why using Python or C++ h_v leads to a slightly different result since the
# difference between them at each time step is 1e-16 at max (butterfly effect?)
+ # Use position and velocities from motion capture for solo3D
+ if self.solo3D:
+ self.q_mes_3d[:3,0] = self.estimator.getQFilt()[:3]
+ self.q_mes_3d[6:,0] = self.estimator.getQFilt()[7:]
+ self.q_mes_3d[3:6] = quaternionToRPY(self.estimator.getQFilt()[3:7])
+ self.v_mes_3d[:,0] = self.estimator.getVFilt()
+ # Quantities go through a 1st order low pass filter with fc = 15 Hz (avoid >25Hz foldback)
+ self.q_filt_3d[:6, 0] = self.filter_q_3d.filter(self.q_mes_3d[:6, 0:1], True)
+ self.q_filt_3d[6:, 0] = self.q_mes_3d[6:, 0].copy()
+ self.v_filt_3d[:6, 0] = self.filter_v_3d.filter(self.v_mes_3d[:6, 0:1], False)
+ self.v_filt_3d[6:, 0] = self.v_mes_3d[6:, 0].copy()
+ oTh_3d = np.zeros((3,1))
+ oTh_3d[:2,0] = self.q_filt_3d[:2,0]
+ oRh_3d = pin.rpy.rpyToMatrix(self.q_filt_3d[3:6,0])
+
t_filter = time.time()
"""if (self.k % self.k_mpc) == 0 and self.k > 1000:
@@ -273,24 +361,42 @@ class Controller:
self.h_v_filt_mpc[:, 0] = self.filter_mpc_v.filter(self.h_v[:6, 0:1], False)
self.vref_filt_mpc[:, 0] = self.filter_mpc_vref.filter(self.v_ref[:6, 0:1], False)
- # Compute target footstep based on current and reference velocities
- """if self.gait.getIsStatic():
- self.h_v_windowed[0:6, 0:1] *= 0.0"""
- o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
- int(self.k_mpc - self.k % self.k_mpc),
- self.q[:, 0],
- self.h_v_windowed[0:6, 0:1].copy(),
- self.v_ref[0:6, 0:1])
-
- # Run state planner (outputs the reference trajectory of the base)
- self.statePlanner.computeReferenceStates(self.q_filt_mpc[0:6, 0:1], self.h_v_filt_mpc[0:6, 0:1].copy(),
- self.vref_filt_mpc[0:6, 0:1])
+ is_new_step = self.k % self.k_mpc == 0 and self.gait.isNewPhase()
+ if self.solo3D:
+ if is_new_step:
+ if self.surfacePlanner.first_iteration:
+ self.surfacePlanner.first_iteration = False
+ else:
+ self.surfacePlanner.update_latest_results()
+ self.pybEnvironment3D.update_target_SL1M(self.surfacePlanner.all_feet_pos)
+ # Compute target footstep based on current and reference velocities
+ o_targetFootstep = self.footstepPlanner.updateFootsteps(
+ self.k % self.k_mpc == 0 and self.k != 0, int(self.k_mpc - self.k % self.k_mpc), self.q_filt_3d[:, 0],
+ self.h_v_windowed[0:6, 0:1].copy(), self.v_ref[0:6, 0:1], self.surfacePlanner.potential_surfaces,
+ self.surfacePlanner.selected_surfaces, self.surfacePlanner.mip_success,
+ self.surfacePlanner.mip_iteration)
+ # Run state planner (outputs the reference trajectory of the base)
+ self.statePlanner.computeReferenceStates(self.q_filt_3d[0:6, 0:1], self.h_v_filt_mpc[0:6, 0:1].copy(),
+ self.vref_filt_mpc[0:6, 0:1], is_new_step)
+ else:
+ # Compute target footstep based on current and reference velocities
+ o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
+ int(self.k_mpc - self.k % self.k_mpc),
+ self.q[:, 0], self.h_v_windowed[0:6, 0:1].copy(),
+ self.v_ref[0:6, 0:1])
+ # Run state planner (outputs the reference trajectory of the base)
+ self.statePlanner.computeReferenceStates(self.q_filt_mpc[0:6, 0:1], self.h_v_filt_mpc[0:6, 0:1].copy(),
+ self.vref_filt_mpc[0:6, 0:1], 0.0)
# Result can be retrieved with self.statePlanner.getReferenceStates()
xref = self.statePlanner.getReferenceStates()
fsteps = self.footstepPlanner.getFootsteps()
cgait = self.gait.getCurrentGait()
+ if is_new_step and self.solo3D: # Run surface planner
+ configs = self.statePlanner.get_configurations().transpose()
+ self.surfacePlanner.run(configs, cgait, o_targetFootstep, self.vref_filt_mpc.copy())
+
t_planner = time.time()
"""if self.k % 250 == 0:
@@ -319,7 +425,7 @@ class Controller:
"""if (self.k % self.k_mpc) == 0:
from IPython import embed
embed()"""
-
+
# Retrieve reference contact forces in horizontal frame
self.x_f_mpc = self.mpc_wrapper.get_latest_result()
@@ -346,8 +452,12 @@ class Controller:
self.o_targetFootstep[:2, foot] = self.x_f_mpc[24 + 2*foot:24+2*foot+2, id+1]
# Update pos, vel and acc references for feet
- self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep)
-
+ if self.solo3D: # Bezier curves, needs estimated position of the feet
+ currentPosition = self.computeFootPositionFeedback(self.k, device, self.q_filt_3d, self.v_filt_3d)
+ self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep, self.surfacePlanner.selected_surfaces,
+ currentPosition)
+ else:
+ self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep)
# Whole Body Control
# If nothing wrong happened yet in the WBC controller
if (not self.error) and (not self.joystick.getStop()):
@@ -373,8 +483,14 @@ class Controller:
self.x_f_mpc[12:24, 0] = [0.0, 0.0, 9.81 * 2.5 / 4.0] * 4
# Update configuration vector for wbc
- self.q_wbc[3, 0] = self.q_filt_mpc[3, 0] # Roll
- self.q_wbc[4, 0] = self.q_filt_mpc[4, 0] # Pitch
+ if self.solo3D: # Update roll, pitch according to heighmap
+ self.q_wbc[3, 0] = self.dt_wbc * (xref[3, 1] -
+ self.q_filt_mpc[3, 0]) / self.dt_mpc + self.q_filt_mpc[3, 0] # Roll
+ self.q_wbc[4, 0] = self.dt_wbc * (xref[4, 1] -
+ self.q_filt_mpc[4, 0]) / self.dt_mpc + self.q_filt_mpc[4, 0] # Pitch
+ else:
+ self.q_wbc[3, 0] = self.q_filt_mpc[3, 0] # Roll
+ self.q_wbc[4, 0] = self.q_filt_mpc[4, 0] # Pitch
self.q_wbc[6:, 0] = self.wbcWrapper.qdes[:] # with reference angular positions of previous loop
# Update velocity vector for wbc
@@ -382,12 +498,20 @@ class Controller:
self.dq_wbc[6:, 0] = self.wbcWrapper.vdes[:] # with reference angular velocities of previous loop
# Feet command position, velocity and acceleration in base frame
- self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(
- hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_v_cmd = self.footTrajectoryGenerator.getFootVelocityBaseFrame(
- hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(
- hRb @ oRh.transpose(), oTh + np.array([[0.0], [0.0], [self.h_ref]]))
+ if self.solo3D: # Use estimated base frame
+ self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(
+ oRh_3d.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
+ self.feet_v_cmd = self.footTrajectoryGenerator.getFootVelocityBaseFrame(
+ oRh_3d.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
+ self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(
+ oRh_3d.transpose(), oTh_3d + np.array([[0.0], [0.0], [self.h_ref]]))
+ else: # Use ideal base frame
+ self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(
+ hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
+ self.feet_v_cmd = self.footTrajectoryGenerator.getFootVelocityBaseFrame(
+ hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
+ self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(
+ hRb @ oRh.transpose(), oTh + np.array([[0.0], [0.0], [self.h_ref]]))
# Desired position, orientation and velocities of the base
"""self.xgoals[[0, 1, 2, 5], 0] = np.zeros((4,))
@@ -472,7 +596,10 @@ class Controller:
# Update PyBullet camera
# to have yaw update in simu: utils_mpc.quaternionToRPY(self.estimator.q_filt[3:7, 0])[2, 0]
- self.pyb_camera(device, 0.0)
+ # self.pyb_camera(device, 0.0)
+
+ if self.solo3D: # Update 3D Environment
+ self.pybEnvironment3D.update(self.k)
# Update debug display (spheres, ...)
self.pyb_debug(device, fsteps, cgait, xref)
@@ -504,8 +631,10 @@ class Controller:
oTh_pyb[2, 0] += 0.0
oRh_pyb = pin.rpy.rpyToMatrix(0.0, 0.0, device.imu.attitude_euler[2])
for i in range(4):
- pos = oRh_pyb @ self.feet_p_cmd[:, i:(i+1)] + oTh_pyb
- pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids_deb[i], pos[:, 0].tolist(), [0, 0, 0, 1])
+ # pos = oRh_pyb @ self.feet_p_cmd[:, i:(i+1)] + oTh_pyb
+ # pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids_deb[i], pos[:, 0].tolist(), [0, 0, 0, 1])
+ pos = self.o_targetFootstep[:,i]
+ pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids_deb[i], pos, [0, 0, 0, 1])
# Display desired footstep positions as blue spheres
for i in range(4):
@@ -595,3 +724,65 @@ class Controller:
self.t_mpc = t_mpc - t_planner
self.t_wbc = t_wbc - t_mpc
self.t_loop = time.time() - tic
+
+ def computeFootPositionFeedback(self, k, device, q_filt, v_filt):
+ ''' Return the position of the foot using Pybullet feedback, Pybullet feedback with forward dynamics
+ or Estimator feedback with forward dynamics
+ Args :
+ - k (int) : step indice
+ - q_filt (Arrayx18) : q estimated (only for estimator feedback)
+ - v_vilt (arrayx18) : v estimated (only for estimator feedback)
+ Returns :
+ - currentPosition (Array 3x4)
+ '''
+ currentPosition = np.zeros((3, 4))
+ q_filt_ = np.zeros((19, 1))
+ q_filt_[:3] = q_filt[:3]
+ q_filt_[3:7] = pin.Quaternion(pin.rpy.rpyToMatrix(q_filt[3:6, 0])).coeffs().reshape((4, 1))
+ q_filt_[7:] = q_filt[6:]
+
+ # Current position : Pybullet feedback, directly
+ ##########################
+
+ # linkId = [3, 7 ,11 ,15]
+ # if k != 0 :
+ # links = pyb.getLinkStates(device.pyb_sim.robotId, linkId , computeForwardKinematics=True , computeLinkVelocity=True )
+
+ # for j in range(4) :
+ # self.goals[:,j] = np.array(links[j][4])[:] # pos frame world for feet
+ # self.goals[2,j] -= 0.016988 # Z offset due to position of frame in object
+ # self.vgoals[:,j] = np.array(links[j][6]) # vel frame world for feet
+
+ # Current position : Pybullet feedback, with forward dynamics
+ ##########################
+
+ # if k > 0: # Dummy device for k == 0
+ # qmes = np.zeros((19, 1))
+ # revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
+ # jointStates = pyb.getJointStates(device.pyb_sim.robotId, revoluteJointIndices)
+ # baseState = pyb.getBasePositionAndOrientation(device.pyb_sim.robotId)
+ # qmes[:3, 0] = baseState[0]
+ # qmes[3:7, 0] = baseState[1]
+ # qmes[7:, 0] = [state[0] for state in jointStates]
+ # pin.forwardKinematics(self.model, self.data, qmes, v_filt)
+ # else:
+ # pin.forwardKinematics(self.model, self.data, q_filt_, v_filt)
+
+ # Current position : Estimator feedback, with forward dynamics
+ ##########################
+
+ pin.forwardKinematics(self.model, self.data, q_filt_, v_filt)
+
+ contactFrameId = [10, 18, 26, 34] # = [ FL , FR , HL , HR]
+
+ for j in range(4):
+ framePlacement = pin.updateFramePlacement(self.model, self.data,
+ contactFrameId[j]) # = solo.data.oMf[18].translation
+ frameVelocity = pin.getFrameVelocity(self.model, self.data, contactFrameId[j], pin.ReferenceFrame.LOCAL)
+
+ currentPosition[:, j] = framePlacement.translation[:]
+ # if k > 0:
+ # currentPosition[2, j] -= 0.016988 # Pybullet offset on Z
+ # self.vgoals[:,j] = frameVelocity.linear # velocity feedback not working
+
+ return currentPosition
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index b7c7c4f5..0498f0d2 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -191,16 +191,16 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
device.parse_sensor_data()
# Desired torques
- controller.compute(device)
+ controller.compute(device, qc)
# Check that the initial position of actuators is not too far from the
# desired position of actuators to avoid breaking the robot
- if (t <= 10 * params.dt_wbc):
- if np.max(np.abs(controller.result.q_des - device.joints.positions)) > 0.15:
- print("DIFFERENCE: ", controller.result.q_des - device.joints.positions)
- print("q_des: ", controller.result.q_des)
- print("q_mes: ", device.joints.positions)
- break
+ # if (t <= 10 * params.dt_wbc):
+ # if np.max(np.abs(controller.result.q_des - device.joints.positions)) > 0.15:
+ # print("DIFFERENCE: ", controller.result.q_des - device.joints.positions)
+ # print("q_des: ", controller.result.q_des)
+ # print("q_mes: ", device.joints.positions)
+ # break
# Set desired quantities for the actuators
device.joints.set_position_gains(controller.result.P)
diff --git a/scripts/solo3D/StatePlanner.py b/scripts/solo3D/StatePlanner.py
new file mode 100644
index 00000000..eb8f4e85
--- /dev/null
+++ b/scripts/solo3D/StatePlanner.py
@@ -0,0 +1,172 @@
+import numpy as np
+import pinocchio as pin
+from sl1m.solver import solve_least_square
+import pickle
+
+# from solo3D.tools.ProfileWrapper import ProfileWrapper
+
+# Store the results from cprofile
+# profileWrap = ProfileWrapper()
+
+
+class StatePlanner():
+
+ def __init__(self, params):
+ self.n_surface_configs = 3
+ self.h_ref = params.h_ref
+ self.dt_mpc = params.dt_mpc
+ self.T_step = params.T_gait / 2
+
+ self.n_steps = int(params.gait.shape[0])
+ self.referenceStates = np.zeros((12, 1 + self.n_steps))
+
+ filehandler = open(params.environment_heightmap, 'rb')
+ self.map = pickle.load(filehandler)
+ self.FIT_SIZE_X = 0.4
+ self.FIT_SIZE_Y = 0.4
+ self.surface_point = np.zeros(3)
+
+ self.configs = [np.zeros(7) for _ in range(self.n_surface_configs)]
+
+ self.result = [0., 0., 0.]
+
+ def computeReferenceStates(self, q, v, v_ref, new_step=False):
+ '''
+ - q (7x1) : [px , py , pz , x , y , z , w] --> here x,y,z,w quaternion
+ - v (6x1) : current v linear in world frame
+ - v_ref (6x1) : vref in world frame
+ '''
+ rpy = q[3:6]
+ if new_step:
+ # id_x, id_y = self.rpy_map.map_index(q[0], q[1])
+ # self.result = self.rpy_map.result[id_x, id_y]
+ self.result = self.compute_mean_surface(q[:3])
+ # from IPython import embed
+ # embed()
+
+ # Update the current state
+ # self.referenceStates[:3, 0] = q[:3] + pin.rpy.rpyToMatrix(rpy).dot(np.array([-0.04, 0., 0.]))
+ self.referenceStates[:2, 0] = np.zeros(2)
+ self.referenceStates[2, 0] = q[2]
+ self.referenceStates[3:6, 0] = rpy
+ self.referenceStates[5, 0] = 0.
+ self.referenceStates[6:9, 0] = v[:3]
+ self.referenceStates[9:12, 0] = v[3:6]
+
+ for i in range(1, self.n_steps + 1):
+ dt = i * self.dt_mpc
+
+ if v_ref[5] < 10e-3:
+ self.referenceStates[0, i] = v_ref[0] * dt
+ self.referenceStates[1, i] = v_ref[1] * dt
+ else:
+ self.referenceStates[0, i] = (v_ref[0] * np.sin(v_ref[5] * dt) + v_ref[1] *
+ (np.cos(v_ref[5] * dt) - 1.)) / v_ref[5]
+ self.referenceStates[1, i] = (v_ref[1] * np.sin(v_ref[5] * dt) - v_ref[0] *
+ (np.cos(v_ref[5] * dt) - 1.)) / v_ref[5]
+
+ self.referenceStates[:2, i] += self.referenceStates[:2, 0]
+
+ # self.referenceStates[5, i] = rpy[2] + v_ref[5] * dt
+ self.referenceStates[5, i] = v_ref[5] * dt
+
+ self.referenceStates[6, i] = v_ref[0] * np.cos(v_ref[5] * dt) - v_ref[1] * np.sin(v_ref[5] * dt)
+ self.referenceStates[7, i] = v_ref[0] * np.sin(v_ref[5] * dt) + v_ref[1] * np.cos(v_ref[5] * dt)
+
+ self.referenceStates[11, i] = v_ref[5]
+
+ # Update according to heightmap
+ # result = self.compute_mean_surface(q[:3])
+
+ rpy_map = np.zeros(3)
+ rpy_map[0] = -np.arctan2(self.result[1], 1.)
+ rpy_map[1] = -np.arctan2(self.result[0], 1.)
+
+ self.referenceStates[3, 1:] = rpy_map[0] * np.cos(rpy[2]) - rpy_map[1] * np.sin(rpy[2])
+ self.referenceStates[4, 1:] = rpy_map[0] * np.sin(rpy[2]) + rpy_map[1] * np.cos(rpy[2])
+
+ v_max = 0.3 # rad.s-1
+ self.referenceStates[9, 1] = max(min((self.referenceStates[3, 1] - rpy[0]) / self.dt_mpc, v_max), -v_max)
+ self.referenceStates[9, 2:] = 0.
+ self.referenceStates[10, 1] = max(min((self.referenceStates[4, 1] - rpy[1]) / self.dt_mpc, v_max), -v_max)
+ self.referenceStates[10, 2:] = 0.
+
+ for k in range(1, self.n_steps + 1):
+ i, j = self.map.map_index(self.referenceStates[0, k], self.referenceStates[1, k])
+ z = self.result[0] * self.map.x[i] + self.result[1] * self.map.y[j] + self.result[2]
+ self.referenceStates[2, k] = z + self.h_ref
+ if k == 1:
+ self.surface_point = z
+
+ v_max = 0.1 # m.s-1
+ self.referenceStates[8, 1] = max(min((self.referenceStates[2, 1] - q[2]) / self.dt_mpc, v_max), -v_max)
+ self.referenceStates[8, 2:] = (self.referenceStates[2, 2] - self.referenceStates[2, 1]) / self.dt_mpc
+
+ if new_step:
+ self.compute_configurations(q, v_ref, self.result)
+
+ def compute_mean_surface(self, q):
+ ''' Compute the surface equation to fit the heightmap, [a,b,c] such as ax + by -z +c = 0
+ Args :
+ - q (array 3x) : current [x,y,z] position in world frame
+ '''
+ # Fit the map
+ i_min, j_min = self.map.map_index(q[0] - self.FIT_SIZE_X, q[1] - self.FIT_SIZE_Y)
+ i_max, j_max = self.map.map_index(q[0] + self.FIT_SIZE_X, q[1] + self.FIT_SIZE_Y)
+
+ n_points = (i_max - i_min) * (j_max - j_min)
+ A = np.zeros((n_points, 3))
+ b = np.zeros(n_points)
+ i_pb = 0
+ for i in range(i_min, i_max):
+ for j in range(j_min, j_max):
+ A[i_pb, :] = [self.map.x[i], self.map.y[j], 1.]
+ b[i_pb] = self.map.zv[i, j]
+ i_pb += 1
+
+ return solve_least_square(np.array(A), np.array(b)).x
+
+ def compute_configurations(self, q, v_ref, result):
+ """
+ Compute the surface equation to fit the heightmap, [a,b,c] such as ax + by -z +c = 0
+ Args :
+ - q (array 6x) : current [x,y,z, r, p, y] position in world frame
+ - v_ref (array 6x) : cdesired velocity in world frame
+ """
+ for k, config in enumerate(self.configs):
+ dt = self.T_step * k
+ config[:2] = q[:2]
+ if v_ref[5] < 10e-3:
+ config[0] += v_ref[0] * dt
+ config[1] += v_ref[1] * dt
+ else:
+ config[0] += (v_ref[0] * np.sin(v_ref[5] * dt) + v_ref[1] * (np.cos(v_ref[5] * dt) - 1.)) / v_ref[5]
+ config[1] += (v_ref[1] * np.sin(v_ref[5] * dt) - v_ref[0] * (np.cos(v_ref[5] * dt) - 1.)) / v_ref[5]
+
+ rpy_config = np.zeros(3)
+ rpy_config[2] = q[5] + v_ref[5] * dt
+
+ # Update according to heightmap
+ i, j = self.map.map_index(config[0], config[1])
+ config[2] = result[0] * self.map.x[i] + result[1] * self.map.y[j] + result[2] + self.h_ref
+
+ rpy_map = np.zeros(3)
+ rpy_map[0] = -np.arctan2(result[1], 1.)
+ rpy_map[1] = -np.arctan2(result[0], 1.)
+
+ rpy_config[0] = rpy_map[0] * np.cos(rpy_config[2]) - rpy_map[1] * np.sin(rpy_config[2])
+ rpy_config[1] = rpy_map[0] * np.sin(rpy_config[2]) + rpy_map[1] * np.cos(rpy_config[2])
+ quat = pin.Quaternion(pin.rpy.rpyToMatrix(rpy_config))
+ config[3:7] = [quat.x, quat.y, quat.z, quat.w]
+
+ def getReferenceStates(self):
+ return self.referenceStates
+
+ # def print_profile(self, output_file):
+ # ''' Print the profile computed with cProfile
+ # Args :
+ # - output_file (str) : file name
+ # '''
+ # profileWrap.print_stats(output_file)
+
+ # return 0
diff --git a/scripts/solo3D/SurfacePlanner.py b/scripts/solo3D/SurfacePlanner.py
new file mode 100644
index 00000000..ac022b0f
--- /dev/null
+++ b/scripts/solo3D/SurfacePlanner.py
@@ -0,0 +1,270 @@
+import pinocchio as pin
+import numpy as np
+import os
+from time import perf_counter as clock
+
+from sl1m.problem_definition import Problem
+from sl1m.generic_solver import solve_MIP
+# import matplotlib.pyplot as plt
+# import sl1m.tools.plot_tools as plot
+
+from solo_rbprm.solo_abstract import Robot as SoloAbstract
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+from hpp.corbaserver.rbprm.tools.surfaces_from_path import getAllSurfacesDict
+from hpp.corbaserver.problem_solver import ProblemSolver
+from hpp.gepetto import ViewerFactory
+
+# from solo3D.tools.ProfileWrapper import ProfileWrapper
+
+# # Store the results from cprofile
+# profileWrap = ProfileWrapper()
+
+# --------------------------------- PROBLEM DEFINITION ---------------------------------------------------------------
+
+paths = [
+ os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/com_inequalities/feet_quasi_flat/",
+ os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/relative_effector_positions/"
+]
+limbs = ['FLleg', 'FRleg', 'HLleg', 'HRleg']
+others = ['FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT']
+rom_names = ['solo_LFleg_rom', 'solo_RFleg_rom', 'solo_LHleg_rom', 'solo_RHleg_rom']
+N_PHASE = 3
+
+class SurfacePlanner:
+ """
+ Choose the next surface to use by solving a MIP problem
+ """
+
+ def __init__(self, environment_URDF, T_gait, shoulders, n_phase):
+ """
+ Initialize the affordance tool and save the solo abstract rbprm builder, and surface dictionary
+ """
+ self.T_gait = T_gait
+ self.shoulders = shoulders
+ self.n_phase = n_phase
+
+ self.solo_abstract = SoloAbstract()
+ self.solo_abstract.setJointBounds("root_joint", [-5., 5., -5., 5., 0.241, 1.5])
+ self.solo_abstract.boundSO3([-3.14, 3.14, -0.01, 0.01, -0.01, 0.01])
+ self.solo_abstract.setFilter(rom_names)
+ for limb in rom_names:
+ self.solo_abstract.setAffordanceFilter(limb, ['Support'])
+ self.ps = ProblemSolver(self.solo_abstract)
+ self.vf = ViewerFactory(self.ps)
+ self.afftool = AffordanceTool()
+ self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+
+ self.afftool.loadObstacleModel(environment_URDF, "environment", self.vf, reduceSizes=[0.05, 0.0, 0.])
+ self.ps.selectPathValidation("RbprmPathValidation", 0.05)
+
+ self.all_surfaces = getAllSurfacesDict(self.afftool)
+
+ self.potential_surfaces = []
+
+ self.pb = Problem(limb_names=limbs, other_names=others, constraint_paths=paths)
+
+ def compute_gait(self, gait_in):
+ """
+ Get a gait matrix with only one line per phase
+ :param gait_in: gait matrix with several line per phase
+ :return: gait matrix
+ """
+ gait = [gait_in[0, :]]
+ # for i in range(1, gait_in.shape[0] - 1):
+ # if (gait_in[i, :] != gait[-1]).any():
+ # gait.append(gait_in[i, :])
+
+ # TODO only works if we the last phase is not a flying phase
+ # gait.pop(-1)
+ # gait = np.roll(gait, -2, axis=0)
+
+ for i in range(1, gait_in.shape[0] - 1):
+ new_phase = True
+ for row in gait:
+ if (gait_in[i, :] == row).any():
+ new_phase = False
+
+ if new_phase:
+ gait.append(gait_in[i, :])
+
+ gait = np.roll(gait, -2, axis=0)
+
+
+ return gait
+
+ def compute_step_length(self, o_v_ref):
+ """
+ Compute the step_length used for the cost
+ :param o_v_ref: desired velocity
+ :return: desired step_length
+ """
+ # TODO: Divide by number of phases in gait
+ # step_length = o_v_ref * self.T_gait/4
+ step_length = o_v_ref * self.T_gait / 2
+
+ return np.array([step_length[i][0] for i in range(2)])
+
+ def compute_effector_positions(self, configs):
+ """
+ Compute the step_length used for the cost
+ :param o_v_ref: desired velocity
+ :return: desired step_length
+ """
+ effector_positions = np.zeros((4, self.pb.n_phases, 2))
+ for phase in self.pb.phaseData:
+ for foot in phase.moving:
+ rpy = pin.rpy.matrixToRpy(pin.Quaternion(configs[phase.id][3:7]).toRotationMatrix())
+ shoulders = np.zeros(2)
+ shoulders[0] = self.shoulders[0, foot] * np.cos(rpy[2]) - self.shoulders[1, foot] * np.sin(rpy[2])
+ shoulders[1] = self.shoulders[0, foot] * np.sin(rpy[2]) + self.shoulders[1, foot] * np.cos(rpy[2])
+ effector_positions[foot][phase.id] = np.array(configs[phase.id][:2] + shoulders)
+
+ return effector_positions
+
+ def get_potential_surfaces(self, configs, gait):
+ """
+ Get the rotation matrix and surface condidates for each configuration in configs
+ :param configs: a list of successive configurations of the robot
+ :param gait: a gait matrix
+ :return: a list of surface candidates
+ """
+ surfaces_list = []
+ empty_list = False
+ for id, config in enumerate(configs):
+ stance_feet = np.nonzero(gait[id % len(gait)] == 1)[0]
+ previous_swing_feet = np.nonzero(gait[(id - 1) % len(gait)] == 0)[0]
+ moving_feet = stance_feet[np.in1d(stance_feet, previous_swing_feet, assume_unique=True)]
+ roms = np.array(rom_names)[moving_feet]
+
+ foot_surfaces = []
+ for rom in roms:
+ surfaces = []
+ surfaces_names = self.solo_abstract.clientRbprm.rbprm.getCollidingObstacleAtConfig(
+ config.tolist(), rom)
+ for name in surfaces_names:
+ surfaces.append(self.all_surfaces[name][0])
+
+ if not len(surfaces_names):
+ empty_list = True
+
+ # Sort and then convert to array
+ surfaces = sorted(surfaces)
+ surfaces_array = []
+ for surface in surfaces:
+ surfaces_array.append(np.array(surface).T)
+
+ # Add to surfaces list
+ foot_surfaces.append(surfaces_array)
+ surfaces_list.append(foot_surfaces)
+
+ return surfaces_list, empty_list
+
+ def retrieve_surfaces(self, surfaces, indices=None):
+ """
+ Get the surface vertices, inequalities and selected surface indices if need be
+ """
+ vertices = []
+ surfaces_inequalities = []
+ if indices is not None:
+ surface_indices = []
+ else:
+ surface_indices = None
+ first_phase_i = 0
+ second_phase_i = 0
+ for foot in range(4):
+ if foot in self.pb.phaseData[0].moving:
+ vertices.append(surfaces[0][first_phase_i])
+ surfaces_inequalities.append(self.pb.phaseData[0].S[first_phase_i])
+ if indices is not None:
+ surface_indices.append(indices[0][first_phase_i])
+ first_phase_i += 1
+ elif foot in self.pb.phaseData[1].moving:
+ vertices.append(surfaces[1][second_phase_i])
+ surfaces_inequalities.append(self.pb.phaseData[1].S[second_phase_i])
+ if indices is not None:
+ surface_indices.append(indices[1][second_phase_i])
+ second_phase_i += 1
+ else:
+ print("Error : the foot is not moving in any of the first two phases")
+
+ return vertices, surfaces_inequalities, surface_indices
+
+ def run(self, configs, gait_in, current_contacts, o_v_ref):
+ """
+ Select the nex surfaces to use
+ :param xref: successive states
+ :param gait: a gait matrix
+ :param current_contacts: the initial_contacts to use in the computation
+ :param o_v_ref: the desired velocity for the cost
+ :return: the selected surfaces for the first phase
+ """
+ t0 = clock()
+
+ R = [pin.XYZQUATToSE3(np.array(config)).rotation for config in configs]
+
+ gait = self.compute_gait(gait_in)
+
+ step_length = self.compute_step_length(o_v_ref)
+
+ surfaces, empty_list = self.get_potential_surfaces(configs, gait)
+
+ initial_contacts = [current_contacts[:, i].tolist() for i in range(4)]
+
+ self.pb.generate_problem(R, surfaces, gait, initial_contacts, c0=None, com=False)
+
+ # from IPython import embed
+ # embed()
+
+ if empty_list:
+ print("Surface planner: one step has no potential surface to use.")
+ vertices, inequalities, indices = self.retrieve_surfaces(surfaces)
+ return vertices, inequalities, indices, None, False
+
+ # effector_positions = self.compute_effector_positions(configs)
+ # costs = {"step_size": [1.0, step_length], "effector_positions": [10.0, effector_positions]}
+ costs = {"step_size": [1.0, step_length]}
+ pb_data = solve_MIP(self.pb, costs=costs, com=False)
+
+ if pb_data.success:
+ surface_indices = pb_data.surface_indices
+
+ selected_surfaces = []
+ for foot, index in enumerate(surface_indices[0]):
+ selected_surfaces.append(surfaces[0][foot][index])
+
+ t1 = clock()
+ if 1000. * (t1 - t0) > 150.:
+ print("Run took ", 1000. * (t1 - t0))
+
+ # import matplotlib.pyplot as plt
+ # import sl1m.tools.plot_tools as plot
+
+ # ax = plot.draw_whole_scene(self.all_surfaces)
+ # plot.plot_planner_result(pb_data.all_feet_pos, step_size=step_length, ax=ax, show=True)
+
+ vertices, inequalities, indices = self.retrieve_surfaces(surfaces, surface_indices)
+
+ return vertices, inequalities, indices, pb_data.all_feet_pos, True
+
+ else:
+ # ax = plot.draw_whole_scene(self.all_surfaces)
+ # plot.plot_initial_contacts(initial_contacts, ax=ax)
+ # ax.scatter([c[0] for c in configs], [c[1] for c in configs], [c[2] for c in configs], marker='o', linewidth=5)
+ # ax.plot([c[0] for c in configs], [c[1] for c in configs], [c[2] for c in configs])
+
+ # plt.show()
+
+ print("The MIP problem did NOT converge")
+ # TODO what if the problem did not converge ???
+
+ vertices, inequalities, indices = self.retrieve_surfaces(surfaces)
+
+ return vertices, inequalities, indices, None, False
+
+ # def print_profile(self, output_file):
+ # ''' Print the profile computed with cProfile
+ # Args :
+ # - output_file (str) : file name
+ # '''
+ # profileWrap.print_stats(output_file)
diff --git a/scripts/solo3D/SurfacePlannerWrapper.py b/scripts/solo3D/SurfacePlannerWrapper.py
new file mode 100644
index 00000000..587193a1
--- /dev/null
+++ b/scripts/solo3D/SurfacePlannerWrapper.py
@@ -0,0 +1,328 @@
+from solo3D.SurfacePlanner import SurfacePlanner
+
+from multiprocessing import Process, Lock
+from multiprocessing.sharedctypes import Value, Array
+from ctypes import Structure, c_double
+import ctypes
+
+import libquadruped_reactive_walking as lqrw
+
+from time import perf_counter as clock
+import numpy as np
+
+# TODO : Modify this, should not be defined here
+N_VERTICES_MAX = 4
+N_SURFACE_MAX = 10
+N_SURFACE_CONFIG = 3
+N_gait = 100
+N_POTENTIAL_SURFACE = 5
+N_FEET = 4
+N_PHASE = 3
+
+
+class SurfaceDataCtype(Structure):
+ ''' ctype data structure for the shared memory between processes, for surfaces
+ Ax <= b
+ If normal as inequalities : A = [A , n , -n] , b = [b , d + eps, -d-eps]
+ Normal as equality : A = [A , n] , b = [b , d]
+ A = inequality matrix, last line equality if normal as equality
+ b = inequality vector,
+ vertices = vertices of the surface [[x1,y1,z1],
+ [x2,y2,z2], ...]
+
+ on = Bool if the surface is used or not
+ TODO : if more than 4 vertices, add a variable for the number of vertice to reshape the appropriate buffer
+ '''
+ nvertices = 4
+ nrow = nvertices + 2
+ _fields_ = [('A', ctypes.c_double * 3 * nrow), ('b', ctypes.c_double * nrow),
+ ('vertices', ctypes.c_double * 3 * nvertices), ('on', ctypes.c_bool)]
+
+
+class DataOutCtype(Structure):
+ ''' ctype data structure for the shared memory between processes
+ Data Out, list of potential and the selected surfaces given by the MIP
+ Potential surfaces are used if the MIP has not converged
+ '''
+ _fields_ = [('potentialSurfaces', SurfaceDataCtype * N_POTENTIAL_SURFACE * N_FEET),
+ ('selectedSurfaces', SurfaceDataCtype * N_FEET), ('all_feet', ctypes.c_double * 12 * N_PHASE),
+ ('success', ctypes.c_bool)]
+
+
+class DataInCtype(Structure):
+ ''' ctype data structure for the shared memory between processes
+ TODO : if more than 4 vertices, add a variable for the number of vertice to reshape the appropriate buffer
+ '''
+ _fields_ = [('gait', ctypes.c_int64 * 4 * N_gait), ('configs', ctypes.c_double * 7 * N_SURFACE_CONFIG),
+ ('o_vref', ctypes.c_double * 6), ('contacts', ctypes.c_double * 12), ('iteration', ctypes.c_int64)]
+
+
+class SurfacePlanner_Wrapper():
+ ''' Wrapper for the class SurfacePlanner for the paralellisation
+ '''
+
+ def __init__(self, params):
+ self.urdf = params.environment_URDF
+ self.T_gait = params.T_gait
+ self.shoulders = np.reshape(params.shoulders.tolist(), (3,4), order = "F")
+
+ # TODO : Modify this
+ # Usefull for 1st iteration of QP
+ A = [[-1.0000000, 0.0000000, 0.0000000],
+ [0.0000000, -1.0000000, 0.0000000],
+ [0.0000000, 1.0000000, 0.0000000],
+ [1.0000000, 0.0000000, 0.0000000],
+ [0.0000000, 0.0000000, 1.0000000],
+ [-0.0000000, -0.0000000, -1.0000000]]
+
+ b = [1.3946447, 0.9646447, 0.9646447, 0.5346446, 0.0000, 0.0000]
+
+ vertices = [[-1.3946447276978748, 0.9646446609406726, 0.0], [-1.3946447276978748, -0.9646446609406726, 0.0],
+ [0.5346445941834704, -0.9646446609406726, 0.0], [0.5346445941834704, 0.9646446609406726, 0.0]]
+
+ self.floor_surface = lqrw.Surface(np.array(A), np.array(b), np.array(vertices))
+
+ # Results from MIP
+ # Potential and selected surfaces for QP planner
+ self.potential_surfaces = lqrw.SurfaceVectorVector()
+ self.selected_surfaces = lqrw.SurfaceVector()
+
+ self.all_feet_pos = []
+
+ # MIP status
+ self.mip_iteration = 0
+ self.mip_success = False
+ self.first_iteration = True
+
+ # MIP status synchronous, this is updated just after the run of SL1M,
+ # but used in the controller only at the next flying phase
+ self.mip_iteration_syn = 0
+ self.mip_success_syn = False
+
+ self.multiprocessing = params.enable_multiprocessing_mip
+ if self.multiprocessing: # Setup variables in the shared memory
+ self.newData = Value('b', False)
+ self.newResult = Value('b', True)
+ self.running = Value('b', True)
+
+ # Data Out :
+ self.dataOut = Value(DataOutCtype)
+ # Data IN :
+ self.dataIn = Value(DataInCtype)
+
+ else:
+ self.surfacePlanner = SurfacePlanner(self.urdf, self.T_gait, self.shoulders, N_PHASE)
+
+ # Store results to mimic multiprocessing behaviour with synchronous loop
+ self.selected_surfaces_syn = lqrw.SurfaceVector()
+ self.all_feet_pos_syn = []
+
+ def run(self, configs, gait_in, current_contacts, o_v_ref):
+ if self.multiprocessing:
+ self.run_asynchronous(configs, gait_in, current_contacts, o_v_ref)
+ else:
+ self.run_synchronous(configs, gait_in, current_contacts, o_v_ref)
+
+ def run_synchronous(self, configs, gait_in, current_contacts, o_v_ref):
+ surfaces, surface_inequalities, surfaces_indices, all_feet_pos, success = self.surfacePlanner.run(
+ configs, gait_in, current_contacts, o_v_ref)
+ self.mip_iteration_syn += 1
+ self.mip_success_syn = success
+
+ # Retrieve potential data, usefull if solver not converged
+ self.potential_surfaces = lqrw.SurfaceVectorVector()
+ for foot, foot_surfaces in enumerate(surface_inequalities):
+ list_surfaces = lqrw.SurfaceVector()
+ for i, (S, s) in enumerate(foot_surfaces):
+ list_surfaces.append(lqrw.Surface(S, s, surfaces[foot][i].T))
+ self.potential_surfaces.append(list_surfaces)
+
+ # Use directly the MIP surfaces computed
+ # self.selected_surfaces = lqrw.SurfaceVector()
+ # if success:
+ # for foot, foot_surfaces in enumerate(surface_inequalities):
+ # i = surfaces_indices[foot]
+ # S, s = foot_surfaces[i]
+ # self.selected_surfaces.append(lqrw.Surface(S, s, surfaces[foot][i].T))
+
+ # self.all_feet_pos = all_feet_pos.copy()
+
+ # Mimic the multiprocessing behaviour, store the resuts and get them with update function
+ self.selected_surfaces_syn = lqrw.SurfaceVector()
+ if success:
+ for foot, foot_surfaces in enumerate(surface_inequalities):
+ i = surfaces_indices[foot]
+ S, s = foot_surfaces[i]
+ self.selected_surfaces_syn.append(lqrw.Surface(S, s, surfaces[foot][i].T))
+
+ self.all_feet_pos_syn = all_feet_pos.copy()
+
+ def run_asynchronous(self, configs, gait_in, current_contacts, o_v_ref):
+
+ # If this is the first iteration, creation of the parallel process
+ with self.dataIn.get_lock():
+ if self.dataIn.iteration == 0:
+ p = Process(target=self.create_MIP_asynchronous,
+ args=(self.newData, self.newResult, self.running, self.dataIn, self.dataOut))
+ p.start()
+ # Stacking data to send them to the parallel process
+ self.compress_dataIn(configs, gait_in, current_contacts, o_v_ref)
+ self.newData.value = True
+
+ def create_MIP_asynchronous(self, newData, newResult, running, dataIn, dataOut):
+ while running.value:
+ # Checking if new data is available to trigger the asynchronous MPC
+ if newData.value:
+ # Set the shared variable to false to avoid re-trigering the asynchronous MPC
+ newData.value = False
+
+ configs, gait_in, o_v_ref, current_contacts = self.decompress_dataIn(dataIn)
+
+ with self.dataIn.get_lock():
+ if self.dataIn.iteration == 0:
+ loop_planner = SurfacePlanner(self.urdf, self.T_gait, self.shoulders, N_PHASE)
+
+ surfaces, surface_inequalities, surfaces_indices, all_feet_pos, success = loop_planner.run(
+ configs, gait_in, current_contacts, o_v_ref)
+
+ with self.dataIn.get_lock():
+ self.dataIn.iteration += 1
+
+ t1 = clock()
+ self.compress_dataOut(surfaces, surface_inequalities, surfaces_indices, all_feet_pos, success)
+ t2 = clock()
+ # print("TIME COMPRESS DATA [ms] : ", 1000 * (t2 - t1))
+
+ # Set shared variable to true to signal that a new result is available
+ newResult.value = True
+
+ def compress_dataIn(self, configs, gait_in, current_contacts, o_v_ref):
+
+ with self.dataIn.get_lock():
+
+ for i, config in enumerate(configs):
+ dataConfig = np.frombuffer(self.dataIn.configs[i])
+ dataConfig[:] = config[:]
+
+ gait = np.frombuffer(self.dataIn.gait).reshape((N_gait, 4))
+ gait[:, :] = gait_in
+
+ o_vref = np.frombuffer(self.dataIn.o_vref)
+ o_vref[:] = o_v_ref[:, 0]
+
+ contact = np.frombuffer(self.dataIn.contacts).reshape((3, 4))
+ contact[:, :] = current_contacts[:, :]
+
+ def decompress_dataIn(self, dataIn):
+
+ with dataIn.get_lock():
+
+ configs = [np.frombuffer(config) for config in dataIn.configs]
+
+ gait = np.frombuffer(self.dataIn.gait).reshape((N_gait, 4))
+
+ o_v_ref = np.frombuffer(self.dataIn.o_vref).reshape((6, 1))
+
+ contacts = np.frombuffer(self.dataIn.contacts).reshape((3, 4))
+
+ return configs, gait, o_v_ref, contacts
+
+ def compress_dataOut(self, surfaces, surface_inequalities, surfaces_indices, all_feet_pos, success):
+ # Modify this
+ nvertices = 4
+ nrow = nvertices + 2
+
+ with self.dataOut.get_lock():
+ # Compress potential surfaces :
+ for foot, foot_surfaces in enumerate(surface_inequalities):
+ i = 0
+ for i, (S, s) in enumerate(foot_surfaces):
+ A = np.frombuffer(self.dataOut.potentialSurfaces[foot][i].A).reshape((nrow, 3))
+ b = np.frombuffer(self.dataOut.potentialSurfaces[foot][i].b)
+ vertices = np.frombuffer(self.dataOut.potentialSurfaces[foot][i].vertices).reshape((nvertices, 3))
+ A[:, :] = S[:, :]
+ b[:] = s[:]
+ vertices[:, :] = surfaces[foot][i].T[:, :]
+ self.dataOut.potentialSurfaces[foot][i].on = True
+
+ for j in range(i + 1, N_POTENTIAL_SURFACE):
+ self.dataOut.potentialSurfaces[foot][j].on = False
+
+ if success:
+ self.dataOut.success = True
+ # self.dataOut.all_feet_pos = all_feet_pos
+
+ # Compress selected surfaces
+ for foot, index in enumerate(surfaces_indices):
+ A = np.frombuffer(self.dataOut.selectedSurfaces[foot].A).reshape((nrow, 3))
+ b = np.frombuffer(self.dataOut.selectedSurfaces[foot].b)
+ vertices = np.frombuffer(self.dataOut.selectedSurfaces[foot].vertices).reshape((nvertices, 3))
+ A[:, :] = surface_inequalities[foot][index][0][:, :]
+ b[:] = surface_inequalities[foot][index][1][:]
+ vertices = surfaces[foot][index].T[:, :]
+ self.dataOut.selectedSurfaces[foot].on = True
+
+ else:
+ self.dataOut.success = False
+
+ def update_latest_results(self):
+ ''' Update latest results : 2 list
+ - potential surfaces : used if MIP has not converged
+ - selected_surfaces : surfaces selected for the next phases
+ '''
+ if self.multiprocessing:
+ with self.dataOut.get_lock():
+ if self.newResult.value:
+ self.newResult.value = False
+
+ self.potential_surfaces = lqrw.SurfaceVectorVector()
+ for foot_surfaces in self.dataOut.potentialSurfaces:
+ list_surfaces = lqrw.SurfaceVector()
+ for s in foot_surfaces:
+ if s.on:
+ list_surfaces.append(lqrw.Surface(np.array(s.A), np.array(s.b), np.array(s.vertices)))
+ self.potential_surfaces.append(list_surfaces)
+
+ self.selected_surfaces = lqrw.SurfaceVector()
+
+ if self.dataOut.success:
+ self.mip_success = True
+ self.mip_iteration += 1
+
+ for s in self.dataOut.selectedSurfaces:
+ self.selected_surfaces.append(
+ lqrw.Surface(np.array(s.A), np.array(s.b), np.array(s.vertices)))
+
+ # self.all_feet_pos = self.dataOut.all_feet_pos.copy()
+ # for foot in self.all_feet_pos:
+ # foot.pop(0)
+
+ else:
+ self.mip_success = False
+ self.mip_iteration += 1
+
+ else:
+ # TODO : So far, only the convergence or not of the solver has been taken into account,
+ # What if the solver take too long ?
+ pass
+ else:
+ # Results have been already updated
+ self.mip_success = self.mip_success_syn
+ self.mip_iteration = self.mip_iteration_syn
+
+ if self.mip_success:
+ self.selected_surfaces = self.selected_surfaces_syn
+ self.all_feet_pos = self.all_feet_pos_syn.copy()
+
+ def stop_parallel_loop(self):
+ """Stop the infinite loop in the parallel process to properly close the simulation
+ """
+
+ self.running.value = False
+
+ # def print_profile(self, output_file):
+ # ''' Print the profile computed with cProfile
+ # Args :
+ # - output_file (str) : file name
+ # '''
+ # profileWrap.print_stats(output_file)
diff --git a/scripts/solo3D/tools/Surface.py b/scripts/solo3D/tools/Surface.py
new file mode 100644
index 00000000..f19cd9c4
--- /dev/null
+++ b/scripts/solo3D/tools/Surface.py
@@ -0,0 +1,279 @@
+import pickle
+import numpy as np
+import matplotlib.pyplot as plt
+import mpl_toolkits.mplot3d as a3
+import math
+from scipy.spatial import ConvexHull
+
+
+def plane_intersect(P1, P2):
+ """ Get the intersection between 2 plan, return Point and direction
+
+:param P1,P2: Plan equalities
+ np.array([a,b,c,d])
+ ax + by + cz + d = 0
+
+
+Returns : 1 point and 1 direction vect of the line of intersection, np.arrays, shape (3,)
+
+"""
+
+ P1_normal, P2_normal = P1[:3], P2[:3]
+
+ aXb_vec = np.cross(P1_normal, P2_normal)
+
+ A = np.array([P1_normal, P2_normal, aXb_vec])
+ d = np.array([-P1[3], -P2[3], 0.]).reshape(3, 1)
+
+ # could add np.linalg.det(A) == 0 test to prevent linalg.solve throwing error
+
+ p_inter = np.linalg.solve(A, d).T
+
+ return p_inter[0], (p_inter + aXb_vec)[0]
+
+
+def LinePlaneCollision(P, A, B, epsilon=1e-6):
+ """ Get the intersection point between 1 plane and 1 line
+
+:param P: Plane equality
+ np.array([a,b,c,d])
+ ax + by + cz + d = 0
+param A,B : 2 points defining the line np.arrays, shape(3,)
+
+
+Returns : 1 point, np.array, shape (3,)
+"""
+ plane_normal = P[:3]
+ if P[0] == 0:
+ if P[1] == 0:
+ planePoint = np.array([0, 0, -P[-1] / P[2]]) # a,b = 0 --> z = -d/c
+ else:
+ planePoint = np.array([0, -P[-1] / P[1], 0]) # a,c = 0 --> y = -d/b
+ else:
+ planePoint = np.array([-P[-1] / P[0], 0., 0]) # b,c = 0 --> x = -d/a
+
+ rayDirection = A - B
+ ndotu = plane_normal.dot(rayDirection)
+ if abs(ndotu) < epsilon:
+ raise RuntimeError("no intersection or line is within plane")
+
+ w = A - planePoint
+ si = -plane_normal.dot(w) / ndotu
+ Psi = w + si * rayDirection + planePoint
+ return Psi
+
+
+class Surface():
+
+ def __init__(self,
+ vertices=np.zeros((3, 3)),
+ vertices_inner=None,
+ ineq=None,
+ ineq_vect=None,
+ normal=None,
+ order_bool=False,
+ margin=0.):
+ # Inital surface equations
+ self.vertices = vertices
+ self.ineq = ineq
+ self.ineq_vect = ineq_vect
+ # Inner surface equations
+ self.vertices_inner = vertices_inner
+ self.ineq_inner = ineq
+ self.ineq_vect_inner = ineq_vect
+
+ self.normal = normal
+ self.order_bool = False
+ self.margin = margin
+
+ def compute_all_inequalities(self):
+ """" Compute all inequalities, update inner vertices...
+ """
+ self.compute_inequalities()
+ self.compute_inner_inequalities()
+ self.compute_inner_vertices()
+ return 0
+
+ def norm(self, sq):
+ """" norm ...
+ """
+ cr = np.cross(sq[2] - sq[0], sq[1] - sq[0])
+ return np.abs(cr / np.linalg.norm(cr))
+
+ def order(self, method="convexHull"):
+ """" Order the array of vertice in counterclock wise using convex Hull method
+ """
+ if not (self.order_bool):
+ if len(self.vertices) <= 3:
+ return 0
+ v = np.unique(self.vertices, axis=0)
+ n = self.norm(v[:3])
+ y = np.cross(n, v[1] - v[0])
+ y = y / np.linalg.norm(y)
+ c = np.dot(v, np.c_[v[1] - v[0], y])
+ if method == "convexHull":
+ h = ConvexHull(c)
+ self.vertices = v[h.vertices]
+ else:
+ mean = np.mean(c, axis=0)
+ d = c - mean
+ s = np.arctan2(d[:, 0], d[:, 1])
+ self.vertices = v[np.argsort(s)]
+ self.order_bool = True
+ return 0
+
+ def compute_inequalities(self):
+ """Compute surface inequalities from the vertices list
+ --> update self.ineq, self.normal,self.ineq_vect
+ S x <= d
+ the last row contains the equality vector
+ Vertice of the surface = [[x1 ,y1 ,z1 ]
+ [x2 ,y2 ,z2 ]
+ ... ]]
+ """
+ vert = self.vertices
+ nb_vert = vert.shape[0]
+ # In .obj, by convention, the direction of the normal AB cross AC
+ # is outside the object
+ # We know that only the 3 first point are in good size
+ S_normal = np.cross(vert[0, :] - vert[1, :], vert[0, :] - vert[2, :])
+ self.normal = S_normal / np.linalg.norm(S_normal)
+
+ self.ineq = np.zeros((nb_vert + 1, 3))
+ self.ineq_vect = np.zeros((nb_vert + 1))
+
+ self.ineq[-1, :] = self.normal
+ self.ineq_vect[-1] = -(-self.normal[0] * vert[0, 0] - self.normal[1] * vert[0, 1] -
+ self.normal[2] * vert[0, 2])
+
+ # Order the whole list (convex hull on 2D order in counterclock wise)
+ # Not ordering the list for the previous step is importamt since the 3 first points comes from
+ # the .obj, and with the convex, we obtain the proper direction for the normal
+ self.order()
+ self.order_bool = True
+ vert = self.vertices
+
+ for i in range(nb_vert):
+
+ if i < nb_vert - 1:
+ AB = vert[i, :] - vert[i + 1, :]
+ else:
+ AB = vert[i, :] - vert[0, :] # last point of the list with first
+
+ n_plan = np.cross(AB, self.normal)
+ n_plan = n_plan / np.linalg.norm(n_plan)
+
+ # normal = [a,b,c].T
+ # To keep the half space in the direction of the normal :
+ # ax + by + cz + d >= 0
+ # - [a,b,c] * X <= d
+
+ self.ineq[i, :] = -np.array([n_plan[0], n_plan[1], n_plan[2]])
+ self.ineq_vect[i] = -n_plan[0] * vert[i, 0] - n_plan[1] * vert[i, 1] - n_plan[2] * vert[i, 2]
+
+ return 0
+
+ def compute_inner_inequalities(self):
+ """Compute surface inequalities from the vertices list with a margin, update self.ineq_inner,
+ self.ineq_vect_inner
+ ineq_iner X <= ineq_vect_inner
+ the last row contains the equality vector
+ Keyword arguments:
+ Vertice of the surface = [[x1 ,y1 ,z1 ]
+ [x2 ,y2 ,z2 ]
+ ... ]]
+ """
+ if self.ineq is None or self.normal is None or self.ineq_vect is None:
+ self.compute_inequalities()
+
+ nb_vert = self.vertices.shape[0]
+ self.ineq_inner = np.zeros((nb_vert + 1, 3))
+ self.ineq_vect_inner = np.zeros((nb_vert + 1))
+
+ # same normal vector
+ self.ineq_inner[-1, :] = self.ineq[-1, :]
+ self.ineq_vect_inner[-1] = self.ineq_vect[-1]
+
+ for i in range(nb_vert):
+
+ if i < nb_vert - 1:
+ AB = self.vertices[i, :] - self.vertices[i + 1, :]
+ else:
+ AB = self.vertices[i, :] - self.vertices[0, :] # last point of the list with first
+
+ n_plan = np.cross(AB, self.normal)
+ n_plan = n_plan / np.linalg.norm(n_plan)
+
+ # normal = [a,b,c].T
+ # To keep the half space in the direction of the normal :
+ # ax + by + cz + d >= 0
+ # - [a,b,c] * X <= d
+
+ # Take a point M along the normal of the plan, from a distance margin
+ # OM = OA + AM = OA + margin*n_plan
+
+ M = self.vertices[i, :] + self.margin * n_plan
+
+ # Create the parallel plan that pass trhough M
+ self.ineq_inner[i, :] = -np.array([n_plan[0], n_plan[1], n_plan[2]])
+ self.ineq_vect_inner[i] = -n_plan[0] * M[0] - n_plan[1] * M[1] - n_plan[2] * M[2]
+
+ return 0
+
+ def compute_inner_vertices(self):
+ """" Compute the list of vertice defining the inner surface :
+ update self.vertices_inner = = [[x1 ,y1 ,z1 ] shape((nb vertice , 3))
+ [x2 ,y2 ,z2 ]
+ ... ]]
+ """
+ S_inner = []
+ nb_vert = self.vertices.shape[0]
+
+ if self.ineq is None or self.normal is None or self.ineq_vect is None:
+ self.compute_inequalities()
+ self.compute_inner_inequalities()
+
+ # P = np.array([a,b,c,d]) , (Plan) ax + by + cz + d = 0
+ P_normal = np.zeros(4)
+ P_normal[:3] = self.ineq[-1, :]
+ P_normal[-1] = -self.ineq_vect[-1]
+
+ P1, P2 = np.zeros(4), np.zeros(4)
+
+ for i in range(nb_vert):
+ if i < nb_vert - 1:
+ P1[:3], P2[:3] = self.ineq_inner[i, :], self.ineq_inner[i + 1, :]
+ P1[-1], P2[-1] = -self.ineq_vect_inner[i], -self.ineq_vect_inner[i + 1]
+
+ A, B = plane_intersect(P1, P2)
+ S_inner.append(LinePlaneCollision(P_normal, A, B))
+ else:
+ P1[:3], P2[:3] = self.ineq_inner[i, :], self.ineq_inner[0, :]
+ P1[-1], P2[-1] = -self.ineq_vect_inner[i], -self.ineq_vect_inner[0]
+
+ A, B = plane_intersect(P1, P2)
+ S_inner.append(LinePlaneCollision(P_normal, A, B))
+
+ self.vertices_inner = np.array(S_inner)
+ return 0
+
+ def isPointInside(self, pt, epsilon=10e-4):
+ """" Compute if pt is inside the surface
+ Params : pt : np.array, shape(3,) , [x,y,z]
+ epsilon : float, for the equality
+ return : Bool, inside surface or not
+ """
+
+ if abs(np.dot(self.ineq[-1, :], pt) - self.ineq_vect[-1]) < epsilon:
+ # inside the plan
+ Sx = np.dot(self.ineq[:-1, :], pt)
+ return np.sum(Sx <= self.ineq_vect[:-1]) == len(self.vertices)
+
+ else:
+ return False
+
+ def isInsideIneq(self, pt, epsilon=10e-4):
+
+ Sx = np.dot(self.ineq[:-1, :], pt)
+
+ return np.sum(Sx <= self.ineq_vect[:-1]) == len(self.vertices)
diff --git a/scripts/solo3D/tools/heightmap_generator.py b/scripts/solo3D/tools/heightmap_generator.py
new file mode 100644
index 00000000..b9a090d0
--- /dev/null
+++ b/scripts/solo3D/tools/heightmap_generator.py
@@ -0,0 +1,67 @@
+import matplotlib.pyplot as plt
+import os
+
+import sl1m.tools.plot_tools as plot
+from sl1m.tools.plot_tools import draw_whole_scene
+from solo3D.tools.heightmap_tools import Heightmap
+
+from solo_rbprm.solo_abstract import Robot
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+from hpp.corbaserver.rbprm.tools.surfaces_from_path import getAllSurfacesDict
+from hpp.corbaserver.problem_solver import ProblemSolver
+from hpp.gepetto import ViewerFactory
+import libquadruped_reactive_walking as lqrw
+
+# --------------------------------- PROBLEM DEFINITION ---------------------------------------------------------------
+params = lqrw.Params()
+
+N_X = 100
+N_Y = 100
+X_BOUNDS = [-0.5, 1.5]
+Y_BOUNDS = [-1.5, 1.5]
+
+rom_names = ['solo_LFleg_rom', 'solo_RFleg_rom', 'solo_LHleg_rom', 'solo_RHleg_rom']
+others = ['FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT']
+LIMBS = ['solo_RHleg_rom', 'solo_LHleg_rom', 'solo_LFleg_rom', 'solo_RFleg_rom']
+paths = [
+ os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/com_inequalities/feet_quasi_flat/",
+ os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/relative_effector_positions/"
+]
+# --------------------------------- METHODS ---------------------------------------------------------------
+
+def init_afftool():
+ """
+ Initialize the affordance tool and return the solo abstract rbprm builder, the surface
+ dictionary and all the affordance points
+ """
+ robot = Robot()
+ robot.setJointBounds("root_joint", [-5., 5., -5., 5., 0.241, 1.5])
+ robot.boundSO3([-3.14, 3.14, -0.01, 0.01, -0.01, 0.01])
+ robot.setFilter(LIMBS)
+ for limb in LIMBS:
+ robot.setAffordanceFilter(limb, ['Support'])
+ ps = ProblemSolver(robot)
+ vf = ViewerFactory(ps)
+ afftool = AffordanceTool()
+ afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+ afftool.loadObstacleModel(params.environment_URDF, "environment", vf)
+ ps.selectPathValidation("RbprmPathValidation", 0.05)
+
+ return afftool
+
+
+# --------------------------------- MAIN ---------------------------------------------------------------
+if __name__ == "__main__":
+ afftool = init_afftool()
+ affordances = afftool.getAffordancePoints('Support')
+ all_surfaces = getAllSurfacesDict(afftool)
+
+ heightmap = Heightmap(N_X, N_Y, X_BOUNDS, Y_BOUNDS)
+ heightmap.build(affordances)
+ # heightmap.save_pickle(ENV_HEIGHTMAP)
+ heightmap.save_binary(params.environment_heightmap)
+
+ ax_heightmap = plot.plot_heightmap(heightmap)
+ draw_whole_scene(all_surfaces)
+ plt.show(block = True)
diff --git a/scripts/solo3D/tools/heightmap_tools.py b/scripts/solo3D/tools/heightmap_tools.py
new file mode 100644
index 00000000..b095a7f4
--- /dev/null
+++ b/scripts/solo3D/tools/heightmap_tools.py
@@ -0,0 +1,173 @@
+from numpy import identity, zeros, ones, array
+import numpy as np
+import matplotlib.pyplot as plt
+import mpl_toolkits.mplot3d as a3
+import hppfcl
+from time import perf_counter as clock
+import trimesh
+import pickle
+import ctypes
+
+
+class MapHeader(ctypes.Structure):
+ _fields_ = [
+ ("size_x", ctypes.c_int),
+ ("size_y", ctypes.c_int),
+ ("x_init", ctypes.c_double),
+ ("x_end", ctypes.c_double),
+ ("y_init", ctypes.c_double),
+ ("y_end", ctypes.c_double),
+ ]
+
+
+class Heightmap:
+
+ def __init__(self, n_x, n_y, x_lim, y_lim):
+ """
+ :param n_x number of samples in x
+ :param n_y number of samples in y
+ :param x_lim bounds in x
+ :param y_lim bounds in y
+ """
+
+ self.n_x = n_x
+ self.n_y = n_y
+
+ self.x = np.linspace(x_lim[0], x_lim[1], n_x)
+ self.y = np.linspace(y_lim[0], y_lim[1], n_y)
+
+ self.xv, self.yv = np.meshgrid(self.x, self.y, sparse=False, indexing='ij')
+ self.zv = np.zeros((n_x, n_y))
+
+ def save_pickle(self, filename):
+ filehandler = open(filename, 'wb')
+ pickle.dump(self, filehandler)
+
+ def save_binary(self, filename):
+ """
+ Save heightmap matrix as binary file.
+ Args :
+ - filename (str) : name of the file saved.
+ """
+
+ arr_bytes = self.zv.astype(ctypes.c_double).tobytes()
+ h = MapHeader(self.n_x, self.n_y, self.x[0], self.x[-1], self.y[0], self.y[-1])
+ h_bytes = bytearray(h)
+
+ with open(filename, "ab") as f:
+ f.truncate(0)
+ f.write(h_bytes)
+ f.write(arr_bytes)
+
+ def build(self, affordances):
+ """
+ Build the heightmap and return it
+ For each slot in the grid create a vertical segment and check its collisions with the
+ affordances until one is found
+ :param affordances list of affordances
+ """
+ for i in range(self.n_x):
+ for j in range(self.n_y):
+ p1 = np.array([self.xv[i, j], self.yv[i, j], -1.])
+ p2 = np.array([self.xv[i, j], self.yv[i, j], 10.])
+ segment = np.array([p1, p2])
+ fcl_segment = convex(segment, [0, 1, 0])
+
+ intersections = []
+ for affordance in affordances:
+ fcl_affordance = affordance_to_convex(affordance)
+ if distance(fcl_affordance, fcl_segment) < 0:
+ for triangle_list in affordance:
+ triangle = [np.array(p) for p in triangle_list]
+ if intersect_line_triangle(segment, triangle):
+ intersections.append(get_point_intersect_line_triangle(segment, triangle)[2])
+
+ if len(intersections) != 0:
+ self.zv[i, j] = np.max(np.array(intersections))
+
+ def map_index(self, x, y):
+ """
+ Get the i, j indices of a given position in the heightmap
+ """
+ i = np.searchsorted(self.x, x) - 1
+ j = np.searchsorted(self.y, y) - 1
+ return i, j
+
+
+def affordance_to_convex(affordance):
+ """
+ Creates a hpp-FCL convex object with an affordance
+ """
+ vertices = hppfcl.StdVec_Vec3f()
+ faces = hppfcl.StdVec_Triangle()
+ for triangle_list in affordance:
+ [vertices.append(np.array(p)) for p in triangle_list]
+ faces.append(hppfcl.Triangle(0, 1, 2))
+ return hppfcl.Convex(vertices, faces)
+
+
+def convex(points, indices):
+ """
+ Creates a hpp-FCL convex object with a list of points and three indices of the vertices of the
+ triangle (or segment)
+ """
+ vertices = hppfcl.StdVec_Vec3f()
+ faces = hppfcl.StdVec_Triangle()
+ vertices.extend(points)
+ faces.append(hppfcl.Triangle(indices[0], indices[1], indices[2]))
+ return hppfcl.Convex(vertices, faces)
+
+
+def distance(object1, object2):
+ """
+ Returns the distance between object1 and object2
+ """
+ guess = np.array([1., 0., 0.])
+ support_hint = np.array([0, 0], dtype=np.int32)
+
+ shape = hppfcl.MinkowskiDiff()
+ shape.set(object1, object2, hppfcl.Transform3f(), hppfcl.Transform3f())
+ gjk = hppfcl.GJK(150, 1e-8)
+ gjk.evaluate(shape, guess, support_hint)
+ return gjk.distance
+
+
+# Method to intersect triangle and segment
+def signed_tetra_volume(a, b, c, d):
+ return np.sign(np.dot(np.cross(b - a, c - a), d - a) / 6.0)
+
+
+def intersect_line_triangle(segment, triangle):
+ s1 = signed_tetra_volume(segment[0], triangle[0], triangle[1], triangle[2])
+ s2 = signed_tetra_volume(segment[1], triangle[0], triangle[1], triangle[2])
+
+ if s1 != s2:
+ s3 = signed_tetra_volume(segment[0], segment[1], triangle[0], triangle[1])
+ s4 = signed_tetra_volume(segment[0], segment[1], triangle[1], triangle[2])
+ s5 = signed_tetra_volume(segment[0], segment[1], triangle[2], triangle[0])
+
+ if s3 == s4 and s4 == s5:
+ return True
+ else:
+ return False
+ else:
+ return False
+
+
+def get_point_intersect_line_triangle(segment, triangle):
+ s1 = signed_tetra_volume(segment[0], triangle[0], triangle[1], triangle[2])
+ s2 = signed_tetra_volume(segment[1], triangle[0], triangle[1], triangle[2])
+
+ if s1 != s2:
+ s3 = signed_tetra_volume(segment[0], segment[1], triangle[0], triangle[1])
+ s4 = signed_tetra_volume(segment[0], segment[1], triangle[1], triangle[2])
+ s5 = signed_tetra_volume(segment[0], segment[1], triangle[2], triangle[0])
+
+ if s3 == s4 and s4 == s5:
+ n = np.cross(triangle[1] - triangle[0], triangle[2] - triangle[0])
+ t = np.dot(triangle[0] - segment[0], n) / np.dot(segment[1] - segment[0], n)
+ return segment[0] + t * (segment[1] - segment[0])
+ else:
+ return np.zeros(3)
+ else:
+ return np.zeros(3)
diff --git a/scripts/solo3D/tools/pyb_environment_3D.py b/scripts/solo3D/tools/pyb_environment_3D.py
new file mode 100644
index 00000000..2fa07281
--- /dev/null
+++ b/scripts/solo3D/tools/pyb_environment_3D.py
@@ -0,0 +1,208 @@
+import numpy as np
+import pybullet as pyb
+import pybullet_data
+
+class PybEnvironment3D():
+ ''' Class to vizualise the 3D environment and foot trajectory and in PyBullet simulation.
+ '''
+
+ def __init__(self, params, gait, statePlanner, footStepPlanner, footTrajectoryGenerator):
+ """
+ Store the solo3D class, used for trajectory vizualisation.
+ Args:
+ - params: parameters of the simulation.
+ - gait: Gait class.
+ - statePlanner: State planner class.
+ - footStepPlannerQP: Footstep planner class (QP version).
+ - footTrajectoryGenerator: Foot trajectory class (Bezier version).
+ """
+ self.enable_pyb_GUI = params.enable_pyb_GUI
+ self.URDF = params.environment_URDF
+ self.params = params
+
+ # Solo3D python class
+ self.footStepPlanner = footStepPlanner
+ self.statePlanner = statePlanner
+ self.gait = gait
+ self.footTrajectoryGenerator = footTrajectoryGenerator
+
+ self.n_points = 15
+ self.trajectory_Ids = np.zeros((3, 4, self.n_points))
+ self.sl1m_Ids_target = np.zeros((7, 4))
+
+ # Int to determine when refresh object position (k % refresh == 0)
+ self.refresh = 1
+
+ def update(self, k):
+ ''' Update position of the objects in pybullet environment.
+ Args :
+ - k (int) : step of the simulation.
+ '''
+ # On iteration 0, PyBullet env has not been started
+ if k == 1:
+ self.initializeEnv()
+
+ if self.enable_pyb_GUI and k > 1 and not self.params.enable_multiprocessing_mip:
+
+ if k % self.refresh == 0:
+
+ # Update target trajectory, current and next phase
+ self.updateTargetTrajectory()
+
+ return 0
+
+ def updateCamera(self, k, device):
+ # Update position of PyBullet camera on the robot position to do as if it was attached to the robot
+ if k > 10 and self.enable_pyb_GUI:
+ pyb.resetDebugVisualizerCamera(cameraDistance=0.95,
+ cameraYaw=357,
+ cameraPitch=-29,
+ cameraTargetPosition=[0.6, 0.14, -0.22])
+ # pyb.resetDebugVisualizerCamera(cameraDistance=1., cameraYaw=357, cameraPitch=-28,
+ # cameraTargetPosition=[device.dummyHeight[0], device.dummyHeight[1], 0.0])
+
+ return 0
+
+ def update_target_SL1M(self, all_feet_pos):
+ ''' Update position of the SL1M target
+ Args :
+ - all_feet_pos : list of optimized position such as : [[Foot 1 next_pos, None , Foot1 next_pos] , [Foot 2 next_pos, None , Foot2 next_pos] ]
+ '''
+
+ for i in range(len(all_feet_pos[0])):
+ for j in range(len(all_feet_pos)):
+ if all_feet_pos[j][i] is None:
+ pyb.resetBasePositionAndOrientation(int(self.sl1m_Ids_target[i, j]),
+ posObj=np.array([0., 0., -0.5]),
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ else:
+ pyb.resetBasePositionAndOrientation(int(self.sl1m_Ids_target[i, j]),
+ posObj=all_feet_pos[j][i],
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ return 0
+
+ def updateTargetTrajectory(self):
+ ''' Update the target trajectory for current and next phases. Hide the unnecessary spheres.
+ '''
+
+ gait = self.gait.getCurrentGait()
+ fsteps = self.footStepPlanner.getFootsteps()
+
+ for j in range(4):
+ # Count the position of the plotted trajectory in the temporal horizon
+ # c = 0 --> Current trajectory/foot pos
+ # c = 1 --> Next trajectory/foot pos
+ c = 0
+ i = 0
+
+ for i in range(gait.shape[0]):
+ # footsteps = fsteps[i].reshape((3, 4), order="F")
+ if i > 0:
+ if (1 - gait[i - 1, j]) * gait[i, j] > 0: # from flying phase to stance
+ if c == 0:
+ # Current flying phase, using coeff store in Bezier curve class
+ t0 = self.footTrajectoryGenerator.t0s[j]
+ t1 = self.footTrajectoryGenerator.t_swing[j]
+ t_vector = np.linspace(t0, t1, self.n_points)
+
+ for id_t, t in enumerate(t_vector):
+ # Bezier trajectory
+ pos = self.footTrajectoryGenerator.evaluateBezier(j, 0, t)
+ # Polynomial Curve 5th order
+ # pos = self.footTrajectoryGenerator.evaluatePoly(j, 0, t)
+ pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[c, j, id_t]),
+ posObj=pos,
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+ c += 1
+
+ else:
+ if gait[i, j] == 1:
+ # not hidden in the floor, traj
+ if not pyb.getBasePositionAndOrientation(int(self.trajectory_Ids[0, j, 0]))[0][2] == -0.1:
+ for t in range(self.n_points):
+ pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[0, j, t]),
+ posObj=np.array([0., 0., -0.1]),
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ c += 1
+
+ i += 1
+
+ # Hide the sphere objects not used
+ while c < self.trajectory_Ids.shape[0]:
+
+ # not hidden in the floor, traj
+ if not pyb.getBasePositionAndOrientation(int(self.trajectory_Ids[c, j, 0]))[0][2] == -0.1:
+ for t in range(self.n_points):
+ pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[c, j, t]),
+ posObj=np.array([0., 0., -0.1]),
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+
+ c += 1
+
+ return 0
+
+ def initializeEnv(self):
+ '''
+ Load objects in pybullet simulation.
+ '''
+ print("Loading PyBullet object ...")
+ pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+ # Load 3D environment
+ tmpId = pyb.loadURDF(self.URDF)
+ pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
+
+ # Sphere Object for trajcetories :
+ for i in range(self.trajectory_Ids.shape[0]):
+
+ # rgbaColor : [R , B , G , alpha opacity]
+ if i == 0:
+ rgba = [0.41, 1., 0., 1.]
+ else:
+ rgba = [0.41, 1., 0., 0.5]
+
+ mesh_scale = [0.0035, 0.0035, 0.0035]
+ visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ fileName="sphere_smooth.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=rgba,
+ specularColor=[0.4, .4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale)
+ for j in range(4):
+ for id_t in range(self.n_points):
+ self.trajectory_Ids[i, j, id_t] = pyb.createMultiBody(baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.0, 0.0, -0.1],
+ useMaximalCoordinates=True)
+ # Sphere Object for SLM
+ # 5 phases + init pos
+ for i in range(6):
+
+ rgba_list = [[1., 0., 0., 1.], [1., 0., 1., 1.], [1., 1., 0., 1.], [0., 0., 1., 1.]]
+ mesh_scale = [0.01, 0.01, 0.01]
+
+ for j in range(4):
+ rgba = rgba_list[j]
+ rgba[-1] = 1 - (1 / 9) * i
+ visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
+ fileName="sphere_smooth.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=rgba,
+ specularColor=[0.4, .4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale)
+
+ self.sl1m_Ids_target[i, j] = pyb.createMultiBody(baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.0, 0.0, -0.1],
+ useMaximalCoordinates=True)
+
+ print("PyBullet object loaded")
+
+ return 0
diff --git a/scripts/solo3D/tools/readme.txt b/scripts/solo3D/tools/readme.txt
new file mode 100644
index 00000000..d44f77a6
--- /dev/null
+++ b/scripts/solo3D/tools/readme.txt
@@ -0,0 +1,13 @@
+heightMapGenerator :
+
+Generate height map from .stl file object.
+
+- Add new_object.stl in a new folder : objects/new_folder
+- Modify path
+- Select X,Y range and number of points for the heighMap
+
+--> Generate new_folder/heightmap/heightMap.dat ( [x,y, [height,Surface associated index]])
+ new_folder/heightmap/surfaces.dat (liste of surfaces)
+
+
+--> Generate new_folder/meshes/object_* : useful to import object properly into pybullet (one single .stl do not allow to load properly the collision shape with .stl)
diff --git a/scripts/solo3D/tools/utils.py b/scripts/solo3D/tools/utils.py
new file mode 100644
index 00000000..f4970494
--- /dev/null
+++ b/scripts/solo3D/tools/utils.py
@@ -0,0 +1,74 @@
+import numpy as np
+
+def EulerToQuaternion(roll_pitch_yaw):
+ roll, pitch, yaw = roll_pitch_yaw
+ sr = np.sin(roll / 2.)
+ cr = np.cos(roll / 2.)
+ sp = np.sin(pitch / 2.)
+ cp = np.cos(pitch / 2.)
+ sy = np.sin(yaw / 2.)
+ cy = np.cos(yaw / 2.)
+ qx = sr * cp * cy - cr * sp * sy
+ qy = cr * sp * cy + sr * cp * sy
+ qz = cr * cp * sy - sr * sp * cy
+ qw = cr * cp * cy + sr * sp * sy
+ return [qx, qy, qz, qw]
+
+
+def inertiaTranslation(inertia, vect, mass):
+ ''' Translation of the inertia matrix using Parallel Axis theorem
+ Translation from frame expressed in G to frame expressed in O.
+ Args :
+ - inertia (Array 3x3): initial inertia matrix expressed in G
+ - vect (Array 3x) : translation vector to apply (OG) (!warning sign)
+ - mass (float) : mass
+ '''
+ inertia_o = inertia.copy()
+
+ # Diagonal coeff :
+ inertia_o[0, 0] += mass * (vect[1]**2 + vect[2]**2) # Ixx_o = Ixx_g + m*(yg**2 + zg**2)
+ inertia_o[1, 1] += mass * (vect[0]**2 + vect[2]**2) # Iyy_o = Iyy_g + m*(xg**2 + zg**2)
+ inertia_o[2, 2] += mass * (vect[0]**2 + vect[1]**2) # Izz_o = Iyy_g + m*(xg**2 + zg**2)
+
+ inertia_o[0, 1] += mass * vect[0] * vect[1] # Ixy_o = Ixy_g + m*xg*yg
+ inertia_o[0, 2] += mass * vect[0] * vect[2] # Ixz_o = Ixz_g + m*xg*zg
+ inertia_o[1, 2] += mass * vect[1] * vect[2] # Iyz_o = Iyz_g + m*yg*zg
+
+ inertia_o[1, 0] = inertia_o[0, 1] # Ixy_o = Iyx_o
+ inertia_o[2, 0] = inertia_o[0, 2] # Ixz_o = Izx_o
+ inertia_o[2, 1] = inertia_o[1, 2] # Iyz_o = Izy_o
+
+ return inertia_o
+
+
+def quaternionToRPY(quat):
+ """Quaternion (4 x 0) to Roll Pitch Yaw (3 x 1)"""
+
+ qx = quat[0]
+ qy = quat[1]
+ qz = quat[2]
+ qw = quat[3]
+
+ rotateXa0 = 2.0 * (qy * qz + qw * qx)
+ rotateXa1 = qw * qw - qx * qx - qy * qy + qz * qz
+ rotateX = 0.0
+
+ if (rotateXa0 != 0.0) and (rotateXa1 != 0.0):
+ rotateX = np.arctan2(rotateXa0, rotateXa1)
+
+ rotateYa0 = -2.0 * (qx * qz - qw * qy)
+ rotateY = 0.0
+ if (rotateYa0 >= 1.0):
+ rotateY = np.pi / 2.0
+ elif (rotateYa0 <= -1.0):
+ rotateY = -np.pi / 2.0
+ else:
+ rotateY = np.arcsin(rotateYa0)
+
+ rotateZa0 = 2.0 * (qx * qy + qw * qz)
+ rotateZa1 = qw * qw + qx * qx - qy * qy - qz * qz
+ rotateZ = 0.0
+ if (rotateZa0 != 0.0) and (rotateZa1 != 0.0):
+ rotateZ = np.arctan2(rotateZa0, rotateZa1)
+
+ return np.array([[rotateX], [rotateY], [rotateZ]])
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 84300131..b08efd3c 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -45,6 +45,7 @@ Estimator::Estimator()
alpha_secu_(0.0),
offset_yaw_IMU_(0.0),
perfect_estimator(false),
+ solo3D(false),
N_SIMULATION(0),
k_log_(0),
IMU_lin_acc_(Vector3::Zero()),
@@ -86,6 +87,7 @@ void Estimator::initialize(Params& params) {
dt_wbc = params.dt_wbc;
N_SIMULATION = params.N_SIMULATION;
perfect_estimator = params.perfect_estimator;
+ solo3D = params.solo3D;
// Filtering estimated linear velocity
int k_mpc = static_cast<int>(std::round(params.dt_mpc / params.dt_wbc));
@@ -136,7 +138,7 @@ void Estimator::initialize(Params& params) {
}
void Estimator::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
- Vector3 const& baseOrientation) {
+ Vector3 const& baseOrientation, VectorN const& dummyPos) {
// Linear acceleration of the trunk (base frame)
IMU_lin_acc_ = baseLinearAcceleration;
@@ -151,6 +153,10 @@ void Estimator::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 cons
}
IMU_RPY_(2, 0) -= offset_yaw_IMU_; // Remove initial offset of IMU
+ if (solo3D) {
+ IMU_RPY_.tail(1) = dummyPos.tail(1); // Yaw angle from motion capture
+ }
+
IMU_ang_pos_ =
pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(IMU_RPY_(0, 0), IMU_RPY_(1, 0), IMU_RPY_(2, 0)));
// Above could be commented since IMU_ang_pos yaw is not used anywhere and instead
@@ -268,7 +274,7 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
}
// Update IMU data
- get_data_IMU(baseLinearAcceleration, baseAngularVelocity, baseOrientation);
+ get_data_IMU(baseLinearAcceleration, baseAngularVelocity, baseOrientation, dummyPos);
// Angular position of the trunk
Vector4 filt_ang_pos = IMU_ang_pos_.coeffs();
@@ -307,6 +313,11 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
// Use cascade of complementary filters
+ // Base velocity estimated by FK, estimated by motion capture
+ if (solo3D) {
+ FK_lin_vel_ = b_baseVel;
+ }
+
// Rotation matrix to go from base frame to world frame
Matrix3 oRb = IMU_ang_pos_.toRotationMatrix();
@@ -330,12 +341,19 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
Vector3 ob_filt_lin_vel = oRb * b_filt_lin_vel_;
// Position of the center of the base from FGeometry and filtered velocity (world frame)
- Vector3 filt_lin_pos =
- filter_xyz_pos_.compute(FK_xyz_ + xyz_mean_feet_, ob_filt_lin_vel, Vector3(0.995, 0.995, 0.9));
+ Vector3 filt_lin_pos = Vector3::Zero();
+
+ if (solo3D) {
+ Vector3 offset_ = Vector3::Zero();
+ offset_.tail(3) << -0.0155;
+ filt_lin_pos = filter_xyz_pos_.compute(dummyPos.head(3) - offset_, ob_filt_lin_vel, Vector3(0.995, 0.995, 0.9));
+ } else {
+ filt_lin_pos = filter_xyz_pos_.compute(FK_xyz_ + xyz_mean_feet_, ob_filt_lin_vel, Vector3(0.995, 0.995, 0.9));
+ }
// Output filtered position vector (19 x 1)
q_filt_.head(3) = filt_lin_pos;
- if (perfect_estimator) { // Base height directly from PyBullet
+ if (perfect_estimator || solo3D) {
q_filt_(2, 0) = dummyPos(2, 0) - 0.0155; // Minus feet radius
}
q_filt_.block(3, 0, 4, 1) = filt_ang_pos;
diff --git a/src/FootTrajectoryGeneratorBezier.cpp b/src/FootTrajectoryGeneratorBezier.cpp
new file mode 100644
index 00000000..05c155a4
--- /dev/null
+++ b/src/FootTrajectoryGeneratorBezier.cpp
@@ -0,0 +1,648 @@
+#include "qrw/FootTrajectoryGeneratorBezier.hpp"
+#include <chrono>
+
+using namespace std::chrono;
+
+
+// Trajectory generator functions (output reference pos, vel and acc of feet in swing phase)
+
+FootTrajectoryGeneratorBezier::FootTrajectoryGeneratorBezier()
+ : gait_(NULL),
+ dt_wbc(0.0),
+ k_mpc(0),
+ maxHeight_(0.0),
+ lockTime_(0.0),
+ feet(),
+ t0s(Vector4::Zero()),
+ t0_bezier(Vector4::Zero()),
+ t_stop(Vector4::Zero()),
+ t_swing(Vector4::Zero()),
+ targetFootstep_(Matrix34::Zero()),
+ Ax(Matrix64::Zero()),
+ Ay(Matrix64::Zero()),
+ Az(Matrix74::Zero()),
+ position_(Matrix34::Zero()),
+ velocity_(Matrix34::Zero()),
+ acceleration_(Matrix34::Zero()),
+ jerk_(Matrix34::Zero()),
+ position_base_(Matrix34::Zero()),
+ velocity_base_(Matrix34::Zero()),
+ acceleration_base_(Matrix34::Zero()),
+ intersectionPoint_(Vector2::Zero()),
+ ineq_vector_{Vector4::Zero()},
+ x_margin_{Vector4::Zero()} {
+ // Initialise vector
+ for (int i = 0; i < 4; i++) {
+ pDefs.push_back(optimization::problem_definition<pointX_t, double>(3));
+ pDefs[i].degree = 7;
+ pDefs[i].flag = optimization::INIT_POS | optimization::END_POS | optimization::INIT_VEL | optimization::END_VEL |
+ optimization::INIT_ACC | optimization::END_ACC;
+ Vector6 vector = Vector6::Zero();
+ problem_data_t pbData = optimization::setup_control_points<pointX_t, double, safe>(pDefs[0]);
+ bezier_linear_variable_t* bez = pbData.bezier;
+
+ bezier_t bezi = evaluateLinear<bezier_t, bezier_linear_variable_t>(*bez, vector);
+ fitBeziers.push_back(bezi);
+
+ ineq_.push_back(Vector3::Zero());
+ }
+}
+
+void FootTrajectoryGeneratorBezier::initialize(Params& params, Gait& gaitIn, Surface initialSurface_in,
+ double x_margin_max_in, double t_margin_in, double z_margin_in,
+ int N_samples_in, int N_samples_ineq_in, int degree_in) {
+ N_samples = N_samples_in;
+ N_samples_ineq = N_samples_ineq_in;
+ degree = degree_in;
+ res_size = dim * (degree + 1 - 6);
+
+ P_ = MatrixN::Zero(res_size, res_size);
+ q_ = VectorN::Zero(res_size);
+ C_ = MatrixN::Zero(res_size, 0);
+ d_ = VectorN::Zero(0);
+ x = VectorN::Zero(res_size);
+ G_ = MatrixN::Zero(N_samples_ineq, res_size);
+ h_ = VectorN::Zero(N_samples_ineq);
+ dt_wbc = params.dt_wbc;
+ k_mpc = (int)std::round(params.dt_mpc / params.dt_wbc);
+ maxHeight_ = params.max_height;
+ lockTime_ = params.lock_time;
+ targetFootstep_ <<
+ Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_init.data(), params.footsteps_init.size());
+ position_ <<
+ Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_init.data(), params.footsteps_init.size());
+ gait_ = &gaitIn;
+ for (int foot = 0; foot < 4; foot++) {
+ newSurface_.push_back(initialSurface_in);
+ pastSurface_.push_back(initialSurface_in);
+ }
+ x_margin_max_ = x_margin_max_in;
+ t_margin_ = t_margin_in; // 1 % of the curve after critical point
+ z_margin_ = z_margin_in;
+}
+
+void FootTrajectoryGeneratorBezier::updatePolyCoeff_XY(int const& i_foot, Vector3 const& x_init, Vector3 const& v_init,
+ Vector3 const& a_init, Vector3 const& x_target,
+ double const& t0, double const& t1) {
+ double x0 = x_init(0);
+ double y0 = x_init(1);
+ double dx0 = v_init(0);
+ double dy0 = v_init(1);
+ double ddx0 = a_init(0);
+ double ddy0 = a_init(1);
+ double x1 = x_target(0);
+ double y1 = x_target(1);
+
+ double d = t1;
+ double t = t0;
+
+ // Compute polynoms coefficients for x and y
+ Ax(5, i_foot) =
+ (ddx0 * std::pow(t, 2) - 2 * ddx0 * t * d - 6 * dx0 * t + ddx0 * std::pow(d, 2) + 6 * dx0 * d + 12 * x0 -
+ 12 * x1) /
+ (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+
+ Ax(4, i_foot) =
+ (30 * t * x1 - 30 * t * x0 - 30 * d * x0 + 30 * d * x1 - 2 * std::pow(t, 3) * ddx0 - 3 * std::pow(d, 3) * ddx0 +
+ 14 * std::pow(t, 2) * dx0 - 16 * std::pow(d, 2) * dx0 + 2 * t * d * dx0 + 4 * t * std::pow(d, 2) * ddx0 +
+ std::pow(t, 2) * d * ddx0) /
+ (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ax(3, i_foot) =
+ (std::pow(t, 4) * ddx0 + 3 * std::pow(d, 4) * ddx0 - 8 * std::pow(t, 3) * dx0 + 12 * std::pow(d, 3) * dx0 +
+ 20 * std::pow(t, 2) * x0 - 20 * std::pow(t, 2) * x1 + 20 * std::pow(d, 2) * x0 - 20 * std::pow(d, 2) * x1 +
+ 80 * t * d * x0 - 80 * t * d * x1 + 4 * std::pow(t, 3) * d * ddx0 + 28 * t * std::pow(d, 2) * dx0 -
+ 32 * std::pow(t, 2) * d * dx0 - 8 * std::pow(t, 2) * std::pow(d, 2) * ddx0) /
+ (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ax(2, i_foot) = -(std::pow(d, 5) * ddx0 + 4 * t * std::pow(d, 4) * ddx0 + 3 * std::pow(t, 4) * d * ddx0 +
+ 36 * t * std::pow(d, 3) * dx0 - 24 * std::pow(t, 3) * d * dx0 + 60 * t * std::pow(d, 2) * x0 +
+ 60 * std::pow(t, 2) * d * x0 - 60 * t * std::pow(d, 2) * x1 - 60 * std::pow(t, 2) * d * x1 -
+ 8 * std::pow(t, 2) * std::pow(d, 3) * ddx0 - 12 * std::pow(t, 2) * std::pow(d, 2) * dx0) /
+ (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ax(1, i_foot) =
+ -(2 * std::pow(d, 5) * dx0 - 2 * t * std::pow(d, 5) * ddx0 - 10 * t * std::pow(d, 4) * dx0 +
+ std::pow(t, 2) * std::pow(d, 4) * ddx0 + 4 * std::pow(t, 3) * std::pow(d, 3) * ddx0 -
+ 3 * std::pow(t, 4) * std::pow(d, 2) * ddx0 - 16 * std::pow(t, 2) * std::pow(d, 3) * dx0 +
+ 24 * std::pow(t, 3) * std::pow(d, 2) * dx0 - 60 * std::pow(t, 2) * std::pow(d, 2) * x0 +
+ 60 * std::pow(t, 2) * std::pow(d, 2) * x1) /
+ (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ax(0, i_foot) = (2 * x1 * std::pow(t, 5) - ddx0 * std::pow(t, 4) * std::pow(d, 3) - 10 * x1 * std::pow(t, 4) * d +
+ 2 * ddx0 * std::pow(t, 3) * std::pow(d, 4) + 8 * dx0 * std::pow(t, 3) * std::pow(d, 3) +
+ 20 * x1 * std::pow(t, 3) * std::pow(d, 2) - ddx0 * std::pow(t, 2) * std::pow(d, 5) -
+ 10 * dx0 * std::pow(t, 2) * std::pow(d, 4) - 20 * x0 * std::pow(t, 2) * std::pow(d, 3) +
+ 2 * dx0 * t * std::pow(d, 5) + 10 * x0 * t * std::pow(d, 4) - 2 * x0 * std::pow(d, 5)) /
+ (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+
+ Ay(5, i_foot) =
+ (ddy0 * std::pow(t, 2) - 2 * ddy0 * t * d - 6 * dy0 * t + ddy0 * std::pow(d, 2) + 6 * dy0 * d + 12 * y0 -
+ 12 * y1) /
+ (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(4, i_foot) =
+ (30 * t * y1 - 30 * t * y0 - 30 * d * y0 + 30 * d * y1 - 2 * std::pow(t, 3) * ddy0 - 3 * std::pow(d, 3) * ddy0 +
+ 14 * std::pow(t, 2) * dy0 - 16 * std::pow(d, 2) * dy0 + 2 * t * d * dy0 + 4 * t * std::pow(d, 2) * ddy0 +
+ std::pow(t, 2) * d * ddy0) /
+ (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(3, i_foot) =
+ (std::pow(t, 4) * ddy0 + 3 * std::pow(d, 4) * ddy0 - 8 * std::pow(t, 3) * dy0 + 12 * std::pow(d, 3) * dy0 +
+ 20 * std::pow(t, 2) * y0 - 20 * std::pow(t, 2) * y1 + 20 * std::pow(d, 2) * y0 - 20 * std::pow(d, 2) * y1 +
+ 80 * t * d * y0 - 80 * t * d * y1 + 4 * std::pow(t, 3) * d * ddy0 + 28 * t * std::pow(d, 2) * dy0 -
+ 32 * std::pow(t, 2) * d * dy0 - 8 * std::pow(t, 2) * std::pow(d, 2) * ddy0) /
+ (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(2, i_foot) = -(std::pow(d, 5) * ddy0 + 4 * t * std::pow(d, 4) * ddy0 + 3 * std::pow(t, 4) * d * ddy0 +
+ 36 * t * std::pow(d, 3) * dy0 - 24 * std::pow(t, 3) * d * dy0 + 60 * t * std::pow(d, 2) * y0 +
+ 60 * std::pow(t, 2) * d * y0 - 60 * t * std::pow(d, 2) * y1 - 60 * std::pow(t, 2) * d * y1 -
+ 8 * std::pow(t, 2) * std::pow(d, 3) * ddy0 - 12 * std::pow(t, 2) * std::pow(d, 2) * dy0) /
+ (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(1, i_foot) =
+ -(2 * std::pow(d, 5) * dy0 - 2 * t * std::pow(d, 5) * ddy0 - 10 * t * std::pow(d, 4) * dy0 +
+ std::pow(t, 2) * std::pow(d, 4) * ddy0 + 4 * std::pow(t, 3) * std::pow(d, 3) * ddy0 -
+ 3 * std::pow(t, 4) * std::pow(d, 2) * ddy0 - 16 * std::pow(t, 2) * std::pow(d, 3) * dy0 +
+ 24 * std::pow(t, 3) * std::pow(d, 2) * dy0 - 60 * std::pow(t, 2) * std::pow(d, 2) * y0 +
+ 60 * std::pow(t, 2) * std::pow(d, 2) * y1) /
+ (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(0, i_foot) = (2 * y1 * std::pow(t, 5) - ddy0 * std::pow(t, 4) * std::pow(d, 3) - 10 * y1 * std::pow(t, 4) * d +
+ 2 * ddy0 * std::pow(t, 3) * std::pow(d, 4) + 8 * dy0 * std::pow(t, 3) * std::pow(d, 3) +
+ 20 * y1 * std::pow(t, 3) * std::pow(d, 2) - ddy0 * std::pow(t, 2) * std::pow(d, 5) -
+ 10 * dy0 * std::pow(t, 2) * std::pow(d, 4) - 20 * y0 * std::pow(t, 2) * std::pow(d, 3) +
+ 2 * dy0 * t * std::pow(d, 5) + 10 * y0 * t * std::pow(d, 4) - 2 * y0 * std::pow(d, 5)) /
+ (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+}
+
+void FootTrajectoryGeneratorBezier::updatePolyCoeff_Z(int const& i_foot, Vector3 const& x_init,
+ Vector3 const& x_target, double const& t1, double const& h) {
+ double z0 = x_init(2);
+ double z1 = x_target(2);
+
+ // coefficients for z (deterministic)
+ // Version 2D (z1 = 0)
+ // Az[6,i_foot] = -h/((t1/2)**3*(t1 - t1/2)**3)
+ // Az[5,i_foot] = (3*t1*h)/((t1/2)**3*(t1 - t1/2)**3)
+ // Az[4,i_foot] = -(3*t1**2*h)/((t1/2)**3*(t1 - t1/2)**3)
+ // Az[3,i_foot] = (t1**3*h)/((t1/2)**3*(t1 - t1/2)**3)
+ // Az[:3,i_foot] = 0
+
+ // Version 3D (z1 != 0)
+ Az(6, i_foot) = (32. * z0 + 32. * z1 - 64. * h) / std::pow(t1, 6);
+ Az(5, i_foot) = -(102. * z0 + 90. * z1 - 192. * h) / std::pow(t1, 5);
+ Az(4, i_foot) = (111. * z0 + 81. * z1 - 192. * h) / std::pow(t1, 4);
+ Az(3, i_foot) = -(42. * z0 + 22. * z1 - 64. * h) / std::pow(t1, 3);
+ Az(2, i_foot) = 0;
+ Az(1, i_foot) = 0;
+ Az(0, i_foot) = z0;
+}
+
+Vector3 FootTrajectoryGeneratorBezier::evaluateBezier(int const& i_foot, int const& indice, double const& t) {
+ double t1 = t_swing(i_foot);
+ double delta_t = t1 - t0_bezier(i_foot);
+ double t_b = std::min((t - t0_bezier(i_foot)) / (t1 - t0_bezier(i_foot)), 1.);
+
+ if (indice == 0) {
+ return fitBeziers[i_foot](t_b);
+ } else if (indice == 1) {
+ return fitBeziers[i_foot].derivate(t_b, 1) / delta_t;
+ } else if (indice == 2) {
+ return fitBeziers[i_foot].derivate(t_b, 2) / std::pow(delta_t, 2);
+ } else {
+ Vector3 vector = Vector3::Zero();
+ return vector;
+ }
+}
+
+Vector3 FootTrajectoryGeneratorBezier::evaluatePoly(int const& i_foot, int const& indice, double const& t) {
+ Vector3 vector = Vector3::Zero();
+ if (indice == 0) {
+ double x = Ax(0, i_foot) + Ax(1, i_foot) * t + Ax(2, i_foot) * std::pow(t, 2) + Ax(3, i_foot) * std::pow(t, 3) +
+ Ax(4, i_foot) * std::pow(t, 4) + Ax(5, i_foot) * std::pow(t, 5);
+ double y = Ay(0, i_foot) + Ay(1, i_foot) * t + Ay(2, i_foot) * std::pow(t, 2) + Ay(3, i_foot) * std::pow(t, 3) +
+ Ay(4, i_foot) * std::pow(t, 4) + Ay(5, i_foot) * std::pow(t, 5);
+ double z = Az(0, i_foot) + Az(1, i_foot) * t + Az(2, i_foot) * std::pow(t, 2) + Az(3, i_foot) * std::pow(t, 3) +
+ Az(4, i_foot) * std::pow(t, 4) + Az(5, i_foot) * std::pow(t, 5) + Az(6, i_foot) * std::pow(t, 6);
+ vector << x, y, z;
+ }
+
+ if (indice == 1) {
+ double vx = Ax(1, i_foot) + 2 * Ax(2, i_foot) * t + 3 * Ax(3, i_foot) * std::pow(t, 2) +
+ 4 * Ax(4, i_foot) * std::pow(t, 3) + 5 * Ax(5, i_foot) * std::pow(t, 4);
+ double vy = Ay(1, i_foot) + 2 * Ay(2, i_foot) * t + 3 * Ay(3, i_foot) * std::pow(t, 2) +
+ 4 * Ay(4, i_foot) * std::pow(t, 3) + 5 * Ay(5, i_foot) * std::pow(t, 4);
+ double vz = Az(1, i_foot) + 2 * Az(2, i_foot) * t + 3 * Az(3, i_foot) * std::pow(t, 2) +
+ 4 * Az(4, i_foot) * std::pow(t, 3) + 5 * Az(5, i_foot) * std::pow(t, 4) +
+ 6 * Az(6, i_foot) * std::pow(t, 5);
+ vector << vx, vy, vz;
+ }
+
+ if (indice == 2) {
+ double ax = 2 * Ax(2, i_foot) + 6 * Ax(3, i_foot) * t + 12 * Ax(4, i_foot) * std::pow(t, 2) +
+ 20 * Ax(5, i_foot) * std::pow(t, 3);
+ double ay = 2 * Ay(2, i_foot) + 6 * Ay(3, i_foot) * t + 12 * Ay(4, i_foot) * std::pow(t, 2) +
+ 20 * Ay(5, i_foot) * std::pow(t, 3);
+ double az = 2 * Az(2, i_foot) + 6 * Az(3, i_foot) * t + 12 * Az(4, i_foot) * std::pow(t, 2) +
+ 20 * Az(5, i_foot) * std::pow(t, 3) + 30 * Az(6, i_foot) * std::pow(t, 4);
+ vector << ax, ay, az;
+ }
+
+ return vector;
+}
+
+void FootTrajectoryGeneratorBezier::updateFootPosition(int const& k, int const& i_foot, Vector3 const& targetFootstep) {
+ double t0 = t0s[i_foot];
+ double t1 = t_swing[i_foot];
+ double h = maxHeight_;
+ double delta_t = t1 - t0;
+ double dt = dt_wbc;
+
+ if (t0 < t1 - lockTime_) {
+ // compute reference polynoms coefficients
+ if (t0s[i_foot] < 10e-4 || k == 0) {
+ // Update Z coefficients only at the beginning of the flying phase
+ updatePolyCoeff_Z(i_foot, position_.col(i_foot), targetFootstep, t1, h + targetFootstep[2]);
+ // Initale velocity and acceleration nulle
+ updatePolyCoeff_XY(i_foot, position_.col(i_foot), Vector3::Zero(), Vector3::Zero(), targetFootstep, t0, t1);
+
+ // Update initial conditions of the Problem Definition
+ Vector3 vector;
+ vector << position_(0, i_foot), position_(1, i_foot), evaluatePoly(i_foot, 0, t0)(2);
+ pDefs[i_foot].init_pos = vector;
+
+ vector << 0., 0., delta_t * evaluatePoly(i_foot, 1, t0)(2);
+ pDefs[i_foot].init_vel = vector;
+
+ vector << 0., 0., std::pow(delta_t, 2) * evaluatePoly(i_foot, 2, t0)(2);
+ pDefs[i_foot].init_acc = vector;
+
+ // New swing phase --> ineq surface
+ t_stop[i_foot] = 0.;
+
+
+ if ((newSurface_[i_foot].getHeight(targetFootstep.head(2)) -
+ pastSurface_[i_foot].getHeight(targetFootstep.head(2))) >= 10e-3)
+ // Only uphill
+ {
+ std::cout << "\n\n\n\n\n--------------------" << std::endl;
+ std::cout << "DIFF SURFACES" << std::endl;
+ std::cout << "newSurface_[i_foot].getb" << std::endl;
+ std::cout << newSurface_[i_foot].getb() << std::endl;
+ std::cout << "pastSurface_[i_foot].getb" << std::endl;
+ std::cout << pastSurface_[i_foot].getb() << std::endl;
+
+ int nb_vert = newSurface_[i_foot].vertices_.rows();
+ MatrixN vert = newSurface_[i_foot].vertices_;
+
+ Vector2 P1 = position_.col(i_foot).head(2);
+ Vector2 P2 = targetFootstep.head(2);
+
+ Vector2 Q1;
+ Vector2 Q2;
+ for (int l = 0; l < nb_vert; l++) {
+ Q1 << vert(l, 0), vert(l, 1);
+ if (l < nb_vert - 1) {
+ Q2 << vert(l + 1, 0), vert(l + 1, 1);
+ } else {
+ Q2 << vert(0, 0), vert(0, 1);
+ }
+ if (doIntersect_segment(P1, P2, Q1, Q2)) {
+ get_intersect_segment(P1, P2, Q1, Q2);
+ // Should be sorted
+ // self.ineq[i_foot] = surface.ineq[k, :]
+ // self.ineq_vect[i_foot] = surface.ineq_vect[k]
+ double a = 0.;
+ if ((Q1[0] - Q2[0]) != 0.) {
+ a = (Q2[1] - Q1[1]) / (Q2[0] - Q1[0]);
+ double b = Q1[1] - a * Q1[0];
+ ineq_[i_foot] << -a, 1., 0.; // -ax + y = b
+ ineq_vector_[i_foot] = b;
+ } else {
+ // Inequality of the surface corresponding to these vertices
+ ineq_[i_foot] << -1., 0., 0.;
+ ineq_vector_[i_foot] = -Q1[0];
+ }
+ if (ineq_[i_foot].transpose() * position_.col(i_foot) > ineq_vector_[i_foot]) {
+ // Wrong side, the targeted point is inside the surface
+ ineq_[i_foot] = -ineq_[i_foot];
+ ineq_vector_[i_foot] = -ineq_vector_[i_foot];
+ }
+
+ // If foot position already closer than margin
+ x_margin_[i_foot] = std::max(std::min(x_margin_max_, std::abs(intersectionPoint_[0] - P1[0]) - 0.001), 0.);
+ }
+ }
+ } else {
+ ineq_[i_foot].setZero();
+ ineq_vector_[i_foot] = 0.;
+ }
+ } else {
+ updatePolyCoeff_XY(i_foot, position_.col(i_foot), velocity_.col(i_foot), acceleration_.col(i_foot),
+ targetFootstep, t0, t1);
+ // Update initial conditions of the Problem Definition
+ pDefs[i_foot].init_pos = position_.col(i_foot);
+ pDefs[i_foot].init_vel = delta_t * velocity_.col(i_foot);
+ pDefs[i_foot].init_acc = std::pow(delta_t, 2) * acceleration_.col(i_foot);
+ }
+ // REset inequalities to zero
+ G_.setZero();
+ for (int l = 0; l < h_.size(); l++) {
+ h_(l) = 0.;
+ }
+ // Update final conditions of the Problem Definition
+ pDefs[i_foot].end_pos = targetFootstep;
+ pDefs[i_foot].end_vel = Vector3::Zero();
+ pDefs[i_foot].end_acc = Vector3::Zero();
+
+ pDefs[i_foot].flag = optimization::INIT_POS | optimization::END_POS | optimization::INIT_VEL |
+ optimization::END_VEL | optimization::INIT_ACC | optimization::END_ACC;
+
+ // generates the linear variable of the bezier curve with the parameters of problemDefinition
+ problem_data_t pbData = optimization::setup_control_points<pointX_t, double, safe>(pDefs[i_foot]);
+ bezier_linear_variable_t* linear_bezier = pbData.bezier;
+
+ // Prepare the inequality matrix :
+ Vector3 x_t = evaluatePoly(i_foot, 0, t0);
+ double t_margin = t_margin_ * t1; // 10% around the limit point !inferior to 1/nb point in linspace
+
+ // No surface switch or already overpass the critical point
+ if (!(ineq_[i_foot].isZero()) and ((x_t[2] < targetFootstep(2)) or (t0s[i_foot] < t_stop[i_foot] + t_margin)) and
+ x_margin_[i_foot] != 0.) {
+ int nb_vert = newSurface_[i_foot].vertices_.rows();
+ MatrixN vert = newSurface_[i_foot].vertices_;
+
+ Vector2 P1 = position_.col(i_foot).head(2);
+ Vector2 P2 = targetFootstep.head(2);
+
+ Vector2 Q1;
+ Vector2 Q2;
+ for (int l = 0; l < nb_vert; l++) {
+ Q1 << vert(l, 0), vert(l, 1);
+ if (l < nb_vert - 1) {
+ Q2 << vert(l + 1, 0), vert(l + 1, 1);
+ } else {
+ Q2 << vert(0, 0), vert(0, 1);
+ }
+ if (doIntersect_segment(P1, P2, Q1, Q2)) {
+ get_intersect_segment(P1, P2, Q1, Q2);
+ // Should be sorted
+ // self.ineq[i_foot] = surface.ineq[k, :]
+ // self.ineq_vect[i_foot] = surface.ineq_vect[k]
+ double a = 0.;
+ if ((Q1[0] - Q2[0]) != 0.) {
+ a = (Q2[1] - Q1[1]) / (Q2[0] - Q1[0]);
+ double b = Q1[1] - a * Q1[0];
+ ineq_[i_foot] << -a, 1., 0.; // -ax + y = b
+ ineq_vector_[i_foot] = b;
+ } else {
+ // Inequality of the surface corresponding to these vertices
+ ineq_[i_foot] << -1., 0., 0.;
+ ineq_vector_[i_foot] = -Q1[0];
+ }
+ if (ineq_[i_foot].transpose() * position_.col(i_foot) > ineq_vector_[i_foot]) {
+ // Wrong side, the targeted point is inside the surface
+ ineq_[i_foot] = -ineq_[i_foot];
+ ineq_vector_[i_foot] = -ineq_vector_[i_foot];
+ }
+ // If foot position already closer than margin
+ x_margin_[i_foot] = std::max(std::min(x_margin_max_, std::abs(intersectionPoint_[0] - P1[0]) - 0.001), 0.);
+ }
+ }
+
+ double z_margin = targetFootstep(2) * z_margin_; // 10% around the limit height
+ double t_s;
+ double zt;
+
+ linear_variable_t linear_var_;
+
+ for (int its = 0; its < N_samples_ineq; its++) {
+ t_s = (its + 1.0) / N_samples_ineq;
+ zt = evaluatePoly(i_foot, 0, t0 + (t1 - t0) * t_s)[2];
+ if (t0 + (t1 - t0) * t_s < t_stop[i_foot] + t_margin) {
+ if (zt < targetFootstep(2) + z_margin) {
+ t_stop[i_foot] = t0 + (t1 - t0) * t_s;
+ }
+ linear_var_ = linear_bezier->operator()(t_s);
+ G_.row(its) = -ineq_[i_foot].transpose() * linear_var_.B();
+ h_(its) = -ineq_[i_foot].transpose() * linear_var_.c() + ineq_vector_[i_foot] - (x_margin_[i_foot]);
+ } else {
+ G_.row(its).setZero();
+ h_(its) = 0.;
+ }
+ }
+
+ } else {
+ G_.setZero();
+ for (int l = 0; l < h_.size(); l++) {
+ h_(l) = 0.;
+ }
+ }
+
+ P_.setZero();
+ q_.setZero();
+ linear_variable_t linear_var;
+ double t_b_;
+ for (int j = 0; j < N_samples; j++) {
+ t_b_ = (j + 1.0) / N_samples;
+
+ linear_var = linear_bezier->operator()(t_b_);
+
+ P_ += linear_var.B().transpose() * linear_var.B();
+ q_ += linear_var.B().transpose() * (linear_var.c() - evaluatePoly(i_foot, 0, t0 + (t1 - t0) * t_b_));
+ }
+
+ // Eiquadprog-Fast solves the problem :
+ // min. 1/2 * x' C_ x + q_' x
+ // s.t. C_ x + d_ = 0
+ // G_ x + h_ >= 0
+ status = qp.solve_quadprog(P_, q_, C_, d_, G_, h_, x);
+
+ // Evaluate Bezier Linear with optimsed Points
+ fitBeziers[i_foot] = evaluateLinear<bezier_t, bezier_linear_variable_t>(*linear_bezier, x);
+
+ t0_bezier[i_foot] = t0;
+ }
+
+ // Get the next point
+ double ev = t0 + dt;
+
+ double t_b = std::min((ev - t0_bezier[i_foot]) / (t1 - t0_bezier[i_foot]), 1.);
+ delta_t = t1 - t0_bezier[i_foot];
+
+ if (t0 < 0.0 || t0 > t1) // Just vertical motion
+ {
+ position_(0, i_foot) = targetFootstep(0);
+ position_(1, i_foot) = targetFootstep(1);
+ position_(2, i_foot) = evaluatePoly(i_foot, 0, ev)[2];
+ velocity_(0, i_foot) = 0.0;
+ velocity_(1, i_foot) = 0.0;
+ velocity_(2, i_foot) = evaluatePoly(i_foot, 1, ev)[2];
+ acceleration_(0, i_foot) = 0.0;
+ acceleration_(1, i_foot) = 0.0;
+ acceleration_(2, i_foot) = evaluatePoly(i_foot, 2, ev)[2];
+ } else {
+ position_.col(i_foot) = fitBeziers[i_foot](t_b);
+ velocity_.col(i_foot) = fitBeziers[i_foot].derivate(t_b, 1) / delta_t;
+ acceleration_.col(i_foot) = fitBeziers[i_foot].derivate(t_b, 2) / std::pow(delta_t, 2);
+ // position_.col(i_foot) = evaluatePoly(i_foot, 0, ev);
+ // velocity_.col(i_foot) = evaluatePoly(i_foot, 1, ev);
+ // acceleration_.col(i_foot) = evaluatePoly(i_foot, 2, ev);
+ }
+}
+
+void FootTrajectoryGeneratorBezier::update(int k, MatrixN const& targetFootstep, SurfaceVector const& surfacesSelected,
+ MatrixN const& currentPosition) {
+ if ((k % k_mpc) == 0) {
+ // Indexes of feet in swing phase
+ feet.clear();
+ for (int i = 0; i < 4; i++) {
+ if (gait_->getCurrentGait()(0, i) == 0) feet.push_back(i);
+ }
+ // If no foot in swing phase
+ if (feet.size() == 0) return;
+
+ // For each foot in swing phase get remaining duration of the swing phase
+ for (int j = 0; j < (int)feet.size(); j++) {
+ int i = feet[j];
+ t_swing[i] = gait_->getPhaseDuration(0, feet[j], 0.0); // 0.0 for swing phase
+ double value = t_swing[i] - (gait_->getRemainingTime() * k_mpc - ((k + 1) % k_mpc)) * dt_wbc - dt_wbc;
+ t0s[i] = std::max(0.0, value);
+ }
+ } else {
+ // If no foot in swing phase
+ if (feet.size() == 0) return;
+
+ // Increment of one time step for feet in swing phase
+ for (int i = 0; i < (int)feet.size(); i++) {
+ double value = t0s[feet[i]] + dt_wbc;
+ t0s[feet[i]] = std::max(0.0, value);
+ }
+ }
+ // Update new surface and past if t0 == 0 (new swing phase)
+ if (((k % k_mpc) == 0) and (surfacesSelected.size() != 0)) {
+ for (int i_foot = 0; i_foot < (int)feet.size(); i_foot++) {
+ if (t0s[i_foot] <= 10e-5) {
+ pastSurface_[i_foot] = newSurface_[i_foot];
+ newSurface_[i_foot] = surfacesSelected[i_foot];
+ }
+ }
+ }
+
+ for (int i = 0; i < (int)feet.size(); i++) {
+ position_.col(feet[i]) = currentPosition.col(feet[i]);
+ updateFootPosition(k, feet[i], targetFootstep.col(feet[i]));
+ }
+ return;
+}
+
+bool FootTrajectoryGeneratorBezier::doIntersect_segment(Vector2 const& p1, Vector2 const& q1, Vector2 const& p2,
+ Vector2 const& q2) {
+ // Find the 4 orientations required for
+ // the general and special cases
+ int o1 = orientation(p1, q1, p2);
+ int o2 = orientation(p1, q1, q2);
+ int o3 = orientation(p2, q2, p1);
+ int o4 = orientation(p2, q2, q1);
+
+ // General case
+ if ((o1 != o2) and (o3 != o4)) {
+ return true;
+ }
+
+ // Special Cases
+ // p1 , q1 and p2 are colinear and p2 lies on segment p1q1
+ if ((o1 == 0) and onSegment(p1, p2, q1)) {
+ return true;
+ }
+
+ // p1 , q1 and q2 are colinear and q2 lies on segment p1q1
+ if ((o2 == 0) and onSegment(p1, q2, q1)) {
+ return true;
+ }
+
+ // p2 , q2 and p1 are colinear and p1 lies on segment p2q2
+ if ((o3 == 0) and onSegment(p2, p1, q2)) {
+ return true;
+ }
+
+ // p2 , q2 and q1 are colinear and q1 lies on segment p2q2
+ if ((o4 == 0) and onSegment(p2, q1, q2)) {
+ return true;
+ }
+
+ // If none of the cases
+ return false;
+}
+
+// Given three colinear points p, q, r, the function checks if
+// point q lies on line segment 'pr'
+bool FootTrajectoryGeneratorBezier::onSegment(Vector2 const& p, Vector2 const& q, Vector2 const& r) {
+ if ((q[0] <= std::max(p[0], r[0])) and (q[0] >= std::min(p[0], r[0])) and (q[1] <= std::max(p[1], r[1])) and
+ (q[1] >= std::min(p[1], r[1]))) {
+ return true;
+ }
+ return false;
+}
+
+// to find the orientation of an ordered triplet (p,q,r)
+// function returns the following values:
+// 0 : Colinear points
+// 1 : Clockwise points
+// 2 : Counterclockwise
+// See https://www.geeksforgeeks.org/orientation-3-ordered-points/amp/
+// for details of below formula.
+// Modified, remove class Point, directly handle p = [px,py]
+int FootTrajectoryGeneratorBezier::orientation(Vector2 const& p, Vector2 const& q, Vector2 const& r) {
+ double val = ((q[1] - p[1]) * (r[0] - q[0])) - ((q[0] - p[0]) * (r[1] - q[1]));
+ if (val > 0) {
+ // Clockwise orientation
+ return 1;
+ } else if (val < 0) {
+ // Counterclockwise orientation
+ return 2;
+ } else {
+ // Colinear orientation
+ return 0;
+ }
+}
+
+// Method to intersect 2 segment --> useful to retrieve which inequality is crossed in a surface (2D)
+// Returns the point of intersection of the lines passing through a2,a1 and b2,b1.
+// a1: [x, y] a point on the first line
+// a2: [x, y] another point on the first line
+// b1: [x, y] a point on the second line
+// b2: [x, y] another point on the second line
+
+void FootTrajectoryGeneratorBezier::get_intersect_segment(Vector2 a1, Vector2 a2, Vector2 b1, Vector2 b2) {
+ Vector3 cross_l1;
+ Vector3 cross_l2;
+ Vector3 cross_ll;
+
+ cross_l1 << a1[1] - a2[1], a1[0] - a2[0], a2[0] * a1[1] - a2[1] * a1[0];
+ cross_l2 << b1[1] - b2[1], b1[0] - b2[0], b2[0] * b1[1] - b2[1] * b1[0];
+
+ cross_ll << cross_l1[1] - cross_l2[1], cross_l1[0] - cross_l2[0],
+ cross_l2[0] * cross_l1[1] - cross_l2[1] * cross_l1[0];
+
+ intersectionPoint_(0) = cross_ll[0] / cross_ll[2];
+ intersectionPoint_(1) = cross_ll[1] / cross_ll[2];
+}
+
+Eigen::MatrixXd FootTrajectoryGeneratorBezier::getFootPositionBaseFrame(const Eigen::Matrix<double, 3, 3>& R,
+ const Eigen::Matrix<double, 3, 1>& T) {
+ position_base_ =
+ R * (position_ - T.replicate<1, 4>()); // Value saved because it is used to get velocity and acceleration
+ return position_base_;
+}
+
+Eigen::MatrixXd FootTrajectoryGeneratorBezier::getFootVelocityBaseFrame(const Eigen::Matrix<double, 3, 3>& R,
+ const Eigen::Matrix<double, 3, 1>& v_ref,
+ const Eigen::Matrix<double, 3, 1>& w_ref) {
+ velocity_base_ = R * velocity_ - v_ref.replicate<1, 4>() +
+ position_base_.colwise().cross(w_ref); // Value saved because it is used to get acceleration
+ return velocity_base_;
+}
+
+Eigen::MatrixXd FootTrajectoryGeneratorBezier::getFootAccelerationBaseFrame(const Eigen::Matrix<double, 3, 3>& R,
+ const Eigen::Matrix<double, 3, 1>& w_ref,
+ const Eigen::Matrix<double, 3, 1>& a_ref) {
+ return R * acceleration_ - (position_base_.colwise().cross(w_ref)).colwise().cross(w_ref) +
+ 2 * velocity_base_.colwise().cross(w_ref) - a_ref.replicate<1, 4>();
+}
diff --git a/src/FootstepPlannerQP.cpp b/src/FootstepPlannerQP.cpp
new file mode 100644
index 00000000..7a2360c5
--- /dev/null
+++ b/src/FootstepPlannerQP.cpp
@@ -0,0 +1,587 @@
+#include "qrw/FootstepPlannerQP.hpp"
+
+FootstepPlannerQP::FootstepPlannerQP()
+ : gait_(NULL),
+ g(9.81),
+ L(0.155),
+ next_footstep_(Vector3::Zero()),
+ next_footstep_qp_(Vector3::Zero()),
+ footsteps_(),
+ Rz(MatrixN::Identity(3, 3)),
+ Rz_tmp(MatrixN::Identity(3, 3)),
+ dt_cum(),
+ yaws(),
+ yaws_b(),
+ dx(),
+ dy(),
+ q_dxdy(Vector3::Zero()),
+ q_tmp(Vector3::Zero()),
+ RPY_(Vector3::Zero()),
+ pos_feet_(Matrix34::Zero()),
+ q_FK_(Vector19::Zero()),
+ k_mpc(0),
+ feet_(),
+ t0s(Vector4::Zero()),
+ t_swing(Vector4::Zero()),
+ weights_(VectorN::Zero(N)),
+ voptim_{Vector3::Zero()},
+ b_voptim_{Vector6::Zero()},
+ P_{MatrixN::Zero(N, N)},
+ q_{VectorN::Zero(N)},
+ G_{MatrixN::Zero(M, N)},
+ h_{VectorN::Zero(M)},
+ C_{MatrixN::Zero(N, 0)},
+ d_{VectorN::Zero(0)},
+ x{VectorN::Zero(N)},
+ surfaceStatus_(false),
+ surfaceIteration_(0){
+ // Empty
+}
+
+void FootstepPlannerQP::initialize(Params& params, Gait& gaitIn, Surface initialSurface_in) {
+ params_ = ¶ms;
+ dt = params.dt_mpc;
+ dt_wbc = params.dt_wbc;
+ h_ref = params.h_ref;
+ n_steps = static_cast<int>(params.gait.rows());
+ k_mpc = (int)std::round(params.dt_mpc / params.dt_wbc);
+ footsteps_under_shoulders_ << Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_under_shoulders.data(),
+ params.footsteps_under_shoulders.size());
+ // Offsets to make the support polygon smaller
+ double ox = 0.0;
+ double oy = 0.0;
+ footsteps_offset_ << -ox, -ox, ox, ox, -oy, +oy, +oy, -oy, 0.0, 0.0, 0.0, 0.0;
+ currentFootstep_ << Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_init.data(),
+ params.footsteps_init.size());
+ gait_ = &gaitIn;
+ targetFootstep_ = currentFootstep_;
+ o_targetFootstep_ = currentFootstep_;
+ dt_cum = VectorN::Zero(params.gait.rows());
+ yaws = VectorN::Zero(params.gait.rows());
+ yaws_b = VectorN::Zero(params.gait.rows());
+ dx = VectorN::Zero(params.gait.rows());
+ dy = VectorN::Zero(params.gait.rows());
+ for (int i = 0; i < params.gait.rows(); i++) {
+ footsteps_.push_back(Matrix34::Zero());
+ b_footsteps_.push_back(Matrix34::Zero());
+ }
+ Rz(2, 2) = 1.0;
+
+ // Surfaces initialization
+ initialSurface_ = initialSurface_in;
+ for (int foot = 0; foot < 4; foot++) {
+ selectedSurfaces_.push_back(initialSurface_in);
+ }
+
+ // QP initialization
+ qp.reset(N, 0, M);
+ weights_.setZero(N);
+ weights_ << 0.05, 0.05, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;;
+ P_.diagonal() << weights_;
+
+ // Path to the robot URDF (TODO: Automatic path)
+ const std::string filename =
+ std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
+
+ // Build model from urdf (base is not free flyer)
+ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
+
+ // Construct data from model
+ data_ = pinocchio::Data(model_);
+
+ // Update all the quantities of the model
+ VectorN q_tmp = VectorN::Zero(model_.nq);
+ q_tmp(6, 0) = 1.0; // Quaternion (0, 0, 0, 1)
+ pinocchio::computeAllTerms(model_, data_, q_tmp, VectorN::Zero(model_.nv));
+
+ // Get feet frame IDs
+ foot_ids_[0] = static_cast<int>(model_.getFrameId("FL_FOOT")); // from long uint to int
+ foot_ids_[1] = static_cast<int>(model_.getFrameId("FR_FOOT"));
+ foot_ids_[2] = static_cast<int>(model_.getFrameId("HL_FOOT"));
+ foot_ids_[3] = static_cast<int>(model_.getFrameId("HR_FOOT"));
+}
+
+MatrixN FootstepPlannerQP::updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v,
+ Vector6 const& b_vref, SurfaceVectorVector const& potentialSurfaces,
+ SurfaceVector const& surfaces, bool const surfaceStatus,
+ int const surfaceIteration) {
+ if (q.rows() != 18) {
+ throw std::runtime_error("q should be a vector of size 18 (pos+RPY+mot)");
+ }
+
+ // Update location of feet in stance phase (for those which just entered stance phase)
+ if (refresh && gait_->isNewPhase()) {
+ updateNewContact(q);
+ }
+
+ // Compute location of footsteps
+ return computeTargetFootstep(k, q.head(6), b_v, b_vref, potentialSurfaces, surfaces, surfaceStatus,
+ surfaceIteration);
+}
+
+MatrixN FootstepPlannerQP::computeTargetFootstep(int k, Vector6 const& q, Vector6 const& b_v, Vector6 const& b_vref,
+ SurfaceVectorVector const& potentialSurfaces,
+ SurfaceVector const& surfaces, bool const surfaceStatus,
+ int const surfaceIteration) {
+ surfaceStatus_ = surfaceStatus;
+ surfaceIteration_ = surfaceIteration;
+ surfaces_ = surfaces;
+ potentialSurfaces_ = potentialSurfaces;
+ // Rotation matrix along z axis
+ RPY_ = q.tail(3);
+ double c = std::cos(RPY_(2));
+ double s = std::sin(RPY_(2));
+ Rz.topLeftCorner<2, 2>() << c, -s, s, c;
+
+ // Current position in world frame, z = 0
+ q_tmp = q.head(3);
+ q_tmp(2) = 0.0;
+
+ // Compute the desired location of footsteps over the prediction horizon
+ computeFootsteps(k, b_v, b_vref);
+
+ // Update desired location of footsteps on the ground
+ updateTargetFootsteps();
+
+ // // Update target footstep in base frame
+ // for (int i = 0; i < 4; i++) {
+ // int index = 0;
+ // while (footsteps_[index](0, i) == 0.0) {
+ // index++;
+ // }
+ // targetFootstep_.col(i) << b_footsteps_[index](0, i), b_footsteps_[index](1, i), 0.0;
+ // }
+
+ return o_targetFootstep_;
+}
+
+void FootstepPlannerQP::computeFootsteps(int k, Vector6 const& b_v, Vector6 const& b_vref) {
+ for (uint i = 0; i < footsteps_.size(); i++) {
+ footsteps_[i] = Matrix34::Zero();
+ b_footsteps_[i] = Matrix34::Zero();
+ }
+ MatrixN gait = gait_->getCurrentGait();
+
+ // Set current position of feet for feet in stance phase
+ std::fill(footsteps_.begin(), footsteps_.end(), Matrix34::Zero());
+ for (int j = 0; j < 4; j++) {
+ if (gait(0, j) == 1.0) {
+ footsteps_[0].col(j) = currentFootstep_.col(j);
+ b_footsteps_[0].col(j) = Rz.transpose() * (currentFootstep_.col(j) - q_tmp );
+ }
+ }
+
+ // Cumulative time by adding the terms in the first column (remaining number of timesteps)
+ // Get future yaw yaws compared to current position
+ dt_cum(0) = dt_wbc * k;
+ yaws(0) = b_vref(5) * dt_cum(0) + RPY_(2);
+ yaws_b(0) = b_vref(5) * dt_cum(0);
+ for (uint j = 1; j < footsteps_.size(); j++) {
+ dt_cum(j) = gait.row(j).isZero() ? dt_cum(j - 1) : dt_cum(j - 1) + dt;
+ yaws(j) = b_vref(5) * dt_cum(j) + RPY_(2);
+ yaws_b(j) = b_vref(5) * dt_cum(j);
+ }
+
+ // Displacement following the reference velocity compared to current position
+ if (b_vref(5, 0) != 0) {
+ for (uint j = 0; j < footsteps_.size(); j++) {
+ dx(j) = (b_vref(0) * std::sin(b_vref(5) * dt_cum(j)) + b_vref(1) * (std::cos(b_vref(5) * dt_cum(j)) - 1.0)) /
+ b_vref(5);
+ dy(j) = (b_vref(1) * std::sin(b_vref(5) * dt_cum(j)) - b_vref(0) * (std::cos(b_vref(5) * dt_cum(j)) - 1.0)) /
+ b_vref(5);
+ }
+ } else {
+ for (uint j = 0; j < footsteps_.size(); j++) {
+ dx(j) = b_vref(0) * dt_cum(j);
+ dy(j) = b_vref(1) * dt_cum(j);
+ }
+ }
+
+ update_remaining_time(k);
+
+ // Update the footstep matrix depending on the different phases of the gait (swing & stance)
+ int phase = 0;
+ optimVector_.clear();
+ for (int i = 1; i < gait.rows(); i++)
+ {
+ // Feet that were in stance phase and are still in stance phase do not move
+ for (int j = 0; j < 4; j++) {
+ if (gait(i - 1, j) * gait(i, j) > 0) {
+ footsteps_[i].col(j) = footsteps_[i - 1].col(j);
+ b_footsteps_[i].col(j) = b_footsteps_[i - 1].col(j);
+ }
+ }
+
+ // Feet that were in swing phase and are now in stance phase need to be updated
+ for (int j = 0; j < 4; j++) {
+ if ((1 - gait(i - 1, j)) * gait(i, j) > 0) {
+ // Offset to the future position
+ q_dxdy << dx(i - 1, 0), dy(i - 1, 0), 0.0;
+
+ // Get future desired position of footsteps
+ computeNextFootstep(i, j, b_v, b_vref, next_footstep_, true);
+ computeNextFootstep(i, j, b_v, b_vref, next_footstep_qp_, false);
+
+ // Get desired position of footstep compared to current position
+ Rz_tmp.setZero();
+ double c = std::cos(yaws(i - 1));
+ double s = std::sin(yaws(i - 1));
+ Rz_tmp.topLeftCorner<2, 2>() << c, -s, s, c;
+
+ // Use directly the heuristic method
+ // footsteps_[i].col(j) = (Rz_tmp * next_footstep_ + q_tmp + Rz*q_dxdy).transpose();
+ next_footstep_qp_ = (Rz_tmp * next_footstep_qp_ + q_tmp + Rz*q_dxdy).transpose();
+
+
+ // Get desired position of footstep compared to current position
+ // Rz_tmp.setZero();
+ // c = std::cos(yaws_b(i - 1));
+ // s = std::sin(yaws_b(i - 1));
+ // Rz_tmp.topLeftCorner<2, 2>() << c, -s, s, c;
+ // b_footsteps_[i].col(j) = (Rz_tmp * next_footstep_ + q_dxdy).transpose();
+
+
+
+ // Check if current flying phase
+ bool flying_foot = false;
+ for (int other_foot = 0; other_foot < (int)feet_.size(); other_foot++) {
+ if (feet_[other_foot] == j) {
+ flying_foot = true;
+ }
+ }
+
+ if (flying_foot && phase == 0) // feet currently in flying phase
+ {
+ if (t0s[j] < 10e-4 and k % k_mpc == 0) // Beginning of flying phase
+ {
+ if (surfaceStatus_) {
+ selectedSurfaces_[j] = surfaces_[j];
+ } else {
+ selectedSurfaces_[j] = selectSurfaceFromPoint(next_footstep_, phase, j);
+ }
+ }
+ optimData optim_data = {i, j, selectedSurfaces_[j], next_footstep_qp_};
+ optimVector_.push_back(optim_data);
+ } else {
+ Surface sf_ = Surface();
+ if (surfaceStatus_) {
+ sf_ = surfaces_[j];
+ } else {
+ sf_ = selectSurfaceFromPoint(next_footstep_, phase, j);
+ }
+ optimData optim_data = {i, j, sf_, next_footstep_qp_};
+ optimVector_.push_back(optim_data);
+ }
+ }
+ }
+
+ if (!(gait.row(i - 1) - gait.row(i)).isZero()) {
+ phase += 1;
+ }
+ }
+
+ // Reset matrix Inequalities
+ G_.setZero();
+ h_.setZero();
+ x.setZero();
+ qp.reset(N, 0, M);
+
+ // Adapting q_ vector with reference velocity
+ Vector3 o_vref = Rz * b_vref.head(3);
+ std::cout<< "Rz :" << std::endl;
+ std::cout<< Rz << std::endl;
+ std::cout<< "b_vref :" << std::endl;
+ std::cout<< b_vref << std::endl;
+
+ std::cout<< "o_vref :" << std::endl;
+ std::cout<< o_vref << std::endl;
+ std::cout<< "weights_ :" << std::endl;
+ std::cout<< weights_ << std::endl;
+
+ q_(0) = -weights_(0) * o_vref(0);
+ q_(1) = -weights_(1) * o_vref(1);
+
+ // Convert problem to inequalities
+ int iStart = 0;
+ int foot = 0;
+ for (uint id_l = 0; id_l < optimVector_.size(); id_l++)
+ {
+ iStart = surfaceInequalities(iStart, optimVector_[id_l].surface, optimVector_[id_l].next_pos, id_l);
+ foot++;
+ }
+ std::cout<< "G :" << std::endl;
+ std::cout<< G_ << std::endl;
+ std::cout<< "h :" << std::endl;
+ std::cout<< h_ << std::endl;
+ std::cout<< "P :" << std::endl;
+ std::cout<< P_ << std::endl;
+ std::cout<< "Q :" << std::endl;
+ std::cout<< q_ << std::endl;
+
+
+ // Eiquadprog-Fast solves the problem :
+ // min. 1/2 * x' C_ x + q_' x
+ // s.t. C_ x + d_ = 0
+ // G_ x + h_ >= 0
+ status = qp.solve_quadprog(P_, q_, C_, d_, G_, h_, x);
+ std::cout << "status : " << status << std::endl;
+
+ // Retrieve results
+ voptim_.head(2) = x.head(2);
+ std::cout << "voptim : " << voptim_ << std::endl;
+
+ // Get new reference velocity in base frame to recompute the new footsteps
+ b_voptim_.head(3) = Rz.transpose() * voptim_; // lin velocity in base frame (rotated yaw)
+
+ // Update the foostep matrix with the position optimised, for changing phase index
+ for (uint id_l = 0; id_l < optimVector_.size(); id_l++) {
+ int i = optimVector_[id_l].phase;
+ int foot = optimVector_[id_l].foot;
+ std::cout << "optim vector surface : " << std::endl;
+ std::cout << optimVector_[id_l].surface.getVertices() << std::endl;
+ std::cout << optimVector_[id_l].phase << std::endl;
+ std::cout << optimVector_[id_l].foot << std::endl;
+ std::cout << optimVector_[id_l].next_pos << std::endl;
+
+ // Offset to the future position
+ q_dxdy << dx(i - 1, 0), dy(i - 1, 0), 0.0;
+
+ // Get future desired position of footsteps with k_feedback
+ computeNextFootstep(i, foot, b_voptim_, b_vref, next_footstep_qp_, true);
+
+ Vector3 delta_x = Vector3::Zero();
+ delta_x(0) += x(2 + 3 * id_l);
+ delta_x(1) += x(2 + 3 * id_l + 1);
+ delta_x(2) += x(2 + 3 * id_l + 2);
+
+ // World frame
+ Rz_tmp.setZero();
+ double c = std::cos(yaws(i - 1));
+ double s = std::sin(yaws(i - 1));
+ Rz_tmp.topLeftCorner<2, 2>() << c, -s, s, c;
+
+ Vector3 next_tmp = (Rz_tmp * next_footstep_qp_ + q_tmp + Rz*q_dxdy).transpose();
+ footsteps_[i].col(foot) = next_tmp + delta_x;
+ std::cout << "footsteps_[i].col(foot)" << std::endl;
+ std::cout << footsteps_[i].col(foot) << std::endl;
+
+ // Base frame
+ Rz_tmp.setZero();
+ c = std::cos(yaws_b(i - 1));
+ s = std::sin(yaws_b(i - 1));
+ Rz_tmp.topLeftCorner<2, 2>() << c, -s, s, c;
+
+ next_tmp = (Rz_tmp * next_footstep_qp_ + q_dxdy).transpose();
+ b_footsteps_[i].col(foot) = next_tmp + Rz.transpose()*delta_x;
+ }
+
+ // Update the next stance phase after the changing phase
+ for (int i = 1; i < gait.rows(); i++) {
+ // Feet that were in stance phase and are still in stance phase do not move
+ for (int foot = 0; foot < 4; foot++) {
+ if (gait(i - 1, foot) * gait(i, foot) > 0) {
+ footsteps_[i].col(foot) = footsteps_[i - 1].col(foot);
+ b_footsteps_[i].col(foot) = b_footsteps_[i-1].col(foot);
+ }
+ }
+ }
+
+ // std::cout << "OKOK : " << std::endl;
+}
+
+void FootstepPlannerQP::computeNextFootstep(int i, int j, Vector6 const& b_v, Vector6 const& b_vref,
+ Vector3& footstep, bool feedback_term) {
+ footstep.setZero(); // set to 0 the vector to fill
+
+ double t_stance = gait_->getPhaseDuration(i, j, 1.0); // 1.0 for stance phase
+
+ // Add symmetry term
+ footstep = t_stance * 0.5 * b_v.head(3);
+
+ // Add feedback term
+ footstep += params_->k_feedback * b_v.head(3);
+ if (feedback_term){
+ footstep += - params_->k_feedback * b_vref.head(3);
+ }
+
+ // Add centrifugal term
+ Vector3 cross;
+ cross << b_v(1) * b_vref(5) - b_v(2) * b_vref(4), b_v(2) * b_vref(3) - b_v(0) * b_vref(5), 0.0;
+ footstep += 0.5 * std::sqrt(h_ref / g) * cross;
+
+ // Legs have a limited length so the deviation has to be limited
+ footstep(0) = std::min(footstep(0), L);
+ footstep(0) = std::max(footstep(0), -L);
+ footstep(1) = std::min(footstep(1), L);
+ footstep(1) = std::max(footstep(1), -L);
+
+ // Add shoulders
+ Vector3 RP_ = RPY_;
+ RP_[2] = 0; // Yaw taken into account later
+ footstep += pinocchio::rpy::rpyToMatrix(RP_) * footsteps_under_shoulders_.col(j);
+ footstep += footsteps_offset_.col(j);
+
+ // Remove Z component (working on flat ground)
+ footstep(2) = 0.;
+}
+
+void FootstepPlannerQP::updateTargetFootsteps() {
+ for (int i = 0; i < 4; i++) {
+ int index = 0;
+ while (footsteps_[index](0, i) == 0.0) {
+ index++;
+ }
+ o_targetFootstep_.col(i) << footsteps_[index](0, i), footsteps_[index](1, i), footsteps_[index](2, i);
+ }
+}
+
+void FootstepPlannerQP::updateNewContact(Vector18 const& q) {
+ // Get position of the feet in world frame, using estimated state q
+ q_FK_.head(3) = q.head(3);
+ q_FK_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q(3, 0), q(4, 0), q(5,0))).coeffs();
+ q_FK_.tail(12) = q.tail(12);
+
+ // Update model and data of the robot
+ pinocchio::forwardKinematics(model_, data_, q_FK_);
+ pinocchio::updateFramePlacements(model_, data_);
+
+ // Get data required by IK with Pinocchio
+ for (int i = 0; i < 4; i++) {
+ pos_feet_.col(i) = data_.oMf[foot_ids_[i]].translation();
+ }
+
+ // std::cout << "--- pos_feet_: " << std::endl << pos_feet_ << std::endl;
+ // std::cout << "--- footsteps_:" << std::endl << footsteps_[1] << std::endl;
+
+ // Refresh position with estimated position if foot is in stance phase
+ for (int i = 0; i < 4; i++) {
+ if (gait_->getCurrentGaitCoeff(0, i) == 1.0) {
+ currentFootstep_.block(0, i, 2, 1) = pos_feet_.block(0, i, 2, 1); // Get only x and y from IK
+ currentFootstep_(2, i) = footsteps_[1](2, i); // Z from the height map
+ }
+ }
+}
+
+void FootstepPlannerQP::update_remaining_time(int k) {
+ if ((k % k_mpc) == 0) {
+ feet_.clear();
+ t0s.setZero();
+
+ // Indexes of feet in swing phase
+ for (int i = 0; i < 4; i++) {
+ if (gait_->getCurrentGait()(0, i) == 0) feet_.push_back(i);
+ }
+ // If no foot in swing phase
+ if (feet_.size() == 0) return;
+
+ // For each foot in swing phase get remaining duration of the swing phase
+ for (int foot = 0; foot < (int)feet_.size(); foot++) {
+ int i = feet_[foot];
+ t_swing[i] = gait_->getPhaseDuration(0, feet_[foot], 0.0); // 0.0 for swing phase
+ double value = t_swing[i] - (gait_->getRemainingTime() * k_mpc - ((k + 1) % k_mpc)) * dt_wbc - dt_wbc;
+ t0s[i] = std::max(0.0, value);
+ }
+ } else {
+ // If no foot in swing phase
+ if (feet_.size() == 0) return;
+
+ // Increment of one time step for feet_ in swing phase
+ for (int i = 0; i < (int)feet_.size(); i++) {
+ double value = t0s[feet_[i]] + dt_wbc;
+ t0s[feet_[i]] = std::max(0.0, value);
+ }
+ }
+}
+
+Surface FootstepPlannerQP::selectSurfaceFromPoint(Vector3 const& point, int phase, int moving_foot_index) {
+ double sfHeight = 0.;
+ bool surfaceFound = false;
+
+ Surface sf;
+
+ if (surfaceIteration_) {
+ SurfaceVector potentialSurfaces = potentialSurfaces_[moving_foot_index];
+ sf = potentialSurfaces[0];
+ for (uint i = 0; i < potentialSurfaces.size(); i++) {
+ if (potentialSurfaces[i].hasPoint(point.head(2))) {
+ double height = sf.getHeight(point.head(2));
+ if (height > sfHeight) {
+ sfHeight = height;
+ sf = potentialSurfaces[i];
+ surfaceFound = true;
+ }
+ }
+ }
+
+ if (not surfaceFound) {
+ // TODO
+
+ // obj1 = simple_object(np.array([[point[0], point[1], 0.]]), [[0, 0, 0]])
+ // o1 = fclObj_trimesh(obj1)
+ // t1 = hppfcl.Transform3f()
+ // t2 = hppfcl.Transform3f()
+ // distance = 100
+
+ // for sf in potential_surfaces:
+ // vert = np.zeros(sf.vertices.shape)
+ // vert[:, :2] = sf.vertices[:, :2]
+ // tri = Delaunay(vert[:, :2])
+ // obj2 = simple_object(vert, tri.simplices.tolist())
+ // o2 = fclObj_trimesh(obj2)
+ // gg = gjk(o1, o2, t1, t2)
+
+ // if gg.distance <= distance:
+ // surface_selected = sf
+ // distance = gg.distance
+ }
+ } else {
+ sf = initialSurface_;
+ }
+ return sf;
+}
+
+int FootstepPlannerQP::surfaceInequalities(int i_start, Surface const& surface, Vector3 const& next_ft, int id_l) {
+ int n_rows = int(surface.getA().rows());
+ G_.block(i_start, 0, n_rows, 2) = params_->k_feedback * surface.getA().block(0, 0, n_rows, 2);
+ G_.block(i_start, 2 + 3 * id_l, n_rows, 3) = -surface.getA();
+ h_.segment(i_start, n_rows) = surface.getb() - surface.getA() * next_ft;
+
+ std::cout<< "\n\n :" << std::endl;
+ std::cout<< "id_l :" << std::endl;
+ std::cout<< id_l << std::endl;
+ std::cout<< "id_l :" << std::endl;
+ std::cout<< id_l << std::endl;
+
+ std::cout<< "surface.getb() :" << std::endl;
+ std::cout<< surface.getb() << std::endl;
+ std::cout<< "surface.getA() :" << std::endl;
+ std::cout<< surface.getA() << std::endl;
+ std::cout<< "params_->k_feedback :" << std::endl;
+ std::cout<< params_->k_feedback << std::endl;
+ std::cout<< ">next_ft :" << std::endl;
+ std::cout<< next_ft << std::endl;
+ std::cout<< ">surface.getA().block(0, 0, n_rows, 2) :" << std::endl;
+ std::cout<< surface.getA().block(0, 0, n_rows, 2) << std::endl;
+ std::cout<< "j" << std::endl;
+ std::cout<< surface.getA().block(0, 0, n_rows, 2) << std::endl;
+
+
+
+
+
+ return i_start + n_rows;
+}
+
+MatrixN FootstepPlannerQP::getFootsteps() { return vectorToMatrix(b_footsteps_); }
+MatrixN FootstepPlannerQP::getTargetFootsteps() { return targetFootstep_; }
+MatrixN FootstepPlannerQP::getRz() { return Rz; }
+
+MatrixN FootstepPlannerQP::vectorToMatrix(std::vector<Matrix34> const& array) {
+ MatrixN M = MatrixN::Zero(array.size(), 12);
+ for (uint i = 0; i < array.size(); i++) {
+ for (int j = 0; j < 4; j++) {
+ M.row(i).segment<3>(3 * j) = array[i].col(j);
+ }
+ }
+ return M;
+}
diff --git a/src/Heightmap.cpp b/src/Heightmap.cpp
new file mode 100644
index 00000000..c4b4ac2d
--- /dev/null
+++ b/src/Heightmap.cpp
@@ -0,0 +1,95 @@
+#include "qrw/Heightmap.hpp"
+
+Heightmap::Heightmap() {
+ // empty
+}
+
+void Heightmap::initialize(const std::string& file_name) {
+ // Open the binary file
+ std::ifstream iF(file_name, std::ios::in | std::ios::out | std::ios::binary);
+ if (!iF) {
+ throw std::runtime_error("Error while opening heighmap binary file");
+ }
+
+ // Extract header from binary file
+ iF.read(reinterpret_cast<char*>(&header_), sizeof header_);
+
+ // Resize matrix and vector according to header
+ z_ = MatrixN::Zero(header_.size_x, header_.size_y);
+ x_ = VectorN::LinSpaced(header_.size_x, header_.x_init, header_.x_end);
+ y_ = VectorN::LinSpaced(header_.size_y, header_.y_init, header_.y_end);
+
+ dx_ = std::abs((header_.x_init - header_.x_end) / (header_.size_x - 1));
+ dy_ = std::abs((header_.y_init - header_.y_end) / (header_.size_y - 1));
+
+ FIT_SIZE_X = 0.4;
+ FIT_SIZE_Y = 0.4;
+ FIT_NX = 10;
+ FIT_NY = 5;
+
+ int i = 0;
+ int j = 0;
+ double read;
+ // Read the file and extract heightmap matrix
+ while (i < header_.size_x && !iF.eof()) {
+ j = 0;
+ while (j < header_.size_y && !iF.eof()) {
+ iF.read(reinterpret_cast<char*>(&read), sizeof read);
+ z_(i, j) = read;
+ j++;
+ }
+ i++;
+ }
+
+ A = MatrixN::Zero(FIT_NX * FIT_NY, 3);
+ b = VectorN::Zero(FIT_NX * FIT_NY);
+ surface_eq = Vector3::Zero();
+}
+
+int Heightmap::map_x(double x) {
+ if (x < header_.x_init || x > header_.x_end) {
+ return -10;
+ } else {
+ return (int)std::round((x - header_.x_init) / dx_);
+ }
+}
+
+int Heightmap::map_y(double y) {
+ if (y < header_.y_init || y > header_.y_end) {
+ return -10;
+ } else {
+ return (int)std::round((y - header_.y_init) / dy_);
+ }
+}
+
+double Heightmap::get_height(double x, double y) {
+ int index_x = map_x(x);
+ int index_y = map_y(y);
+ if (index_x == -10 || index_y == -10) {
+ return 0.0;
+ } else {
+ return z_(index_x, index_y);
+ }
+}
+
+void Heightmap::update_mean_surface(double x, double y) {
+ VectorN x_surface = VectorN::LinSpaced(FIT_NX, x - FIT_SIZE_X / 2, x + FIT_SIZE_X / 2);
+ VectorN y_surface = VectorN::LinSpaced(FIT_NY, y - FIT_SIZE_Y / 2, x + FIT_SIZE_Y / 2);
+
+ int i_pb = 0;
+ int idx, idy = 0;
+ for (int i = 0; i < FIT_NX; i++) {
+ for (int j = 0; j < FIT_NY; j++) {
+ idx = map_x(x_surface[i]);
+ idy = map_y(y_surface[j]);
+ A.block(i_pb, 0, 1, 3) << x_(idx), y_(idy), 1.0;
+ b.block(i_pb, 0, 1, 1) << z_(idx, idy);
+ i_pb += 1;
+ }
+ }
+
+ qp.reset(3, 0, 0);
+ P_ = A.transpose() * A;
+ q_ = -A.transpose() * b;
+ status = qp.solve_quadprog(P_, q_, C_, d_, G_, h_, surface_eq);
+}
diff --git a/src/Params.cpp b/src/Params.cpp
index 355c21bb..37b72072 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -214,6 +214,18 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "Fz_min");
Fz_min = robot_node["Fz_min"].as<double>();
+
+ assert_yaml_parsing(robot_node, "robot", "solo3D");
+ solo3D = robot_node["solo3D"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "enable_multiprocessing_mip");
+ enable_multiprocessing_mip = robot_node["enable_multiprocessing_mip"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "environment_URDF");
+ environment_URDF = robot_node["environment_URDF"].as<std::string>();
+
+ assert_yaml_parsing(robot_node, "robot", "environment_heightmap");
+ environment_heightmap = robot_node["environment_heightmap"].as<std::string>();
}
void Params::convert_gait_vec() {
diff --git a/src/StatePlanner3D.cpp b/src/StatePlanner3D.cpp
new file mode 100644
index 00000000..f57a2d8b
--- /dev/null
+++ b/src/StatePlanner3D.cpp
@@ -0,0 +1,130 @@
+#include "qrw/StatePlanner3D.hpp"
+
+StatePlanner3D::StatePlanner3D() : dt_(0.0), h_ref_(0.0), n_steps_(0), RPY_(Vector3::Zero()) {
+ // Empty
+}
+
+void StatePlanner3D::initialize(Params& params) {
+ dt_ = params.dt_mpc;
+ h_ref_ = params.h_ref;
+ n_steps_ = static_cast<int>(params.gait.rows());
+ T_step_ = params.T_gait / 2;
+ referenceStates_ = MatrixN::Zero(12, 1 + n_steps_);
+ dt_vector_ = VectorN::LinSpaced(n_steps_, dt_, static_cast<double>(n_steps_) * dt_);
+ heightmap_.initialize(params.environment_heightmap);
+ configs = MatrixN::Zero(7,n_surface_configs);
+}
+
+void StatePlanner3D::computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, int is_new_step) {
+ if (q.rows() != 6) {
+ throw std::runtime_error("q should be a vector of size 6");
+ }
+ if (is_new_step) {
+ heightmap_.update_mean_surface(q(0), q(1)); // Update surface equality before new step
+ rpy_map(0) = -std::atan2(heightmap_.surface_eq(1), 1.);
+ rpy_map(1) = -std::atan2(heightmap_.surface_eq(0), 1.);
+ compute_configurations(q,vref);
+ }
+
+ RPY_ = q.tail(3);
+
+ // Update the current state
+ referenceStates_(0, 0) = 0.0; // In horizontal frame x = 0.0
+ referenceStates_(1, 0) = 0.0; // In horizontal frame y = 0.0
+ referenceStates_(2, 0) = q(2, 0); // We keep height
+ referenceStates_.block(3, 0, 2, 1) = RPY_.head(2); // We keep roll and pitch
+ referenceStates_(5, 0) = 0.0; // In horizontal frame yaw = 0.0
+ referenceStates_.block(6, 0, 3, 1) = v.head(3);
+ referenceStates_.block(9, 0, 3, 1) = v.tail(3);
+
+ for (int i = 0; i < n_steps_; i++) {
+ if (std::abs(vref(5)) >= 0.001) {
+ referenceStates_(0, 1 + i) =
+ (vref(0) * std::sin(vref(5) * dt_vector_(i)) + vref(1) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) /
+ vref(5);
+ referenceStates_(1, 1 + i) =
+ (vref(1) * std::sin(vref(5) * dt_vector_(i)) - vref(0) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) /
+ vref(5);
+ } else {
+ referenceStates_(0, 1 + i) = vref(0) * dt_vector_(i);
+ referenceStates_(1, 1 + i) = vref(1) * dt_vector_(i);
+ }
+ referenceStates_(0, 1 + i) += referenceStates_(0, 0);
+ referenceStates_(1, 1 + i) += referenceStates_(1, 0);
+
+ referenceStates_(2, 1 + i) = h_ref_;
+
+ referenceStates_(5, 1 + i) = vref(5) * dt_vector_(i);
+
+ referenceStates_(6, 1 + i) =
+ vref(0) * std::cos(referenceStates_(5, 1 + i)) - vref(1) * std::sin(referenceStates_(5, 1 + i));
+ referenceStates_(7, 1 + i) =
+ vref(0) * std::sin(referenceStates_(5, 1 + i)) + vref(1) * std::cos(referenceStates_(5, 1 + i));
+
+ // referenceStates_(5, 1 + i) += RPY_(2);
+
+ referenceStates_(11, 1 + i) = vref(5);
+
+ // Update according to heightmap
+ int idx = heightmap_.map_x(referenceStates_(0, i + 1));
+ int idy = heightmap_.map_y(referenceStates_(1, i + 1));
+ double z = heightmap_.surface_eq(0) * heightmap_.x_(idx) + heightmap_.surface_eq(1) * heightmap_.y_(idy) +
+ heightmap_.surface_eq(2);
+
+ referenceStates_(2, i + 1) = h_ref_ + z;
+
+ referenceStates_(3, 1 + i) = rpy_map[0] * std::cos(RPY_[2]) - rpy_map[1] * std::sin(RPY_[2]);
+ referenceStates_(4, 1 + i) = rpy_map[0] * std::sin(RPY_[2]) + rpy_map[1] * std::cos(RPY_[2]);
+ }
+
+ // Update velocities according to heightmap
+ for (int i = 0; i < n_steps_; i++) {
+ if (i == 0) {
+ referenceStates_(8, 1) = std::max(std::min((referenceStates_(2, 1) - q[2]) / dt_, v_max_z), -v_max_z);
+ referenceStates_(9, 1) = std::max(std::min((referenceStates_(3, 1) - RPY_[0]) / dt_, v_max), -v_max);
+ referenceStates_(10, 1) = std::max(std::min((referenceStates_(4, 1) - RPY_[1]) / dt_, v_max), -v_max);
+ } else {
+ referenceStates_(9, 1 + i) = 0.;
+ referenceStates_(10, 1 + i) = 0.;
+ referenceStates_(8, 1 + i) = (referenceStates_(2, 2) - referenceStates_(2, 1)) / dt_;
+ }
+ }
+}
+
+void StatePlanner3D::compute_configurations(VectorN const& q, Vector6 const& vref) {
+
+ pinocchio::SE3::Quaternion quat_;
+ for (int i = 0; i < n_surface_configs; i++) {
+ Vector7 config_ = Vector7::Zero();
+ // TODO : Not sure if (i+1)*T_step --> next step for MIP, not current
+ double dt_config = T_step_ * i;
+ // TODO : Not sure, take into account height, maybe useless since most constraints desactivated in MIP
+ config_.head(3) = q.head(3);
+ if (std::abs(vref(5)) >= 0.001) {
+ config_(0) +=
+ (vref(0) * std::sin(vref(5) * dt_config) + vref(1) * (std::cos(vref(5) * dt_config) - 1.0)) / vref(5);
+ config_(1) +=
+ (vref(1) * std::sin(vref(5) * dt_config) - vref(0) * (std::cos(vref(5) * dt_config) - 1.0)) / vref(5);
+ } else {
+ config_(0) += vref(0) * dt_config;
+ config_(1) += vref(1) * dt_config;
+ }
+
+ Vector3 rpy_config = Vector3::Zero();
+ rpy_config(2) = q(5) + vref(5) * dt_config;
+
+ // Update according to heightmap
+ int idx = heightmap_.map_x(config_(0));
+ int idy = heightmap_.map_y(config_(1));
+ config_(2) = heightmap_.surface_eq(0) * heightmap_.x_(idx) + heightmap_.surface_eq(1) * heightmap_.y_(idy) +
+ heightmap_.surface_eq(2) + h_ref_;
+
+ rpy_config(0) = rpy_map[0] * std::cos(rpy_config[2]) - rpy_map[1] * std::sin(rpy_config[2]);
+ rpy_config(1) = rpy_map[0] * std::sin(rpy_config[2]) + rpy_map[1] * std::cos(rpy_config[2]);
+
+ quat_ = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(rpy_config));
+ config_.tail(4) = quat_.coeffs();
+ // configs.push_back(config_);
+ configs.block(0,i,7,1) = config_;
+ }
+}
diff --git a/src/Surface.cpp b/src/Surface.cpp
new file mode 100644
index 00000000..24ab1746
--- /dev/null
+++ b/src/Surface.cpp
@@ -0,0 +1,37 @@
+#include "qrw/Surface.hpp"
+
+Surface::Surface() {
+ // Empty
+}
+
+Surface::Surface(const MatrixN& A_in, const VectorN& b_in, const MatrixN& vertices_in)
+ : A_{A_in}, b_{b_in}, vertices_{vertices_in} {
+ // Empty
+}
+
+MatrixN Surface::getA() const { return A_; }
+
+void Surface::setA(MatrixN const& A_in) { A_ = A_in; }
+
+VectorN Surface::getb() const { return b_; }
+
+void Surface::setb(VectorN const& b_in) { b_ = b_in; }
+
+MatrixN Surface::getVertices() const { return vertices_; }
+
+void Surface::setVertices(const MatrixN& vertices_in) { vertices_ = vertices_in; }
+
+double Surface::getHeight(Vector2 const& point) const {
+ int id = A_.rows() - 1;
+ return abs(b_(id) - point(0) * A_(id, 0) / A_(id, 2) - point(1) * A_(id, 1) / A_(id, 2));
+}
+
+bool Surface::hasPoint(Vector2 const& point) const {
+ VectorN Ax = A_.block(0, 0, A_.rows() - 2, 2) * point;
+ for (int i; i < b_.size() - 2; i++) {
+ if (Ax(i) > b_(i)) {
+ return false;
+ }
+ }
+ return true;
+}
\ No newline at end of file
diff --git a/src/walk_parameters.yaml b/src/walk_parameters.yaml
index 17f0e731..8956a87e 100644
--- a/src/walk_parameters.yaml
+++ b/src/walk_parameters.yaml
@@ -15,7 +15,7 @@ robot:
enable_corba_viewer: true # Enable/disable Corba Viewer
enable_multiprocessing: false # Enable/disable running the MPC in another process in parallel of the main loop
perfect_estimator: false # Enable/disable perfect estimator by using data directly from PyBullet
-
+
# General control parameters
# [0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583] # h_com = 0.2
q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407] #Â h_com = 0.218
@@ -23,8 +23,8 @@ robot:
dt_wbc: 0.001 # Time step of the whole body control
dt_mpc: 0.02 # Time step of the model predictive control
type_MPC: 0 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
- Kp_main: [3.0, 3.0, 3.0] # Proportional gains for the PD+
- Kd_main: [0.3, 0.3, 0.3] # Derivative gains for the PD+
+ Kp_main: [1.0, 1.0, 1.0] # Proportional gains for the PD+
+ Kd_main: [0.1, 0.1, 0.1] # Derivative gains for the PD+
Kff_main: 1.0 # Feedforward torques multiplier for the PD+
# Parameters of Gait
@@ -68,3 +68,12 @@ robot:
Q2: 10.0 # Weights for the "delta contact forces" optimization variables
Fz_max: 35.0 # Maximum vertical contact force [N]
Fz_min: 0.0 # Minimal vertical contact force [N]
+<<<<<<< HEAD:src/walk_parameters.yaml
+=======
+
+ # Parameters fro solo3D simulation
+ solo3D: true # Activation of the 3D environment, and corresponding planner blocks
+ enable_multiprocessing_mip: false # Enable/disable running the MIP in another process in parallel of the main loop
+ environment_URDF: "/home/thomas_cbrs/Desktop/edin/quadruped-files/one_step_large/one_step_large.urdf" # URDF files
+ environment_heightmap: "/home/thomas_cbrs/Desktop/edin/quadruped-files/one_step_large/one_step_large.bin"
+>>>>>>> solo3D integration:src/config_solo12.yaml
--
GitLab