diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 74bed7a99b7503d7b7a53698e83d8a7a1c915ea8..d3f328f32a9676abfb3936daa9d648de543a8f01 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, VectorN const& dummyPos);
+ Vector3 const& baseOrientation, VectorN const& q_perfect);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -158,13 +158,13 @@ class Estimator {
/// \param[in] baseOrientation Quaternion orientation of the IMU
/// \param[in] q_mes Position of the 12 actuators
/// \param[in] v_mes Velocity of the 12 actuators
- /// \param[in] dummyPos Position of the robot in PyBullet simulator (only for simulation)
- /// \param[in] b_baseVel Velocity of the robot in PyBullet simulator (only for simulation)
+ /// \param[in] q_perfect Position of the robot in PyBullet simulator (only for simulation)
+ /// \param[in] b_baseVel_perfect Velocity of the robot in PyBullet simulator (only for simulation)
///
////////////////////////////////////////////////////////////////////////////////////////////////
void run_filter(MatrixN const& gait, MatrixN const& goals, VectorN const& baseLinearAcceleration,
VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q_mes,
- VectorN const& v_mes, VectorN const& dummyPos, Vector3 const& b_baseVel);
+ VectorN const& v_mes, VectorN const& q_perfect, Vector3 const& b_baseVel_perfect);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/include/qrw/FootTrajectoryGeneratorBezier.hpp b/include/qrw/FootTrajectoryGeneratorBezier.hpp
index aa9a8781aa6fe274e800344af9af37b1bd3f4c4a..7cf1f09c92e1cd7b878f2414fcf714839f060afc 100644
--- a/include/qrw/FootTrajectoryGeneratorBezier.hpp
+++ b/include/qrw/FootTrajectoryGeneratorBezier.hpp
@@ -10,25 +10,23 @@
#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 "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 "ndcurves/fwd.h"
-#include "ndcurves/bezier_curve.h"
-#include "ndcurves/optimization/details.h"
#include <Eigen/Dense>
#include <vector>
#include "eiquadprog/eiquadprog-fast.hpp"
+#include "ndcurves/bezier_curve.h"
+#include "ndcurves/fwd.h"
+#include "ndcurves/optimization/details.h"
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "qrw/Gait.hpp"
+#include "qrw/Params.hpp"
+#include "qrw/Surface.hpp"
+#include "qrw/Types.h"
using namespace ndcurves;
using namespace eiquadprog::solvers;
@@ -86,6 +84,8 @@ class FootTrajectoryGeneratorBezier {
///
////////////////////////////////////////////////////////////////////////////////////////////////
void update(int k, MatrixN const& targetFootstep, SurfaceVector const& surfacesSelected, VectorN const& q);
+ void updateDebug(int k, MatrixN const& targetFootstep, SurfaceVector const& surfacesSelected,
+ MatrixN const& currentPosition);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -156,6 +156,7 @@ class FootTrajectoryGeneratorBezier {
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;
+ bool useBezier;
// Number of points in the least square problem
int N_samples;
diff --git a/include/qrw/FootstepPlannerQP.hpp b/include/qrw/FootstepPlannerQP.hpp
index ba389fee8f0943302360f382212e17814e331f8e..2c4ffd199d1d3a80a441e4b37cc6272fc1e357bf 100644
--- a/include/qrw/FootstepPlannerQP.hpp
+++ b/include/qrw/FootstepPlannerQP.hpp
@@ -11,18 +11,19 @@
#ifndef FOOTSTEPPLANNERQP_H_INCLUDED
#define FOOTSTEPPLANNERQP_H_INCLUDED
+#include <vector>
+
+#include "eiquadprog/eiquadprog-fast.hpp"
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/math/rpy.hpp"
-#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/multibody/model.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
@@ -30,13 +31,17 @@ 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 constant_term;
- Matrix3 Rz_tmp;
+struct Pair {
+ double F; // First
+ double S; // Second
+};
+
+struct optimData {
+ int phase;
+ int foot;
+ Surface surface;
+ Vector3 constant_term;
+ Matrix3 Rz_tmp;
};
class FootstepPlannerQP {
@@ -80,10 +85,18 @@ class FootstepPlannerQP {
/// \param[in] b_vref Desired velocity vector of the flying base in horizontal frame (linear and angular
/// stacked)
///
+ /// Precondition : updateSurfaces should have been called to store the new surface planner result
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ MatrixN updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref);
+
////////////////////////////////////////////////////////////////////////////////////////////////
- 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);
+ ///
+ /// \brief Updates the surfaces result from the surface planner
+ ///
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+ void updateSurfaces(SurfaceVectorVector const& potentialSurfaces, SurfaceVector const& surfaces,
+ bool const surfaceStatus, int const surfaceIteration);
MatrixN getFootsteps();
MatrixN getTargetFootsteps();
@@ -104,9 +117,7 @@ class FootstepPlannerQP {
/// 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);
+ MatrixN computeTargetFootstep(int k, Vector6 const& q, Vector6 const& b_v, Vector6 const& b_vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -119,7 +130,8 @@ class FootstepPlannerQP {
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Compute a X by 13 matrix containing the remaining number of steps of each phase of the gait (first column)
+ /// \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.
///
@@ -183,6 +195,13 @@ class FootstepPlannerQP {
////////////////////////////////////////////////////////////////////////////////////////////////
MatrixN vectorToMatrix(std::vector<Matrix34> const& array);
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute a distance from a point to a segment.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ double minDistance(Pair const& A, Pair const& B, Pair const& E);
+
Params* params_; // Params object to store parameters
Gait* gait_; // Gait object to hold the gait informations
@@ -253,13 +272,13 @@ class FootstepPlannerQP {
MatrixN C_;
VectorN d_;
-
// qp solver
EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
EiquadprogFast_status status;
VectorN x;
EiquadprogFast qp;
+ bool useSL1M;
bool surfaceStatus_;
int surfaceIteration_;
SurfaceVector surfaces_;
diff --git a/include/qrw/Heightmap.hpp b/include/qrw/Heightmap.hpp
index 90dcb711c65d88327ff4a66b1ea50fed9deaf710..124eb39d4eb3bcdd33c0aa41e2a5657a02bac478 100644
--- a/include/qrw/Heightmap.hpp
+++ b/include/qrw/Heightmap.hpp
@@ -10,16 +10,18 @@
#define HEIGHTMAP_H_INCLUDED
#include <stdio.h>
+
+#include <Eigen/Dense>
#include <fstream>
#include <iostream>
-#include "qrw/Types.h"
-#include "qrw/Params.hpp"
-#include <Eigen/Dense>
+
#include "eiquadprog/eiquadprog-fast.hpp"
+#include "qrw/Params.hpp"
+#include "qrw/Types.h"
using namespace std;
-struct Header {
+struct Map {
int size_x;
int size_y;
double x_init;
@@ -60,7 +62,7 @@ class Heightmap {
/// \param[in] x x-axis position
///
////////////////////////////////////////////////////////////////////////////////////////////////
- int map_x(double x);
+ int xIndex(double x);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -69,7 +71,7 @@ class Heightmap {
/// \param[in] y y-axis position
///
////////////////////////////////////////////////////////////////////////////////////////////////
- int map_y(double y);
+ int yIndex(double y);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -79,7 +81,7 @@ class Heightmap {
/// \param[in] y y-axis position
///
////////////////////////////////////////////////////////////////////////////////////////////////
- double get_height(double x, double y);
+ double getHeight(double x, double y);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -90,42 +92,36 @@ class Heightmap {
/// \param[in] y y-axis position
///
////////////////////////////////////////////////////////////////////////////////////////////////
- Vector3 compute_mean_surface(double x, double y);
+ Vector3 fitSurface_(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
+ VectorN fit_; // [a,b,c], such as ax + by -z + c = 0
private:
- Header header_; // Contain the size and parameters of the heightmap
+ Params p; // parameters
+
+ Map map_; // 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 P_ = MatrixN::Zero(3, 3);
+ VectorN q_ = VectorN::Zero(3);
- MatrixN G_= MatrixN::Zero(3, 3);
- VectorN h_= 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);
+ 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;
+ MatrixN A_;
+ VectorN b_;
};
#endif // HEIGHTMAP_H_INCLUDED
diff --git a/include/qrw/Joystick.hpp b/include/qrw/Joystick.hpp
index 1a91010b2933122ff217680871f452045fe4e072..515d4b75e321b05b4d9e39352b12f8310b2c92d7 100644
--- a/include/qrw/Joystick.hpp
+++ b/include/qrw/Joystick.hpp
@@ -10,17 +10,14 @@
#define JOYSTICK_H_INCLUDED
#include <fcntl.h>
+#include <linux/joystick.h>
#include <stdio.h>
#include <unistd.h>
-#include <linux/joystick.h>
-#include <iostream>
-#include <fstream>
-#include <cmath>
+
#include <chrono>
-#include <Eigen/Core>
-#include <Eigen/Dense>
-#include "qrw/Types.h"
+
#include "qrw/Params.hpp"
+#include "qrw/Types.h"
struct gamepad_struct {
double v_x = 0.0; // Up/down status of left pad
@@ -68,20 +65,7 @@ class Joystick {
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Compute the remaining and total duration of a swing phase or a stance phase based
- /// on the content of the gait matrix
- ///
- /// \param[in] k Numero of the current loop
- /// \param[in] k_switch Information about the position of key frames
- /// \param[in] v_switch Information about the desired velocity for each key frame
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- VectorN handle_v_switch_py(double k, VectorN const& k_switch_py, MatrixN const& v_switch_py);
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Compute the remaining and total duration of a swing phase or a stance phase based
- /// on the content of the gait matrix
+ /// \brief update the
///
/// \param[in] k Numero of the current loop
///
@@ -162,8 +146,8 @@ class Joystick {
double dt_wbc = 0.0; // Time step of the WBC
int k_mpc = 0; // Number of WBC time step for one MPC time step
- Eigen::Matrix<int, 1, Eigen::Dynamic> k_switch; // Key frames for the polynomial velocity interpolation
- Eigen::Matrix<double, 6, Eigen::Dynamic> v_switch; // Target velocity for the key frames
+ VectorNint k_switch; // Key frames for the polynomial velocity interpolation
+ Matrix6N v_switch; // Target velocity for the key frames
// How much the gamepad velocity and position is filtered to avoid sharp changes
double gp_alpha_vel = 0.0; // Low pass filter coefficient for v_ref_ (if gamepad-controlled)
@@ -171,8 +155,8 @@ class Joystick {
double gp_alpha_vel_heavy_filter = 0.002; // Low pass filter coefficient for v_ref_heavy_filter_
// Maximum velocity values
- double vXScale = 0.3; // Lateral
- double vYScale = 0.5; // Forward
+ double vXScale = 0.3; // Lateral
+ double vYScale = 0.5; // Forward
double vYawScale = 1.0; // Rotation
// Maximum position values
diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp
index 5bab3bc8cb0e8a73592a5f2ccdaa2f4a3d056830..9cc9a17e95183090ba8eabb3b6939a285b6975f4 100644
--- a/include/qrw/MPC.hpp
+++ b/include/qrw/MPC.hpp
@@ -52,6 +52,14 @@ class MPC {
////////////////////////////////////////////////////////////////////////////////////////////////
int run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in);
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Retrieve the value of the cost function at the end of the resolution
+ /// \return the cost value
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ float retrieve_cost();
+
// Getters
Eigen::MatrixXd get_latest_result(); // Return the latest desired contact forces that have been computed
Eigen::MatrixXd get_gait(); // Return the gait matrix
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index c9366e95879594b13d92f13cfea5dcfc36a76e43..3801c797abcbb36701b6f463e054fe996d36b6a8 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -11,9 +11,10 @@
#ifndef PARAMS_H_INCLUDED
#define PARAMS_H_INCLUDED
-#include "qrw/Types.h"
#include <yaml-cpp/yaml.h>
+#include "qrw/Types.h"
+
class Params {
public:
////////////////////////////////////////////////////////////////////////////////////////////////
@@ -46,7 +47,24 @@ class Params {
////////////////////////////////////////////////////////////////////////////////////////////////
void convert_gait_vec();
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Convert the t_switch vector of the yaml into an Eigen vector
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void convert_t_switch();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Convert the v_switch vector of the yaml into an Eigen matrix
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void convert_v_switch();
+
MatrixN get_gait() { return gait; }
+ VectorN get_t_switch() { return t_switch; }
+ MatrixN get_v_switch() { return v_switch; }
+ void set_v_switch(MatrixN v_switch_in) { v_switch = v_switch_in; }
// See .yaml file for meaning of parameters
// General parameters
@@ -81,8 +99,12 @@ class Params {
std::vector<int> gait_vec; // Initial gait matrix (vector)
// Parameters of Joystick
- double gp_alpha_vel; //Â Coefficient of the low pass filter applied to gamepad velocity
- double gp_alpha_pos; //Â Coefficient of the low pass filter applied to gamepad position
+ double gp_alpha_vel; //Â Coefficient of the low pass filter applied to gamepad velocity
+ double gp_alpha_pos; //Â Coefficient of the low pass filter applied to gamepad position
+ std::vector<double> t_switch_vec; // Predefined velocity switch times vector
+ VectorN t_switch; // Predefined velocity switch times matrix
+ std::vector<double> v_switch_vec; // Predefined velocity switch values vector
+ MatrixN v_switch; // Predefined velocity switch values matrix
// Parameters of Estimator
double fc_v_esti; // Cut frequency for the low pass that filters the estimated base velocity
@@ -119,10 +141,17 @@ class Params {
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
+ std::string environment_heightmap; // Path to the heightmap
+ double heightmap_fit_length; // Size of the heightmap around the robot
+ int heightmap_fit_size; // Number of points used in the heightmap QP
+ int number_steps; // Number of steps to optimize with the MIP
+ std::vector<double> max_velocity; // Maximum velocity of the base
+ bool use_bezier; // Use Bezier to plan trajectories, otherwise use simple 6d polynomial curve.
+ bool use_sl1m; // Use SL1M to select the surfaces, otherwise use Raibert heuristic projection in 2D.
+ bool use_heuristic; // Use heuristic as SL1M cost.
// Not defined in yaml
- Eigen::MatrixXd gait; // Initial gait matrix (Eigen)
+ MatrixN gait; // Initial gait matrix (Eigen)
double T_gait; // Period of the gait
double mass; // Mass of the robot
std::vector<double> I_mat; // Inertia matrix
diff --git a/include/qrw/StatePlanner.hpp b/include/qrw/StatePlanner.hpp
index f78b3fd6a66a5a4e600a1fc0aed769690a31df1d..7d8eedaaf391ba0e05b5bf45c2d3568f87b31dc7 100644
--- a/include/qrw/StatePlanner.hpp
+++ b/include/qrw/StatePlanner.hpp
@@ -55,7 +55,7 @@ class StatePlanner {
void computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref);
MatrixN getReferenceStates() { return referenceStates_; }
- int getNSteps() { return n_steps_; }
+ int getNumberStates() { return n_steps_; }
private:
double dt_; // Time step of the contact sequence (time step of the MPC)
diff --git a/include/qrw/StatePlanner3D.hpp b/include/qrw/StatePlanner3D.hpp
index f16649c3c8b39620c499c6e8cf5696c011f31821..261eea609ea26829a47d0aed9a1710963f80cdd5 100644
--- a/include/qrw/StatePlanner3D.hpp
+++ b/include/qrw/StatePlanner3D.hpp
@@ -12,10 +12,9 @@
#include "pinocchio/math/rpy.hpp"
#include "pinocchio/spatial/se3.hpp"
+#include "qrw/Heightmap.hpp"
#include "qrw/Params.hpp"
#include "qrw/Types.h"
-#include "qrw/Heightmap.hpp"
-#include <vector>
class StatePlanner3D {
public:
@@ -42,6 +41,16 @@ class StatePlanner3D {
////////////////////////////////////////////////////////////////////////////////////////////////
~StatePlanner3D() {}
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Udate the average surface using the heightmap and compute the configurations
+ ///
+ /// \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 updateSurface(VectorN const& q, Vector6 const& vref);
+
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute the reference trajectory of the CoM for each time step of the
@@ -53,10 +62,9 @@ class StatePlanner3D {
/// \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);
+ void computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -66,35 +74,35 @@ class StatePlanner3D {
/// \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);
+ void computeConfigurations(VectorN const& q, Vector6 const& vref);
MatrixN getReferenceStates() { return referenceStates_; }
- int getNSteps() { return n_steps_; }
- MatrixN get_configurations() { return configs; }
+ int getNumberStates() { return nStates_; }
+ MatrixN getConfigurations() { return configs_; }
+ VectorN getFit() { return fit_; }
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
+ int nStates_; // Number of timesteps in the prediction horizon
+ double dt_; // Time step of the contact sequence (time step of the MPC)
+ double referenceHeight_; // Reference height for the base
+ std::vector<double> maxVelocity_; // Maximum velocity of the base
- Vector3 RPY_; // Current roll, pitch and yaw angles
- Matrix3 Rz; // Rotation matrix z-axis, yaw angle
- Vector3 q_dxdy; // Temporary vector for displacement on x and y axis
+ Vector3 rpy_; // Current roll, pitch and yaw angles of the base
+ Matrix3 Rz_; // Rotation matrix z-axis, yaw angle
+ Vector3 DxDy_; // Temporary vector for displacement on x and y axis
- // 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_;
+ MatrixN referenceStates_; // Reference states (12 * (1 + N)) computed for the MPC
- VectorN dt_vector_; // Vector containing all time steps in the prediction horizon
- Heightmap heightmap_;
- Vector3 rpy_map = Vector3::Zero(3);
- Vector3 mean_surface; // Vector3 such as [a,b,c], such as ax + by -z + c = 0
+ VectorN dtVector_; // Vector containing all time steps in the prediction horizon
+ Heightmap heightmap_; // Environment heightmap
+ Vector3 rpyMap_; // Rotation of the heightmap in RPY
+ Vector3 fit_; // Vector3 such as [a,b,c], such as ax + by -z + c = 0 locally fits the heightmap
- double v_max = 0.4; // rad.s-1
- double v_max_z = 0.1; // rad.s-1
- int n_surface_configs = 3;
- MatrixN configs;
+ int nSteps_; // number of steps to optimize with the MIP
+ double stepDuration_; // Duration of a step
+ MatrixN configs_; // Matrix of configurations for the MIP
+ Vector7 config_; // Temp vector to store a config
+ Vector3 rpyConfig_; // Current roll, pitch and yaw angles
};
#endif // STATEPLANNER3D_H_INCLUDED
diff --git a/include/qrw/Types.h b/include/qrw/Types.h
index cd0287471f2517f91e71a2ee9cc253ae3ebbc56c..9a208a84cbad5617ada15856d3395ed2bd587029 100644
--- a/include/qrw/Types.h
+++ b/include/qrw/Types.h
@@ -23,6 +23,7 @@ using Vector18 = Eigen::Matrix<double, 18, 1>;
using Vector19 = Eigen::Matrix<double, 19, 1>;
using Vector24 = Eigen::Matrix<double, 24, 1>;
using VectorN = Eigen::Matrix<double, Eigen::Dynamic, 1>;
+using VectorNint = Eigen::Matrix<int, Eigen::Dynamic, 1>;
using Matrix2 = Eigen::Matrix<double, 2, 2>;
using Matrix3 = Eigen::Matrix<double, 3, 3>;
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 1e18f6ec2e4edfcdea102e2ca5e40c317b45026f..57bab696bce0c2a6845e3a5f037d1144f4957384 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -37,7 +37,8 @@ struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC>>
.def("get_latest_result", &MPC::get_latest_result,
"Get latest result (predicted trajectory forces to apply).\n")
.def("get_gait", &MPC::get_gait, "Get gait matrix.\n")
- .def("get_Sgait", &MPC::get_Sgait, "Get S_gait matrix.\n");
+ .def("get_Sgait", &MPC::get_Sgait, "Get S_gait matrix.\n")
+ .def("retrieve_cost", &MPC::retrieve_cost, "retrieve the cost.\n");
}
static void expose()
@@ -90,7 +91,7 @@ struct StatePlannerPythonVisitor : public bp::def_visitor<StatePlannerPythonVisi
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
.def("getReferenceStates", &StatePlanner::getReferenceStates, "Get xref matrix.\n")
- .def("getNSteps", &StatePlanner::getNSteps, "Get number of steps in prediction horizon.\n")
+ .def("getNumberStates", &StatePlanner::getNumberStates, "Get number of steps in prediction horizon.\n")
.def("initialize", &StatePlanner::initialize, bp::args("params"),
"Initialize StatePlanner from Python.\n")
@@ -168,7 +169,6 @@ struct FootstepPlannerPythonVisitor : public bp::def_visitor<FootstepPlannerPyth
// Compute target location of footsteps from Python
.def("updateFootsteps", &FootstepPlanner::updateFootsteps, bp::args("refresh", "k", "q", "b_v", "b_vref"),
"Update and compute location of footsteps from Python.\n");
-
}
static void expose()
@@ -431,8 +431,7 @@ struct JoystickPythonVisitor : public bp::def_visitor<JoystickPythonVisitor<Joys
.def("getTriangle", &Joystick::getTriangle, "Get Joystick Triangle status")
.def("getSquare", &Joystick::getSquare, "Get Joystick Square status")
.def("getL1", &Joystick::getL1, "Get Joystick L1 status")
- .def("getR1", &Joystick::getR1, "Get Joystick R1 status")
- .def("handle_v_switch", &Joystick::handle_v_switch_py, bp::args("k", "k_switch", "v_switch"), "Run security check.\n");
+ .def("getR1", &Joystick::getR1, "Get Joystick R1 status");
}
static void expose()
@@ -483,6 +482,9 @@ struct ParamsPythonVisitor : public bp::def_visitor<ParamsPythonVisitor<Params>>
.def_readwrite("osqp_w_states", &Params::osqp_w_states)
.def_readwrite("osqp_w_forces", &Params::osqp_w_forces)
.def_readonly("gait", &Params::get_gait)
+ .def_readonly("t_switch", &Params::get_t_switch)
+ .def_readonly("v_switch", &Params::get_v_switch)
+ .def("set_v_switch", &Params::set_v_switch, bp::args("v_switch"), "Set v_switch matrix from Python.\n")
.def_readwrite("enable_pyb_GUI", &Params::enable_pyb_GUI)
.def_readwrite("enable_corba_viewer", &Params::enable_corba_viewer)
.def_readwrite("enable_multiprocessing", &Params::enable_multiprocessing)
@@ -501,7 +503,14 @@ struct ParamsPythonVisitor : public bp::def_visitor<ParamsPythonVisitor<Params>>
.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);
+ .def_readwrite("environment_heightmap", &Params::environment_heightmap)
+ .def_readwrite("heightmap_fit_length", &Params::heightmap_fit_length)
+ .def_readwrite("heightmap_fit_size", &Params::heightmap_fit_size)
+ .def_readwrite("number_steps", &Params::number_steps)
+ .def_readwrite("max_velocity", &Params::max_velocity)
+ .def_readwrite("use_bezier", &Params::use_bezier)
+ .def_readwrite("use_sl1m", &Params::use_sl1m)
+ .def_readwrite("use_heuristic", &Params::max_velocity);
}
static void expose()
@@ -545,8 +554,10 @@ struct FootTrajectoryGeneratorBezierPythonVisitor
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");
+ .def("update", &FootTrajectoryGeneratorBezier::update, bp::args("k", "targetFootstep", "surfaces", "q"),
+ "Compute target location of footsteps from Python.\n")
+ .def("updateDebug", &FootTrajectoryGeneratorBezier::updateDebug, bp::args("k", "targetFootstep", "surface", "currentPosition"),
+ "Compute target location of footsteps from Python, debug version.\n");
}
static void expose() {
@@ -579,7 +590,7 @@ struct SurfacePythonVisitor : public bp::def_visitor<SurfacePythonVisitor<Surfac
.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("getHeight", &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");
}
@@ -609,7 +620,9 @@ struct FootstepPlannerQPPythonVisitor : public bp::def_visitor<FootstepPlannerQP
// 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");
+ "Update and compute location of footsteps from Python.\n")
+ .def("updateSurfaces", &FootstepPlannerQP::updateSurfaces, bp::args("potential_surfaces", "selected_surfaces", "status", "iterations"),
+ "Update the surfaces from surface planner.\n");
}
static void expose() {
@@ -633,14 +646,17 @@ struct StatePlanner3DPythonVisitor : public bp::def_visitor<StatePlanner3DPython
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("getNumberStates", &StatePlanner3D::getNumberStates, "Get number of steps in prediction horizon.\n")
+ .def("getConfigurations", &StatePlanner3D::getConfigurations, "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");
+ bp::args("q", "v", "b_vref"), "Run StatePlanner from Python.\n")
+ .def("getFit", &StatePlanner3D::getFit, "Get the fitted surface.\n")
+ .def("updateSurface", &StatePlanner3D::updateSurface,
+ bp::args("q", "b_vref"), "Update the average surface from heightmap and positions.\n");
}
static void expose() {
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 64a2f07c8b58153edca0ae06f825a9705640cdcb..4edab188e558f7c4eceef5e96cc58a955513fae8 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -1,22 +1,15 @@
-# 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
import MPC_Wrapper
-import Joystick
import pybullet as pyb
import pinocchio as pin
-from solopython.utils.viewerClient import viewerClient, NonBlockingViewerFromRobot
import libquadruped_reactive_walking as lqrw
-from example_robot_data.robots_loader import Solo12Loader
from solo3D.tools.utils import quaternionToRPY
+
class Result:
"""Object to store the result of the control loop
It contains what is sent to the robot (gains, desired positions and velocities,
@@ -72,7 +65,7 @@ class dummyDevice:
self.imu = dummyIMU()
self.joints = dummyJoints()
self.dummyPos = np.zeros(3)
- self.dummyPos[2] = 0.24
+ self.dummyPos[2] = 0.1944
self.b_baseVel = np.zeros(3)
@@ -106,7 +99,6 @@ class Controller:
self.solo = utils_mpc.init_robot(q_init, params)
# Create Joystick object
- # self.joystick = Joystick.Joystick(params)
self.joystick = lqrw.Joystick()
self.joystick.initialize(params)
@@ -116,8 +108,9 @@ class Controller:
self.h_ref = params.h_ref
self.h_ref_mem = params.h_ref
self.q = np.zeros((18, 1)) # Orientation part is in roll pitch yaw
- self.q[0:6, 0] = np.array([0.0, 0.0, self.h_ref, 0.0, 0.0, 0.0])
+ self.q[:6, 0] = np.array([0.0, 0.0, self.h_ref, 0.0, 0.0, 0.0])
self.q[6:, 0] = q_init
+ self.q_init = q_init.copy()
self.v = np.zeros((18, 1))
self.b_v = np.zeros((18, 1))
self.o_v_filt = np.zeros((18, 1))
@@ -142,14 +135,22 @@ class Controller:
self.DEMONSTRATION = params.DEMONSTRATION
self.solo3D = params.solo3D
+ self.SIMULATION = params.SIMULATION
+
+ self.last_q_perfect = np.zeros(6)
+ self.last_b_vel = np.zeros(3)
+ self.n_nan = 0
+
if params.solo3D:
- from solo3D.SurfacePlannerWrapper import SurfacePlanner_Wrapper
- from solo3D.tools.pyb_environment_3D import PybEnvironment3D
+ from solo3D.SurfacePlannerWrapper import Surface_planner_wrapper
+ if self.SIMULATION:
+ from solo3D.tools.pyb_environment_3D import PybEnvironment3D
self.enable_multiprocessing_mip = params.enable_multiprocessing_mip
self.offset_perfect_estimator = 0.
+ self.update_mip = False
if self.solo3D:
- self.surfacePlanner = SurfacePlanner_Wrapper(params) # MIP Wrapper
+ self.surfacePlanner = Surface_planner_wrapper(params)
self.statePlanner = lqrw.StatePlanner3D()
self.statePlanner.initialize(params)
@@ -157,13 +158,11 @@ class Controller:
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
+ x_margin_max_ = 0.06 # margin inside convex surfaces [m].
+ t_margin_ = 0.3 # 100*t_margin_% of the curve around critical point. range: [0, 1]
+ z_margin_ = 0.06 # 100*z_margin_% of the curve after the critical point. range: [0, 1]
+
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
@@ -172,19 +171,10 @@ class Controller:
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,
+ if self.SIMULATION:
+ 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)
@@ -233,10 +223,8 @@ class Controller:
self.feet_p_cmd = np.zeros((3, 4))
self.error = False # True if something wrong happens in the controller
- self.error_flag = 0
- self.q_security = np.array([np.pi*0.4, np.pi*80/180, np.pi] * 4)
- self.q_filt_mpc = np.zeros((18, 1))
+ self.q_filter = np.zeros((18, 1))
self.h_v_filt_mpc = np.zeros((6, 1))
self.vref_filt_mpc = np.zeros((6, 1))
self.filter_mpc_q = lqrw.Filter()
@@ -265,177 +253,135 @@ class Controller:
Args:
device (object): Interface with the masterboard or the simulation
"""
-
t_start = time.time()
- # 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))
+ q_perfect = np.zeros(6)
+ b_baseVel_perfect = np.zeros(3)
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
+ q_perfect[:3] = device.dummyPos
+ q_perfect[3:] = device.imu.attitude_euler
+ b_baseVel_perfect = 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()
+ if self.k <= 1:
+ self.initial_pos = [0., 0., -0.046]
+ self.initial_matrix = pin.rpy.rpyToMatrix(0., 0., 0.).transpose()
+ q_perfect[:3] = self.initial_matrix @ (qc.getPosition() - self.initial_pos)
+ q_perfect[3:] = quaternionToRPY(qc.getOrientationQuat())[:, 0]
+ b_baseVel_perfect[:] = (qc.getOrientationMat9().reshape((3, 3)).transpose() @ qc.getVelocity().reshape((3, 1))).ravel()
+
+ if np.isnan(np.sum(q_perfect)):
+ print("Error: nan values in perfect position of the robot")
+ q_perfect = self.last_q_perfect
+ self.n_nan += 1
+ if not np.any(self.last_q_perfect) or self.n_nan >= 5:
+ self.error = True
+ elif np.isnan(np.sum(b_baseVel_perfect)):
+ print("Error: nan values in perfect velocity of the robot")
+ b_baseVel_perfect = self.last_b_vel
+ self.n_nan += 1
+ if not np.any(self.last_b_vel) or self.n_nan >= 5:
+ self.error = True
+ else:
+ self.last_q_perfect = q_perfect
+ self.last_b_vel = b_baseVel_perfect
+ self.n_nan = 0
- # Process state estimator
self.estimator.run_filter(self.gait.getCurrentGait(),
self.footTrajectoryGenerator.getFootPosition(),
- device.imu.linear_acceleration.reshape((-1, 1)),
- device.imu.gyroscope.reshape((-1, 1)),
- device.imu.attitude_euler.reshape((-1, 1)),
- device.joints.positions.reshape((-1, 1)),
- device.joints.velocities.reshape((-1, 1)),
- dummy_state,
- b_baseVel)
+ device.imu.linear_acceleration,
+ device.imu.gyroscope,
+ device.imu.attitude_euler,
+ device.joints.positions,
+ device.joints.velocities,
+ q_perfect, b_baseVel_perfect)
# Update state vectors of the robot (q and v) + transformation matrices between world and horizontal frames
self.estimator.updateState(self.joystick.getVRef(), self.gait)
- oRb = self.estimator.getoRb()
oRh = self.estimator.getoRh()
hRb = self.estimator.gethRb()
oTh = self.estimator.getoTh().reshape((3, 1))
- self.a_ref[0:6, 0] = self.estimator.getARef()
- self.v_ref[0:6, 0] = self.estimator.getVRef()
- self.h_v[0:6, 0] = self.estimator.getHV()
- self.h_v_windowed[0:6, 0] = self.estimator.getHVWindowed()
- self.q[:, 0] = self.estimator.getQUpdated()
+ self.a_ref[:6, 0] = self.estimator.getARef()
+ self.v_ref[:6, 0] = self.estimator.getVRef()
+ self.h_v[:6, 0] = self.estimator.getHV()
+ self.h_v_windowed[:6, 0] = self.estimator.getHVWindowed()
+ if self.solo3D:
+ self.q[:3, 0] = self.estimator.getQFilt()[:3]
+ self.q[6:, 0] = self.estimator.getQFilt()[7:]
+ self.q[3:6] = quaternionToRPY(self.estimator.getQFilt()[3:7])
+ else:
+ self.q[:, 0] = self.estimator.getQUpdated()
self.v[:, 0] = self.estimator.getVUpdated()
self.yaw_estim = self.estimator.getYawEstim()
- # 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
+ # Quantities go through a 1st order low pass filter with fc = 15 Hz (avoid >25Hz foldback)
+ self.q_filter[:6, 0] = self.filter_mpc_q.filter(self.q[:6, 0], True)
+ self.q_filter[6:, 0] = self.q[6:, 0].copy()
+ self.h_v_filt_mpc[:, 0] = self.filter_mpc_v.filter(self.h_v[:6, 0], False)
+ self.vref_filt_mpc[:, 0] = self.filter_mpc_vref.filter(self.v_ref[:6, 0], False)
+
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])
+ oTh_3d = np.zeros((3, 1))
+ oTh_3d[:2, 0] = self.q_filter[:2, 0]
+ oRh_3d = pin.rpy.rpyToMatrix(0., 0., self.q_filter[5, 0])
t_filter = time.time()
+ self.t_filter = t_filter - t_start
- """if (self.k % self.k_mpc) == 0 and self.k > 1000:
- print(self.v_ref[[0, 1, 5], 0])
- if not self.treshold_static and np.all(self.v_gp[[0, 1, 5], 0] < 0.01):
- print("SWITCH TO STATIC")
- self.treshold_static = True
- elif self.treshold_static and np.any(self.v_gp[[0, 1, 5], 0] > 0.03):
- print("SWITCH TO TROT")
- self.treshold_static = False
-
- if (self.gait.getIsStatic() and not self.treshold_static):
- print("CODE 3")
- self.joystick.joystick_code = 3
- elif (not self.gait.getIsStatic() and self.treshold_static):
- print("CODE 1")
- self.joystick.joystick_code = 1"""
-
- """if self.k == 0:
- self.joystick.joystick_code = 4"""
-
- # Update gait
self.gait.updateGait(self.k, self.k_mpc, self.joystick.getJoystickCode())
- # Quantities go through a 1st order low pass filter with fc = 15 Hz (avoid >25Hz foldback)
- self.q_filt_mpc[:6, 0] = self.filter_mpc_q.filter(self.q[:6, 0:1], True)
- self.q_filt_mpc[6:, 0] = self.q[6:, 0].copy()
- 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)
-
- is_new_step = self.k % self.k_mpc == 0 and self.gait.isNewPhase()
+ self.update_mip = 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])
-
- # Result can be retrieved with self.statePlanner.getReferenceStates()
+ if self.update_mip:
+ self.statePlanner.updateSurface(self.q_filter[:6, :1], self.vref_filt_mpc[:6, :1])
+ if self.surfacePlanner.initialized:
+ self.error = self.surfacePlanner.get_latest_results()
+
+ self.footstepPlanner.updateSurfaces(self.surfacePlanner.potential_surfaces, self.surfacePlanner.selected_surfaces,
+ self.surfacePlanner.mip_success, self.surfacePlanner.mip_iteration)
+
+ self.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_filter[:, 0], self.h_v_windowed[:6, :1].copy(),
+ self.v_ref[:6, :1])
+
+ self.statePlanner.computeReferenceStates(self.q_filter[:6, :1], self.h_v_filt_mpc[:6, :1].copy(), self.vref_filt_mpc[:6, :1])
+
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())
+ if self.update_mip and self.solo3D:
+ configs = self.statePlanner.getConfigurations().transpose()
+ self.surfacePlanner.run(configs, cgait, self.o_targetFootstep, self.vref_filt_mpc[:3, 0].copy())
+ self.surfacePlanner.initialized = True
+ if not self.enable_multiprocessing_mip and self.SIMULATION:
+ self.pybEnvironment3D.update_target_SL1M(self.surfacePlanner.all_feet_pos_syn)
t_planner = time.time()
+ self.t_planner = t_planner - t_filter
- """if self.k % 250 == 0:
- print("iteration : " , self.k) # print iteration"""
-
- # TODO: Add 25Hz filter for the inputs of the MPC
-
- # Solve MPC problem once every k_mpc iterations of the main loop
+ # Solve MPC
if (self.k % self.k_mpc) == 0:
try:
if self.type_MPC == 3:
# Compute the target foostep in local frame, to stop the optimisation around it when t_lock overpass
l_targetFootstep = oRh.transpose() @ (self.o_targetFootstep - oTh)
self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, l_targetFootstep, oRh, oTh,
- self.footTrajectoryGenerator.getFootPosition(),
- self.footTrajectoryGenerator.getFootVelocity(),
- self.footTrajectoryGenerator.getFootAcceleration(),
- self.footTrajectoryGenerator.getFootJerk(),
- self.footTrajectoryGenerator.getTswing() - self.footTrajectoryGenerator.getT0s())
- else :
- self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3,4)))
-
+ self.footTrajectoryGenerator.getFootPosition(),
+ self.footTrajectoryGenerator.getFootVelocity(),
+ self.footTrajectoryGenerator.getFootAcceleration(),
+ self.footTrajectoryGenerator.getFootJerk(),
+ self.footTrajectoryGenerator.getTswing() - self.footTrajectoryGenerator.getT0s())
+ else:
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3, 4)))
except ValueError:
print("MPC Problem")
-
- """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()
-
- """if self.k >= 8220 and (self.k % self.k_mpc == 0):
- print(self.k)
- print(self.x_f_mpc[:, 0])
- from matplotlib import pyplot as plt
- plt.figure()
- plt.plot(self.x_f_mpc[6, :])
- plt.show(block=True)"""
-
- # Store o_targetFootstep, used with MPC_planner
- self.o_targetFootstep = o_targetFootstep.copy()
+ self.x_f_mpc, self.mpc_cost = self.mpc_wrapper.get_latest_result()
t_mpc = time.time()
+ self.t_mpc = t_mpc - t_planner
# If the MPC optimizes footsteps positions then we use them
if self.k > 100 and self.type_MPC == 3:
@@ -447,15 +393,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
- if self.solo3D: # Bezier curves
- self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep, self.surfacePlanner.selected_surfaces,
- self.q_filt_3d)
+ if self.solo3D:
+ self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep, self.surfacePlanner.selected_surfaces, self.q_filter)
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()):
+ if not self.error and not self.joystick.getStop():
if self.DEMONSTRATION and self.gait.getIsStatic():
hRb = np.eye(3)
@@ -463,29 +406,20 @@ class Controller:
self.xgoals[:6, 0] = np.zeros((6,))
if self.DEMONSTRATION and self.joystick.getL1() and self.gait.getIsStatic():
self.p_ref[:, 0] = self.joystick.getPRef()
- # self.p_ref[3, 0] = np.clip((self.k - 2000) / 2000, 0.0, 1.0)
self.xgoals[[3, 4], 0] = self.p_ref[[3, 4], 0]
self.h_ref = self.p_ref[2, 0]
hRb = pin.rpy.rpyToMatrix(0.0, 0.0, self.p_ref[5, 0])
- # print(self.joystick.getPRef())
- # print(self.p_ref[2])
else:
+ self.xgoals[[3, 4], 0] = xref[[3, 4], 1]
self.h_ref = self.h_ref_mem
# If the four feet are in contact then we do not listen to MPC (default contact forces instead)
if self.DEMONSTRATION and self.gait.getIsStatic():
self.x_f_mpc[12:24, 0] = [0.0, 0.0, 9.81 * 2.5 / 4.0] * 4
- # Update configuration vector for wbc
- 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 configuration vector for wbc with filtered roll and pitch and reference angular positions of previous loop
+ self.q_wbc[3:5, 0] = self.q_filter[3:5, 0]
+ self.q_wbc[6:, 0] = self.wbcWrapper.qdes[:]
# Update velocity vector for wbc
self.dq_wbc[:6, 0] = self.estimator.getVFilt()[:6] # Â Velocities in base frame (not horizontal frame!)
@@ -498,7 +432,7 @@ class Controller:
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]]))
+ oRh_3d.transpose(), oTh_3d + np.array([[0.0], [0.0], [xref[2, 1]]]))
else: # Use ideal base frame
self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(
hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
@@ -507,21 +441,8 @@ class Controller:
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,))
- if not self.gait.getIsStatic():
- self.xgoals[3:5, 0] = [0.0, 0.0] # Â Height (in horizontal frame!)
- else:
- self.xgoals[3:5, 0] += self.vref_filt_mpc[3:5, 0] * self.dt_wbc
- self.h_ref += self.vref_filt_mpc[2, 0] * self.dt_wbc
- self.h_ref = np.clip(self.h_ref, 0.19, 0.26)
- self.xgoals[3:5, 0] = np.clip(self.xgoals[3:5, 0], [-0.25, -0.17], [0.25, 0.17])"""
-
-
self.xgoals[6:, 0] = self.vref_filt_mpc[:, 0] # Velocities (in horizontal frame!)
- #print(" ###### ")
-
# Run InvKin + WBC QP
self.wbcWrapper.compute(self.q_wbc, self.dq_wbc,
(self.x_f_mpc[12:24, 0:1]).copy(), np.array([cgait[0, :]]),
@@ -529,107 +450,65 @@ class Controller:
self.feet_v_cmd,
self.feet_a_cmd,
self.xgoals)
-
# Quantities sent to the control board
self.result.P = np.array(self.Kp_main.tolist() * 4)
self.result.D = np.array(self.Kd_main.tolist() * 4)
+ self.result.FF = self.Kff_main * np.ones(12)
self.result.q_des[:] = self.wbcWrapper.qdes[:]
self.result.v_des[:] = self.wbcWrapper.vdes[:]
- self.result.FF = self.Kff_main * np.ones(12)
self.result.tau_ff[:] = self.wbcWrapper.tau_ff
+ self.clamp_result(device)
+
self.nle[:3, 0] = self.wbcWrapper.nle[:3]
# Display robot in Gepetto corba viewer
if self.enable_corba_viewer and (self.k % 5 == 0):
- self.q_display[:3, 0] = self.q_wbc[0:3, 0]
+ self.q_display[:3, 0] = self.q_wbc[:3, 0]
self.q_display[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.q_wbc[3:6, 0])).coeffs()
self.q_display[7:, 0] = self.q_wbc[6:, 0]
self.solo.display(self.q_display)
- """if self.k > 0:
-
- oTh_pyb = device.dummyPos.ravel().tolist()
- oTh_pyb[2] = 0.30
- q_oRb_pyb = pin.Quaternion(pin.rpy.rpyToMatrix(self.k/(57.3 * 500), 0.0,
- device.imu.attitude_euler[2])).coeffs().tolist()
- pyb.resetBasePositionAndOrientation(device.pyb_sim.robotId, oTh_pyb, q_oRb_pyb)"""
-
- """if self.k >= 8220 and (self.k % self.k_mpc == 0):
- print(self.k)
- print("x_f_mpc: ", self.x_f_mpc[:, 0])
- print("ddq delta: ", self.wbcWrapper.ddq_with_delta)
- print("f delta: ", self.wbcWrapper.f_with_delta)
- from matplotlib import pyplot as plt
- plt.figure()
- plt.plot(self.x_f_mpc[6, :])
- plt.show(block=True)
-
- print("f delta: ", self.wbcWrapper.f_with_delta)"""
-
- """if self.k == 1:
- quit()"""
-
- """np.set_printoptions(precision=3, linewidth=300)
- print("---- ", self.k)
- print(self.x_f_mpc[12:24, 0])
- print(self.result.q_des[:])
- print(self.result.v_des[:])
- print(self.result.tau_ff[:])
- print(self.xgoals.ravel())"""
-
- """np.set_printoptions(precision=3, linewidth=300)
- print("#####")
- print(cgait)
- print(self.result.tau_ff[:])"""
-
- t_wbc = time.time()
-
- # Security check
+ self.t_wbc = time.time() - t_mpc
+
self.security_check()
+ if self.error or self.joystick.getStop():
+ self.set_null_control()
# Update PyBullet camera
- # to have yaw update in simu: utils_mpc.quaternionToRPY(self.estimator.q_filt[3:7, 0])[2, 0]
if not self.solo3D:
self.pyb_camera(device, 0.0)
else: # Update 3D Environment
- self.pybEnvironment3D.update(self.k)
+ if self.SIMULATION:
+ self.pybEnvironment3D.update(self.k)
- # Update debug display (spheres, ...)
self.pyb_debug(device, fsteps, cgait, xref)
- # Logs
- self.log_misc(t_start, t_filter, t_planner, t_mpc, t_wbc)
-
- # Increment loop counter
+ self.t_loop = time.time() - t_start
self.k += 1
- return 0.0
+ return self.error
def pyb_camera(self, device, yaw):
-
- # Update position of PyBullet camera on the robot position to do as if it was attached to the robot
+ """
+ Update position of PyBullet camera on the robot position to do as if it was attached to the robot
+ """
if self.k > 10 and self.enable_pyb_GUI:
- # pyb.resetDebugVisualizerCamera(cameraDistance=0.8, cameraYaw=45, cameraPitch=-30,
- # cameraTargetPosition=[1.0, 0.3, 0.25])
pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=45, cameraPitch=-39.9,
cameraTargetPosition=[device.dummyHeight[0], device.dummyHeight[1], 0.0])
def pyb_debug(self, device, fsteps, cgait, xref):
if self.k > 1 and self.enable_pyb_GUI:
-
# Display desired feet positions in WBC as green spheres
oTh_pyb = device.dummyPos.reshape((-1, 1))
- # print("h: ", oTh_pyb[2, 0], " ", self.h_ref)
- 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):
if not self.solo3D:
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])
else:
- pos = self.o_targetFootstep[:,i]
+ 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
@@ -643,13 +522,11 @@ class Controller:
if cpt < cgait.shape[0]:
status = cgait[cpt, i]
if status:
- pos = oRh_pyb @ fsteps[cpt, (3*i):(3*(i+1))].reshape(
- (-1, 1)) + oTh_pyb - np.array([[0.0], [0.0], [self.h_ref]])
+ pos = oRh_pyb @ fsteps[cpt, (3*i):(3*(i+1))].reshape((-1, 1)) + oTh_pyb - np.array([[0.0], [0.0], [oTh_pyb[2, 0]]])
pyb.resetBasePositionAndOrientation(
device.pyb_sim.ftps_Ids[i, j], pos[:, 0].tolist(), [0, 0, 0, 1])
else:
- pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids[i, j], [
- 0.0, 0.0, -0.1], [0, 0, 0, 1])
+ pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids[i, j], [0.0, 0.0, -0.1], [0, 0, 0, 1])
j += 1
# Hide unused spheres underground
@@ -657,9 +534,6 @@ class Controller:
pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids[i, k], [0.0, 0.0, -0.1], [0, 0, 0, 1])
# Display reference trajectory
- """from IPython import embed
- embed()"""
-
xref_rot = np.zeros((3, xref.shape[1]))
for i in range(xref.shape[1]):
xref_rot[:, i:(i+1)] = oRh_pyb @ xref[:3, i:(i+1)] + oTh_pyb + np.array([[0.0], [0.0], [0.05 - self.h_ref]])
@@ -686,37 +560,74 @@ class Controller:
else:
for i in range(self.x_f_mpc.shape[1]-1):
device.pyb_sim.lineId_blue[i] = pyb.addUserDebugLine(x_f_mpc_rot[:3, i].tolist(), x_f_mpc_rot[:3, i+1].tolist(),
- lineColorRGB=[0.0, 0.0, 1.0], lineWidth=8,
- replaceItemUniqueId=device.pyb_sim.lineId_blue[i])
+ lineColorRGB=[0.0, 0.0, 1.0], lineWidth=8,
+ replaceItemUniqueId=device.pyb_sim.lineId_blue[i])
def security_check(self):
-
- if (self.error_flag == 0) and (not self.error) and (not self.joystick.getStop()):
- self.error_flag = self.estimator.security_check(self.wbcWrapper.tau_ff)
- if (self.error_flag != 0):
+ """
+ Check if the command is fine and set the command to zero in case of error
+ """
+ if not (self.error or self.joystick.getStop()):
+ error_flag = self.estimator.security_check(self.wbcWrapper.tau_ff)
+ if (error_flag != 0):
self.error = True
- if (self.error_flag == 1):
- self.error_value = self.estimator.getQFilt()[7:] * 180 / 3.1415
- elif (self.error_flag == 2):
- self.error_value = self.estimator.getVSecu()
+ if (error_flag == 1):
+ print("-- POSITION LIMIT ERROR --")
+ print(self.estimator.getQFilt()[7:])
+ elif (error_flag == 2):
+ print("-- VELOCITY TOO HIGH ERROR --")
+ print(self.estimator.getVSecu())
else:
- self.error_value = self.wbcWrapper.tau_ff
-
- # If something wrong happened in the controller we stick to a security controller
- if self.error or self.joystick.getStop():
-
- # Quantities sent to the control board
- self.result.P = np.zeros(12)
- self.result.D = 0.1 * np.ones(12)
- self.result.q_des[:] = np.zeros(12)
- self.result.v_des[:] = np.zeros(12)
- self.result.FF = np.zeros(12)
- self.result.tau_ff[:] = np.zeros(12)
-
- def log_misc(self, tic, t_filter, t_planner, t_mpc, t_wbc):
-
- self.t_filter = t_filter - tic
- self.t_planner = t_planner - t_filter
- self.t_mpc = t_mpc - t_planner
- self.t_wbc = t_wbc - t_mpc
- self.t_loop = time.time() - tic
+ print("-- FEEDFORWARD TORQUES TOO HIGH ERROR --")
+ print(self.wbcWrapper.tau_ff)
+
+ def clamp(self, num, min_value=None, max_value=None):
+ clamped = False
+ if min_value is not None and num <= min_value:
+ num = min_value
+ clamped = True
+ if max_value is not None and num >= max_value:
+ num = max_value
+ clamped = True
+ return clamped
+
+ def clamp_result(self, device, set_error=False):
+ """
+ Clamp the result
+ """
+ hip_max = 120. * np.pi / 180.
+ knee_min = 5. * np.pi / 180.
+ for i in range(4):
+ if self.clamp(self.result.q_des[3 * i + 1], -hip_max, hip_max):
+ print("Clamping hip n " + str(i))
+ self.error = set_error
+ if self.q_init[3 * i + 2] >= 0. and self.clamp(self.result.q_des[3 * i + 2], knee_min):
+ print("Clamping knee n " + str(i))
+ self.error = set_error
+ elif self.q_init[3 * i + 2] <= 0. and self.clamp(self.result.q_des[3 * i + 2], max_value=-knee_min):
+ print("Clamping knee n " + str(i))
+ self.error = set_error
+
+ for i in range(12):
+ if self.clamp(self.result.q_des[i], device.joints.positions[i] - 4., device.joints.positions[i] + 4.):
+ print("Clamping position difference of motor n " + str(i))
+ self.error = set_error
+
+ if self.clamp(self.result.v_des[i], device.joints.velocities[i] - 100., device.joints.velocities[i] + 100.):
+ print("Clamping velocity of motor n " + str(i))
+ self.error = set_error
+
+ if self.clamp(self.result.tau_ff[i], -8., 8.):
+ print("Clamping torque of motor n " + str(i))
+ self.error = set_error
+
+ def set_null_control(self):
+ """
+ Send default null values to the robot
+ """
+ self.result.P = np.zeros(12)
+ self.result.D = 0.1 * np.ones(12)
+ self.result.q_des[:] = np.zeros(12)
+ self.result.v_des[:] = np.zeros(12)
+ self.result.FF = np.zeros(12)
+ self.result.tau_ff[:] = np.zeros(12)
diff --git a/scripts/Joystick.py b/scripts/Joystick.py
deleted file mode 100644
index d5afb095a5907b866d1a7825b747b1216f89370b..0000000000000000000000000000000000000000
--- a/scripts/Joystick.py
+++ /dev/null
@@ -1,368 +0,0 @@
-# coding: utf8
-
-import numpy as np
-import gamepadClient as gC
-import libquadruped_reactive_walking as lqrw
-
-class Joystick:
- """Joystick-like controller that outputs the reference velocity in local frame
-
- Args:
- predefined (bool): use either a predefined velocity profile (True) or a gamepad (False)
- """
-
- def __init__(self, params, multi_simu=False):
-
- # Controller parameters
- self.dt_wbc = params.dt_wbc
- self.dt_mpc = params.dt_mpc
-
- # Reference velocity in local frame
- self.v_ref = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
- self.v_gp = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
-
- self.reduced = False
- self.stop = False
-
- self.alpha = 0.003 #Â Coefficient to low pass the joystick velocity
-
- # Bool to modify the update of v_ref
- # Used to launch multiple simulations
- self.multi_simu = multi_simu
-
- # If we are using a predefined reference velocity (True) or a joystick (False)
- self.predefined = params.predefined_vel
-
- # If we are performing an analysis from outside
- self.analysis = False
-
- # Joystick variables (linear and angular velocity and their scaling for the joystick)
- self.vX = 0.
- self.vY = 0.
- self.vYaw = 0.
- self.vZ = 0.
- self.VxScale = 0.3
- self.VyScale = 0.6
- self.vYawScale = 0.6
- self.vZScale = 0.3
-
- self.Vx_ref = 0.0
- self.Vy_ref = 0.0
- self.Vw_ref = 0.0
-
- # Y, B, A and X buttons (in that order)
- self.northButton = False
- self.eastButton = False
- self.southButton = False
- self.westButton = False
- self.joystick_code = 0 # Code to carry information about pressed buttons
-
- self.joyCpp = lqrw.Joystick()
-
- def update_v_ref(self, k_loop, velID, is_static=False):
- """Update the reference velocity of the robot along X, Y and Yaw in local frame by
- listening to a gamepad handled by an independent thread
-
- Args:
- k_loop (int): numero of the current iteration
- velID (int): Identifier of the current velocity profile to be able to handle different scenarios
- """
-
- if self.predefined:
- if self.multi_simu:
- self.update_v_ref_multi_simu(k_loop)
- elif self.analysis:
- self.handle_v_switch(k_loop)
- else:
- self.update_v_ref_predefined(k_loop, velID)
- else:
- self.update_v_ref_gamepad(k_loop, is_static)
-
- return 0
-
- def update_v_ref_gamepad(self, k_loop, is_static):
- """Update the reference velocity of the robot along X, Y and Yaw in local frame by
- listening to a gamepad handled by an independent thread
-
- Args:
- k_loop (int): numero of the current iteration
- """
-
- # Create the gamepad client
- if k_loop == 0:
- self.gp = gC.GamepadClient()
- """self.gp.leftJoystickX.value = 0.00390625
- self.gp.leftJoystickY.value = 0.00390625
- self.gp.rightJoystickX.value = 0.00390625
-
- # Get the velocity command based on the position of joysticks
- self.vX = (self.gp.leftJoystickX.value / 0.00778 * 2. - 1.) * self.VxScale
- self.vY = (self.gp.leftJoystickY.value / 0.00778 * 2. - 1.) * self.VyScale
- self.vYaw = (self.gp.rightJoystickX.value / 0.00778 * 2. - 1.) * self.vYawScale"""
- self.vX = self.gp.leftJoystickX.value * self.VxScale
- self.vY = self.gp.leftJoystickY.value * self.VyScale
- self.vYaw = self.gp.rightJoystickX.value * self.vYawScale
- self.vZ = self.gp.rightJoystickY.value * self.vZScale
-
- if is_static and self.gp.L1Button.value: # If static the orientation of the base is controlled
- self.v_gp = np.array(
- [[0.0, 0.0, - self.vZ * 0.5, - self.vX * 5, - self.vY * 2, - self.vYaw]]).T
- # print(self.v_gp.ravel())
- else: # Otherwise the Vx, Vy, Vyaw is controlled
- self.v_gp = np.array(
- [[- self.vY, - self.vX, 0.0, 0.0, 0.0, - self.vYaw]]).T
-
- # Reduce the size of the support polygon by pressing Start
- if self.gp.startButton.value:
- self.reduced = not self.reduced
-
- # Switch to safety controller if the Back key is pressed
- if self.gp.backButton.value:
- self.stop = True
-
- # Switch gaits
- if self.gp.northButton.value:
- self.northButton = True
- self.eastButton = False
- self.southButton = False
- self.westButton = False
- elif self.gp.eastButton.value:
- self.northButton = False
- self.eastButton = True
- self.southButton = False
- self.westButton = False
- elif self.gp.southButton.value:
- self.northButton = False
- self.eastButton = False
- self.southButton = True
- self.westButton = False
- elif self.gp.westButton.value:
- self.northButton = False
- self.eastButton = False
- self.southButton = False
- self.westButton = True
-
- # Low pass filter to slow down the changes of velocity when moving the joysticks
- self.v_gp[(self.v_gp < 0.004) & (self.v_gp > -0.004)] = 0.0
- self.v_ref = self.alpha * self.v_gp + (1-self.alpha) * self.v_ref
-
- # Update joystick code depending on which buttons are pressed
- self.computeCode()
-
- return 0
-
- def computeCode(self):
- # Check joystick buttons to trigger a change of gait type
- self.joystick_code = 0
- if self.southButton:
- self.joystick_code = 1
- self.southButton = False
- elif self.eastButton:
- self.joystick_code = 2
- self.eastButton = False
- elif self.westButton:
- self.joystick_code = 3
- self.westButton = False
- elif self.northButton:
- self.joystick_code = 4
- self.northButton = False
-
-
- def handle_v_switch(self, k):
- """Handle the change of reference velocity according to the chosen predefined velocity profile
-
- Args:
- k (int): numero of the current iteration
- """
-
- self.v_ref[:, 0] = self.joyCpp.handle_v_switch(k, self.k_switch.reshape((-1, 1)), self.v_switch)
-
- def update_v_ref_predefined(self, k_loop, velID):
- """Update the reference velocity of the robot along X, Y and Yaw in local frame
- according to a predefined sequence
-
- Args:
- k_loop (int): numero of the current iteration
- velID (int): identifier of the current velocity profile to be able to handle different scenarios
- """
-
- if (k_loop == 0):
- if velID == 0:
- self.t_switch = np.array([0, 1, 5.5, 8.5, 13, 26, 40, 60])
- self.v_switch = np.array([[0.0, 0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]])
- elif velID == 1:
- V_max = 1.0
- R_max = 0.3
- self.t_switch = np.array([0, 2, 6, 16, 24, 32, 40, 44,
- 46, 52, 60, 66, 68, 80, 82, 86,
- 88, 90])
- self.v_switch = np.zeros((6, self.t_switch.shape[0]))
- self.v_switch[0, :] = np.array([0.0, 0.0, V_max, V_max, 0.0, 0.0, 0.0,
- 0.0, -V_max, -V_max, 0.0, 0.0, 0.0, V_max, V_max, V_max,
- V_max, V_max])
- self.v_switch[1, :] = np.array([0.0, 0.0, 0.0, 0.0, -V_max*0.5, -V_max*0.5, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0])
- self.v_switch[5, :] = np.array([0.0, 0.0, R_max, R_max, R_max, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, R_max, R_max, 0.0, 0.0,
- -R_max, 0.0])
- elif velID == 2:
- self.t_switch = np.array([0, 3, 4, 5, 7, 6, 8 ,13, 14])
- self.v_switch = np.array([[0.0, 0., 0., 0.3, 0.6, 0.5 , 0.4,0.4,0. ],
- [0.0, 0.4, 0.5, 0.2, 0., 0.,0.,0.,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.,0.,0.,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.,0.,0.,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.,0.,0.,0.],
- [0.0, 0., 0.7, 0.7, 0.7 ,0.7, 0.7,0., 0. ]])
- elif velID == 3: # PLANNER > NL > LIN
- self.t_switch = np.array([0, 3, 4, 5, 10,15])
- self.v_switch = np.array([[0.0, 0., 0., 0., 0., 0. ],
- [0.0, 0.0, 0.0, 0.0, 0., 0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.8, 1.5, 1.8, 2.5 ,2.8]])
- elif velID == 4: # PLANNER > NL == LIN
- self.t_switch = np.array([0, 3, 4, 5, 10,15])
- self.v_switch = np.array([[0.0, 0.4, 0.6, 0.6, 0., 0. ],
- [0.0, 0.4, 0.6, 0.6, 0.6 ,0.6],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.5, 0.5 ,0.5]])
- elif velID == 5: # PLANNER GOOD ROLL / PITCH
- self.t_switch = np.array([0, 3, 4, 5, 10,15])
- self.v_switch = np.array([[0.0, 0.4, 0.4, 0.4, 0.4, 0. ],
- [0.0, 0.4, 0.4, 0.4, 0. ,0.4],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.7, 0.7 ,0.7]])
- elif velID == 6:
- self.t_switch = np.array([0, 3, 4, 5, 10,15])
- self.v_switch = np.array([[0.0, 0.4, 0.5, 0.6, 0.6, 0.6 ],
- [0.0, 0., 0., 0., 0. ,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
- [0.0, 0.0, 0.0, 0., 0. ,0.]])
- elif velID == 7: # Zig-zag
- self.t_switch = np.array([0, 1, 3, 4, 6, 7, 9, 10, 11])
- self.v_switch = np.array([[0.0, 0.25, 0.5, 0.5, 0.5, 0.5, 0.5, 0.25, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.8, -0.8, -0.8, 0.8, 0.8, -0.8, 0.0, 0.0]])
- elif velID == 8: # Zig-zag higher speed
- self.t_switch = np.array([0, 1, 3, 5, 7, 8, 9, 10, 11])
- self.v_switch = np.array([[0.0, 0.4, 0.8, 0.8, 0.4, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.8, -1.2, 0.8, 0.0, 0.0, 0.0, 0.0, 0.0]])
- elif velID == 9: # Debug feet tracking
- self.t_switch = np.array([0, 2, 3, 5])
- self.v_switch = np.array([[0.0, 0.4, 0.4, 0.0],
- [0.0, 0.4, 0.4, 0.0],
- [0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0]])
- elif velID == 10: # FORWAAAAAAAAAARD
- self.t_switch = np.array([0, 2, 4, 6, 8, 10, 15])
- self.v_switch = np.array([[0.0, 0.4, 0.8, 1.0, 1.0, 1.0, 1.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]])
- elif velID == 11: # Debug feet tracking
- self.t_switch = np.array([0, 2, 4, 6, 8])
- self.v_switch = np.array([[0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 1.0, 1.5, 1.5, 1.5]])
- self.k_switch = (self.t_switch / self.dt_wbc).astype(int)
- self.handle_v_switch(k_loop)
- return 0
-
- def update_v_ref_multi_simu(self, k_loop):
- """Update the reference velocity of the robot along X, Y and Yaw in local frame
- according to a predefined sequence
-
- Args:
- k_loop (int): number of MPC iterations since the start of the simulation
- velID (int): Identifier of the current velocity profile to be able to handle different scenarios
- """
-
- # Moving forwards
- """if k_loop == self.k_mpc*16*3:
- self.v_ref = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T"""
-
- beta_x = int(max(abs(self.Vx_ref)*10000, 100.0))
- alpha_x = np.max([np.min([(k_loop-self.k_mpc*16*3)/beta_x, 1.0]), 0.0])
-
- beta_y = int(max(abs(self.Vy_ref)*10000, 100.0))
- alpha_y = np.max([np.min([(k_loop-self.k_mpc*16*3)/beta_y, 1.0]), 0.0])
-
- beta_w = int(max(abs(self.Vw_ref)*2500, 100.0))
- alpha_w = np.max([np.min([(k_loop-self.k_mpc*16*3)/beta_w, 1.0]), 0.0])
-
- # self.v_ref = np.array([[0.3*alpha, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
- self.v_ref = np.array(
- [[self.Vx_ref*alpha_x, self.Vy_ref*alpha_y, 0.0, 0.0, 0.0, self.Vw_ref*alpha_w]]).T
-
- return 0
-
- def update_for_analysis(self, des_vel_analysis, N_analysis, N_steady):
-
- self.analysis = True
-
- self.k_switch = np.array([0, int(1/self.dt_wbc), N_analysis, N_analysis + N_steady])
- self.v_switch = np.zeros((6, 4))
- self.v_switch[:, 2] = des_vel_analysis
- self.v_switch[:, 3] = des_vel_analysis
-
- return 0
-
-if __name__ == "__main__":
-
- from matplotlib import pyplot as plt
- import libquadruped_reactive_walking as lqrw
- from time import clock
- params = lqrw.Params() # Object that holds all controller parameters
- params.predefined_vel = False
- joystick = Joystick(params)
- k = 0
- vx = [0.0] * 1000
- fig = plt.figure()
- ax = plt.gca()
- ax.set_ylim([-0.5, 0.5])
- h, = plt.plot(np.linspace(0.001, 1.0, 1000), vx, "b", linewidth=2)
- plt.xlabel("Time [s]")
- plt.ylabel("Forward reference velocity [m/s]")
- plt.show(block=False)
-
- print("Start")
- while True:
- # Update the reference velocity coming from the gamepad
- joystick.update_v_ref(k, 0)
- vx.pop(0)
- vx.append(joystick.v_ref[0, 0])
-
- if k % 50 == 0:
- h.set_ydata(vx)
- print("Joystick raw: ", joystick.v_gp[0, 0])
- print("Joystick filtered: ", joystick.v_ref[0, 0])
- plt.pause(0.0001)
-
- k += 1
\ No newline at end of file
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 0d9362b134898195f980e1a87f022a3da94d82a4..711eefc184b4cab7b61e6558017f76e55a57db81 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -1,34 +1,30 @@
'''This class will log 1d array in Nd matrix from device and qualisys object'''
-from multiprocessing import set_forkserver_preload
-import pickle
import numpy as np
from datetime import datetime as datetime
from time import time
import pinocchio as pin
-class LoggerControl():
- def __init__(self, dt, N0_gait, type_MPC=None, joystick=None, estimator=None, loop=None, gait=None, statePlanner=None,
- footstepPlanner=None, footTrajectoryGenerator=None, logSize=60e3, ringBuffer=False, loading=False, fileName=None):
- self.ringBuffer = ringBuffer
- logSize = np.int(logSize)
- self.logSize = logSize
- self.i = 0
-
- self.dt = dt
- if type_MPC is not None:
- self.type_MPC = int(type_MPC)
- else:
- self.type_MPC = 0
+class LoggerControl():
+ def __init__(self, params, logSize=60e3, ringBuffer=False, loading=False, fileName=None):
if loading:
if fileName is None:
import glob
fileName = np.sort(glob.glob('data_2021_*.npz'))[-1] # Most recent file
-
- self.data = np.load(fileName, allow_pickle = True)
- N0_gait = self.data["planner_gait"].shape[1]
logSize = self.data["planner_gait"].shape[0]
+ n_gait = self.data["planner_gait"].shape[1]
+ self.type_MPC = int(fileName[-5])
self.logSize = logSize
+ self.data = np.load(fileName, allow_pickle=True)
+ else:
+ n_gait = params.gait.shape[0]
+ self.type_MPC = params.type_MPC
+ self.logSize = np.int(logSize)
+
+ self.i = 0
+ self.dt = params.dt_wbc
+ self.ringBuffer = ringBuffer
+ self.solo3d = params.solo3D
# Allocate the data:
# Joystick
@@ -38,14 +34,13 @@ class LoggerControl():
self.esti_feet_status = np.zeros([logSize, 4]) # input feet status (contact or not)
self.esti_feet_goals = np.zeros([logSize, 3, 4]) # input feet goals (desired on the ground)
self.esti_q_filt = np.zeros([logSize, 19]) # estimated state of the robot (complementary filter)
- self.esti_q_up = np.zeros([logSize, 18]) #Â state of the robot in the ideal world
+ self.esti_q_up = np.zeros([logSize, 18]) # Â state of the robot in the ideal world
self.esti_v_filt = np.zeros([logSize, 18]) # estimated velocity of the robot (b frame)
- self.esti_v_filt_bis = np.zeros([logSize, 18]) #Â estimated velocity of the robot (b frame, windowed)
+ self.esti_v_filt_bis = np.zeros([logSize, 18]) # Â estimated velocity of the robot (b frame, windowed)
self.esti_v_up = np.zeros([logSize, 18]) # estimated velocity of the robot in the ideal world (h frame)
self.esti_v_ref = np.zeros([logSize, 6]) # joystick reference velocity (h frame)
self.esti_v_secu = np.zeros([logSize, 12]) # filtered actuators velocity for security checks
self.esti_a_ref = np.zeros([logSize, 6]) # joystick reference acceleration (finite difference of v_ref)
- # h_v, h_v_windowed, yaw_estim, oRb, oRh, hRb, oTh can be reconstructed based on what is logged
self.esti_FK_lin_vel = np.zeros([logSize, 3]) # estimated velocity of the base with FK
self.esti_FK_xyz = np.zeros([logSize, 3]) # estimated position of the base with FK
@@ -68,26 +63,25 @@ class LoggerControl():
self.loop_h_v = np.zeros([logSize, 18]) # estimated velocity in horizontal frame
self.loop_h_v_windowed = np.zeros([logSize, 6]) # estimated velocity in horizontal frame (windowed)
self.loop_t_filter = np.zeros([logSize]) # time taken by the estimator
- self.loop_t_planner = np.zeros([logSize]) #Â time taken by the planning
+ self.loop_t_planner = np.zeros([logSize]) # Â time taken by the planning
self.loop_t_mpc = np.zeros([logSize]) # time taken by the mcp
self.loop_t_wbc = np.zeros([logSize]) # time taken by the whole body control
self.loop_t_loop = np.zeros([logSize]) # time taken by the whole loop (without interface)
self.loop_t_loop_if = np.zeros([logSize]) # time taken by the whole loop (with interface)
- self.loop_q_filt_mpc = np.zeros([logSize, 6]) #Â state in ideal world filtered by 1st order low pass
- self.loop_h_v_filt_mpc = np.zeros([logSize, 6]) #Â vel in h frame filtered by 1st order low pass
- self.loop_vref_filt_mpc = np.zeros([logSize, 6]) #Â ref vel in h frame filtered by 1st order low pass
+ self.loop_q_filt_mpc = np.zeros([logSize, 6]) # Â state in ideal world filtered by 1st order low pass
+ self.loop_h_v_filt_mpc = np.zeros([logSize, 6]) # Â vel in h frame filtered by 1st order low pass
+ self.loop_vref_filt_mpc = np.zeros([logSize, 6]) # Â ref vel in h frame filtered by 1st order low pass
# Gait
- self.planner_gait = np.zeros([logSize, N0_gait, 4]) # Gait sequence
+ self.planner_gait = np.zeros([logSize, n_gait, 4]) # Gait sequence
self.planner_is_static = np.zeros([logSize]) # if the planner is in static mode or not
# State planner
- self.planner_xref = np.zeros([logSize, 12, 1+N0_gait]) # Reference trajectory
+ self.planner_xref = np.zeros([logSize, 12, n_gait + 1]) # Reference trajectory
# Footstep planner
- self.planner_fsteps = np.zeros([logSize, N0_gait, 12]) # Reference footsteps position
+ self.planner_fsteps = np.zeros([logSize, n_gait, 12]) # Reference footsteps position
self.planner_target_fsteps = np.zeros([logSize, 3, 4]) # For each foot, next target on the ground
- # o_target_foosteps can be reconstructed using yaw_ideal and x y ideal (top of q_up)
self.planner_h_ref = np.zeros([logSize]) # reference height of the planner
# Foot Trajectory Generator
@@ -95,22 +89,14 @@ class LoggerControl():
self.planner_vgoals = np.zeros([logSize, 3, 4]) # 3D target feet velocities
self.planner_agoals = np.zeros([logSize, 3, 4]) # 3D target feet accelerations
self.planner_jgoals = np.zeros([logSize, 3, 4]) # 3D target feet accelerations
- #Â References given to the wbc can be retrieved applying a rotation hRb @ oRh.transpose()
- # and a translation oTh + np.array([[0.0], [0.0], [self.h_ref]]) to the position
# Model Predictive Control
- # output vector of the MPC (next state + reference contact force)
- if loading:
- if int(fileName[-5]) == 3:
- self.mpc_x_f = np.zeros([logSize, 32, N0_gait])
- else:
- self.mpc_x_f = np.zeros([logSize, 24, N0_gait])
+ if self.type_MPC == 3:
+ self.mpc_x_f = np.zeros([logSize, 32, n_gait]) # Result of the MPC
else:
- if loop.type_MPC == 3:
- self.mpc_x_f = np.zeros([logSize, 32, N0_gait])
- else:
- self.mpc_x_f = np.zeros([logSize, 24, N0_gait])
- self.mpc_solving_duration = np.zeros([logSize])
+ self.mpc_x_f = np.zeros([logSize, 24, n_gait]) # Result of the MPC
+ self.mpc_solving_duration = np.zeros([logSize]) # Computation time of the MPC
+ self.mpc_cost = np.zeros([logSize, 1]) # Cost of the mpc
# Whole body control
self.wbc_P = np.zeros([logSize, 12]) # proportionnal gains of the PD+
@@ -135,8 +121,15 @@ class LoggerControl():
# Timestamps
self.tstamps = np.zeros(logSize)
- def sample(self, joystick, estimator, loop, gait, statePlanner, footstepPlanner, footTrajectoryGenerator, wbc, dT_whole):
- if (self.i >= self.logSize):
+ # Solo3d logs
+ if self.solo3d:
+ self.update_mip = np.zeros([logSize, 1]) # Boolean to know if mip computation launched
+ self.configs = np.zeros([logSize, 7, params.number_steps]) # Reference configs for surface planner
+ self.initial_contacts = np.zeros([logSize, 3, 4]) # Initial contacts
+ self.t_mip = np.zeros([logSize, 1]) # Surface planner computation time
+
+ def sample(self, joystick, estimator, controller, gait, statePlanner, footstepPlanner, footTrajectoryGenerator, wbc, dT_whole):
+ if self.i >= self.logSize:
if self.ringBuffer:
self.i = 0
else:
@@ -173,19 +166,19 @@ class LoggerControl():
self.esti_LP_filt_x[self.i] = estimator.getFilterPosFiltX()
# Logging from the main loop
- self.loop_o_q[self.i] = loop.q[:, 0]
- self.loop_o_v[self.i] = loop.v[:, 0]
- self.loop_h_v[self.i] = loop.h_v[:, 0]
- self.loop_h_v_windowed[self.i] = loop.h_v_windowed[:, 0]
- self.loop_t_filter[self.i] = loop.t_filter
- self.loop_t_planner[self.i] = loop.t_planner
- self.loop_t_mpc[self.i] = loop.t_mpc
- self.loop_t_wbc[self.i] = loop.t_wbc
- self.loop_t_loop[self.i] = loop.t_loop
+ self.loop_o_q[self.i] = controller.q[:, 0]
+ self.loop_o_v[self.i] = controller.v[:, 0]
+ self.loop_h_v[self.i] = controller.h_v[:, 0]
+ self.loop_h_v_windowed[self.i] = controller.h_v_windowed[:, 0]
+ self.loop_t_filter[self.i] = controller.t_filter
+ self.loop_t_planner[self.i] = controller.t_planner
+ self.loop_t_mpc[self.i] = controller.t_mpc
+ self.loop_t_wbc[self.i] = controller.t_wbc
+ self.loop_t_loop[self.i] = controller.t_loop
self.loop_t_loop_if[self.i] = dT_whole
- self.loop_q_filt_mpc[self.i] = loop.q_filt_mpc[:6, 0]
- self.loop_h_v_filt_mpc[self.i] = loop.h_v_filt_mpc[:, 0]
- self.loop_vref_filt_mpc[self.i] = loop.vref_filt_mpc[:, 0]
+ self.loop_q_filt_mpc[self.i] = controller.q_filter[:6, 0]
+ self.loop_h_v_filt_mpc[self.i] = controller.h_v_filt_mpc[:, 0]
+ self.loop_vref_filt_mpc[self.i] = controller.vref_filt_mpc[:, 0]
# Logging from the planner
self.planner_xref[self.i] = statePlanner.getReferenceStates()
@@ -197,19 +190,20 @@ class LoggerControl():
self.planner_agoals[self.i] = footTrajectoryGenerator.getFootAcceleration()
self.planner_jgoals[self.i] = footTrajectoryGenerator.getFootJerk()
self.planner_is_static[self.i] = gait.getIsStatic()
- self.planner_h_ref[self.i] = loop.h_ref
+ self.planner_h_ref[self.i] = controller.h_ref
# Logging from model predictive control
- self.mpc_x_f[self.i] = loop.x_f_mpc
- self.mpc_solving_duration[self.i] = loop.mpc_wrapper.t_mpc_solving_duration
+ self.mpc_x_f[self.i] = controller.x_f_mpc
+ self.mpc_solving_duration[self.i] = controller.mpc_wrapper.t_mpc_solving_duration
+ self.mpc_cost[self.i] = controller.mpc_cost
# Logging from whole body control
- self.wbc_P[self.i] = loop.result.P
- self.wbc_D[self.i] = loop.result.D
- self.wbc_q_des[self.i] = loop.result.q_des
- self.wbc_v_des[self.i] = loop.result.v_des
- self.wbc_FF[self.i] = loop.result.FF
- self.wbc_tau_ff[self.i] = loop.result.tau_ff
+ self.wbc_P[self.i] = controller.result.P
+ self.wbc_D[self.i] = controller.result.D
+ self.wbc_q_des[self.i] = controller.result.q_des
+ self.wbc_v_des[self.i] = controller.result.v_des
+ self.wbc_FF[self.i] = controller.result.FF
+ self.wbc_tau_ff[self.i] = controller.result.tau_ff
self.wbc_ddq_IK[self.i] = wbc.ddq_cmd
self.wbc_f_ctc[self.i] = wbc.f_with_delta
self.wbc_ddq_QP[self.i] = wbc.ddq_with_delta
@@ -226,6 +220,12 @@ class LoggerControl():
# Logging timestamp
self.tstamps[self.i] = time()
+ # solo3d
+ if self.solo3d:
+ self.update_mip[self.i] = controller.update_mip
+ self.configs[self.i] = statePlanner.getConfigurations()
+ self.initial_contacts[self.i] = controller.o_targetFootstep
+ self.t_mip[self.i] = controller.surfacePlanner.t_mip
self.i += 1
def processMocap(self, N, loggerSensors):
@@ -234,12 +234,12 @@ class LoggerControl():
self.mocap_h_v = np.zeros([N, 3])
self.mocap_b_w = np.zeros([N, 3])
self.mocap_RPY = np.zeros([N, 3])
-
+
for i in range(N):
self.mocap_RPY[i] = pin.rpy.matrixToRpy(pin.Quaternion(loggerSensors.mocapOrientationQuat[i]).toRotationMatrix())
# Robot world to Mocap initial translationa and rotation
- mTo = np.array([loggerSensors.mocapPosition[0, 0], loggerSensors.mocapPosition[0, 1], 0.02])
+ mTo = np.array([loggerSensors.mocapPosition[0, 0], loggerSensors.mocapPosition[0, 1], 0.02])
mRo = pin.rpy.rpyToMatrix(0.0, 0.0, self.mocap_RPY[0, 2])
for i in range(N):
@@ -251,7 +251,87 @@ class LoggerControl():
self.mocap_b_w[i] = (oRb.transpose() @ loggerSensors.mocapAngularVelocity[i].reshape((3, 1))).ravel()
self.mocap_pos[i] = (mRo.transpose() @ (loggerSensors.mocapPosition[i, :] - mTo).reshape((3, 1))).ravel()
- def plotAll(self, loggerSensors):
+ def plotTimes(self):
+ """
+ Estimated computation time for each step of the control architecture
+ """
+ from matplotlib import pyplot as plt
+ t_range = np.array([k*self.dt for k in range(self.tstamps.shape[0])])
+
+ plt.figure()
+ plt.plot(t_range, self.t_mip, '+', color="gold")
+ plt.plot(t_range, self.loop_t_filter, 'r+')
+ plt.plot(t_range, self.loop_t_planner, 'g+')
+ plt.plot(t_range, self.loop_t_mpc, 'b+')
+ plt.plot(t_range, self.loop_t_wbc, '+', color="violet")
+ plt.plot(t_range, self.loop_t_loop, 'k+')
+ plt.plot(t_range, self.loop_t_loop_if, '+', color="rebeccapurple")
+ plt.legend(["SurfacePlanner", "Estimator", "Planner", "MPC", "WBC", "Control loop", "Whole loop"])
+ plt.xlabel("Time [s]")
+ plt.ylabel("Time [s]")
+ self.custom_suptitle("Computation time of each block")
+
+ def plotSurfacePlannerTime(self):
+ """
+ Plot estimated solving time of the model prediction control
+ """
+ from matplotlib import pyplot as plt
+
+ t_range = np.array([k*self.dt for k in range(self.tstamps.shape[0])])
+
+ fig = plt.figure()
+ plt.plot(t_range[100:], self.t_mip[100:], 'k+')
+ plt.legend(["Solving duration"])
+ plt.xlabel("Time [s]")
+ plt.ylabel("Time [s]")
+ self.custom_suptitle("Surface planner solving time")
+
+ def plotMPCCost(self):
+ """
+ Plot the cost of the OSQP MPC
+ """
+ from matplotlib import pyplot as plt
+
+ t_range = np.array([k*self.dt for k in range(self.tstamps.shape[0])])
+
+ fig = plt.figure()
+ plt.plot(t_range[100:], self.mpc_cost[100:], 'k+')
+ plt.legend(["MPC cost"])
+ plt.xlabel("Time [s]")
+ plt.ylabel("Cost value")
+ self.custom_suptitle("MPC cost value")
+
+ def plotMpcTime(self):
+ """
+ Plot estimated solving time of the model prediction control
+ """
+ from matplotlib import pyplot as plt
+ t_range = np.array([k*self.dt for k in range(self.tstamps.shape[0])])
+
+ fig = plt.figure()
+ plt.plot(t_range[35:], self.mpc_solving_duration[35:], 'k+')
+ plt.legend(["Solving duration"])
+ plt.xlabel("Time [s]")
+ plt.ylabel("Time [s]")
+ self.custom_suptitle("MPC solving time")
+
+ def plotStepTime(self):
+ """"
+ Step in system time at each loop
+ """
+ from matplotlib import pyplot as plt
+
+ plt.figure()
+ plt.plot(np.diff(self.tstamps))
+ plt.legend(["System time step"])
+ plt.xlabel("Loop []")
+ plt.ylabel("Time [s]")
+ self.custom_suptitle("System time step between 2 sucessive loops")
+
+ def plotAllGraphs(self, loggerSensors):
+ """"
+ Step in system time at each loop
+ """
from matplotlib import pyplot as plt
@@ -280,7 +360,7 @@ class LoggerControl():
feet_vel = np.zeros([self.esti_q_filt.shape[0], 3, 4])
for i in range(self.esti_q_filt.shape[0]):
q[:3, 0] = self.loop_q_filt_mpc[i, :3]
- q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.loop_q_filt_mpc[i, 3:6])).coeffs()
+ q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.loop_q_filt_mpc[i, 3:6])).coeffs()
q[7:, 0] = self.loop_o_q[i, 6:]
dq[6:, 0] = self.loop_o_v[i, 6:]
pin.forwardKinematics(solo12.model, solo12.data, q, dq)
@@ -397,7 +477,7 @@ class LoggerControl():
plt.plot(t_range, self.joy_v_ref[:, i], "r", linewidth=3)
if i < 3:
plt.legend(["State", "Ground truth",
- "State (LP 15Hz)", "State (windowed)", "Ref state"], prop={'size': 8})
+ "State (LP 15Hz)", "State (windowed)", "Ref state"], prop={'size': 8})
else:
plt.legend(["State", "Ground truth", "Ref state"], prop={'size': 8})
plt.ylabel(lgd[i])
@@ -407,31 +487,6 @@ class LoggerControl():
print("RMSE: ", np.sqrt(((self.joy_v_ref[:-1000, 0] - self.mocap_h_v[:-1000, 0])**2).mean()))
- # Analysis of the footstep locations (current and future) with a slider to move along time
- # self.slider_predicted_footholds()
-
- ####
- # Analysis of the footholds locations during the whole experiment
- ####
- """f_c = ["r", "b", "forestgreen", "rebeccapurple"]
- quat = np.zeros((4, 1))
- steps = np.zeros((12, 1))
- o_step = np.zeros((3, 1))
- plt.figure()
- plt.plot(self.loop_o_q[:, 0], self.loop_o_q[:, 1], linewidth=2, color="k")
- for i in range(self.planner_fsteps.shape[0]):
- fsteps = self.planner_fsteps[i]
- RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q[0, 3:7]).toRotationMatrix())
- quat[:, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(np.array([0.0, 0.0, RPY[2]]))).coeffs()
- oRh = pin.Quaternion(quat).toRotationMatrix()
- for j in range(4):
- #if np.any(fsteps[k, (j*3):((j+1)*3)]) and not np.array_equal(steps[(j*3):((j+1)*3), 0],
- # fsteps[k, (j*3):((j+1)*3)]):
- # steps[(j*3):((j+1)*3), 0] = fsteps[k, (j*3):((j+1)*3)]
- # o_step[:, 0:1] = oRh @ steps[(j*3):((j+1)*3), 0:1] + self.loop_o_q[i:(i+1), 0:3].transpose()
- o_step[:, 0:1] = oRh @ fsteps[0:1, (j*3):((j+1)*3)].transpose() + self.loop_o_q[i:(i+1), 0:3].transpose()
- plt.plot(o_step[0, 0], o_step[1, 0], linestyle=None, linewidth=1, marker="o", color=f_c[j])"""
-
####
# FF torques & FB torques & Sent torques & Meas torques
####
@@ -645,7 +700,6 @@ class LoggerControl():
####
# Power supply profile
####
-
plt.figure()
for i in range(3):
if i == 0:
@@ -663,83 +717,13 @@ class LoggerControl():
plt.plot(t_range, loggerSensors.energy[:], linewidth=2)
plt.ylabel("Bus energy [J]")
plt.xlabel("Time [s]")
-
-
- ####
- # Estimated computation time for each step of the control architecture
- ####
- plt.figure()
- plt.plot(t_range, self.loop_t_filter, 'r+')
- plt.plot(t_range, self.loop_t_planner, 'g+')
- plt.plot(t_range, self.loop_t_mpc, 'b+')
- plt.plot(t_range, self.loop_t_wbc, '+', color="violet")
- plt.plot(t_range, self.loop_t_loop, 'k+')
- plt.plot(t_range, self.loop_t_loop_if, '+', color="rebeccapurple")
- plt.legend(["Estimator", "Planner", "MPC", "WBC", "Control loop", "Whole loop"])
- plt.xlabel("Time [s]")
- plt.ylabel("Time [s]")
- self.custom_suptitle("Computation time of each block")
-
- # Plot estimated solving time of the model prediction control
- fig = plt.figure()
- plt.plot(t_range[35:], self.mpc_solving_duration[35:], 'k+')
- plt.legend(["Solving duration"])
- plt.xlabel("Time [s]")
- plt.ylabel("Time [s]")
- self.custom_suptitle("MPC solving time")
- ####
- # Step in system time at each loop
- ####
- plt.figure()
- plt.plot(np.diff(self.tstamps))
- plt.legend(["System time step"])
- plt.xlabel("Loop []")
- plt.ylabel("Time [s]")
- self.custom_suptitle("System time step between 2 sucessive loops")
+ self.plotTimes()
+ self.plotMpcTime()
+ self.plotSurfacePlannerTime()
+ self.plotStepTime()
+ self.plotMPCCost()
- ####
- #Â Comparison of raw and filtered quantities (15 Hz and windowed)
- ####
- """
- lgd = ["Position X", "Position Y", "Position Z", "Position Roll", "Position Pitch", "Position Yaw"]
- plt.figure()
- for i in range(6):
- if i == 0:
- ax0 = plt.subplot(3, 2, index6[i])
- else:
- plt.subplot(3, 2, index6[i], sharex=ax0)
-
- plt.plot(t_range, self.loop_o_q[:, i], linewidth=3)
- plt.plot(t_range, self.loop_q_filt_mpc[:, i], linewidth=3)
-
- plt.legend(["Estimated", "Estimated 15Hz filt"], prop={'size': 8})
- plt.ylabel(lgd[i])
- self.custom_suptitle("Comparison between position quantities before and after 15Hz low-pass filtering")
-
- lgd = ["Linear vel X", "Linear vel Y", "Linear vel Z",
- "Angular vel Roll", "Angular vel Pitch", "Angular vel Yaw"]
- plt.figure()
- for i in range(6):
- if i == 0:
- ax0 = plt.subplot(3, 2, index6[i])
- else:
- plt.subplot(3, 2, index6[i], sharex=ax0)
-
- plt.plot(t_range, self.loop_h_v[:, i], linewidth=3)
- plt.plot(t_range, self.loop_h_v_windowed[:, i], linewidth=3)
- plt.plot(t_range, self.loop_h_v_filt_mpc[:, i], linewidth=3)
- plt.plot(t_range, self.loop_vref_filt_mpc[:, i], linewidth=3)
-
- plt.legend(["Estimated", "Estimated 3Hz windowed", "Estimated 15Hz filt",
- "Estimated 15Hz filt 3Hz windowed", "Reference 15Hz filt"], prop={'size': 8})
- plt.ylabel(lgd[i])
- self.custom_suptitle("Comparison between velocity quantities before and after 15Hz low-pass filtering")
- """
-
- ###############################
- # Display all graphs and wait #
- ###############################
plt.show(block=True)
def custom_suptitle(self, name):
@@ -749,11 +733,11 @@ class LoggerControl():
fig.suptitle(name)
fig.canvas.manager.set_window_title(name)
-
def saveAll(self, loggerSensors, fileName="data"):
date_str = datetime.now().strftime('_%Y_%m_%d_%H_%M')
+ name = fileName + date_str + "_" + str(self.type_MPC) + ".npz"
- np.savez_compressed(fileName + date_str + "_" + str(self.type_MPC) + ".npz",
+ np.savez_compressed(name,
joy_v_ref=self.joy_v_ref,
@@ -832,6 +816,12 @@ class LoggerControl():
tstamps=self.tstamps,
+ update_mip=self.update_mip,
+ configs=self.configs,
+ initial_contacts=self.initial_contacts,
+ t_mip=self.t_mip,
+ mpc_cost=self.mpc_cost,
+
q_mes=loggerSensors.q_mes,
v_mes=loggerSensors.v_mes,
baseOrientation=loggerSensors.baseOrientation,
@@ -849,8 +839,9 @@ class LoggerControl():
voltage=loggerSensors.voltage,
energy=loggerSensors.energy,
)
+ print("Log saved in " + name)
- def loadAll(self, loggerSensors, fileName=None):
+ def loadAll(self, loggerSensors):
if self.data is None:
print("No data file loaded. Need one in the constructor.")
@@ -936,6 +927,12 @@ class LoggerControl():
self.tstamps = self.data["tstamps"]
+ self.update_mip = self.data["update_mip"]
+ self.configs = self.data["configs"]
+ self.initial_contacts = self.data["initial_contacts"]
+ self.t_mip = self.data["t_mip"]
+ self.mpc_cost = self.data["mpc_cost"]
+
# Load LoggerSensors arrays
loggerSensors.q_mes = self.data["q_mes"]
loggerSensors.v_mes = self.data["v_mes"]
@@ -954,7 +951,6 @@ class LoggerControl():
loggerSensors.current = self.data["current"]
loggerSensors.voltage = self.data["voltage"]
loggerSensors.energy = self.data["energy"]
-
def slider_predicted_trajectory(self):
@@ -994,7 +990,7 @@ class LoggerControl():
#ax.set_xlabel('Time [s]')
axcolor = 'lightgoldenrodyellow'
- #ax.margins(x=0)
+ # ax.margins(x=0)
# Make a horizontal slider to control the time.
axtime = plt.axes([0.25, 0.03, 0.65, 0.03], facecolor=axcolor)
@@ -1020,7 +1016,7 @@ class LoggerControl():
h2s_vel.append(h2)
#axcolor = 'lightgoldenrodyellow'
- #ax.margins(x=0)
+ # ax.margins(x=0)
# Make a horizontal slider to control the time.
axtime_vel = plt.axes([0.25, 0.03, 0.65, 0.03], facecolor=axcolor)
@@ -1147,20 +1143,16 @@ if __name__ == "__main__":
import LoggerSensors
import sys
import os
- from sys import argv
- sys.path.insert(0, os.getcwd()) # adds current directory to python path
+ import libquadruped_reactive_walking as lqrw
- # Data file name to load
- file_name = "/home/odri/data_2021_10_07_17_40.npz"
+ sys.path.insert(0, os.getcwd())
- # Create loggers
- logger = LoggerControl(0.001, 30, loading=True)
- loggerSensors = LoggerSensors.LoggerSensors(logSize=logger.logSize)
+ file_name = "/home/odri/git/fanny/logs/data_2022_02_16_13_33_0.npz"
- # Load data from .npz file
- logger.loadAll(LoggerSensors)
+ params = lqrw.Params()
+ logger = LoggerControl(params, loading=True, fileName=file_name)
- # Call all ploting functions
- logger.plotAll(loggerSensors)
+ loggerSensors = LoggerSensors.LoggerSensors(logSize=logger.logSize)
- # logger.slider_predicted_trajectory()
+ logger.loadAll(LoggerSensors)
+ logger.plotAllGraphs(loggerSensors)
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index 2ef221d8455ba585864a027940c8f3c4c960f176..9bc1b048a588346659b2b242768cea323748810c 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -14,6 +14,7 @@ import libquadruped_reactive_walking as lqrw
import ctypes
from ctypes import Structure, c_double
+
class MPC_type(Enum):
OSQP = 0
CROCODDYL_LINEAR = 1
@@ -21,6 +22,7 @@ class MPC_type(Enum):
CROCODDYL_PLANNER = 3
CROCODDYL_PLANNER_TIME = 4
+
class DataInCtype(Structure):
''' Ctype data structure for the shared memory between processes.
'''
@@ -30,21 +32,21 @@ class DataInCtype(Structure):
N_gait = int(params.gait.shape[0]) # Row size for fsteps (N_gait x 12), from utils_mpc.py
if mpc_type == MPC_type.CROCODDYL_PLANNER:
- _fields_ = [('k', ctypes.c_int64 ),
- ('xref', ctypes.c_double * 12 * (n_steps+1) ),
- ('fsteps', ctypes.c_double * 12 * N_gait ),
- ('l_fsteps_target', ctypes.c_double * 3 * 4 ),
- ('oRh', ctypes.c_double * 3 * 3 ),
- ('oTh', ctypes.c_double * 3 * 1 ),
- ('position', ctypes.c_double * 3 * 4 ),
- ('velocity', ctypes.c_double * 3 * 4 ),
- ('acceleration', ctypes.c_double * 3 * 4 ),
- ('jerk', ctypes.c_double * 3 * 4 ),
+ _fields_ = [('k', ctypes.c_int64),
+ ('xref', ctypes.c_double * 12 * (n_steps+1)),
+ ('fsteps', ctypes.c_double * 12 * N_gait),
+ ('l_fsteps_target', ctypes.c_double * 3 * 4),
+ ('oRh', ctypes.c_double * 3 * 3),
+ ('oTh', ctypes.c_double * 3 * 1),
+ ('position', ctypes.c_double * 3 * 4),
+ ('velocity', ctypes.c_double * 3 * 4),
+ ('acceleration', ctypes.c_double * 3 * 4),
+ ('jerk', ctypes.c_double * 3 * 4),
('dt_flying', ctypes.c_double * 4)]
else:
- _fields_ = [('k', ctypes.c_int64 ),
- ('xref', ctypes.c_double * 12 * (n_steps+1) ),
- ('fsteps', ctypes.c_double * 12 * N_gait )]
+ _fields_ = [('k', ctypes.c_int64),
+ ('xref', ctypes.c_double * 12 * (n_steps+1)),
+ ('fsteps', ctypes.c_double * 12 * N_gait)]
class Dummy:
@@ -98,6 +100,7 @@ class MPC_Wrapper:
if self.multiprocessing: # Setup variables in the shared memory
self.newData = Value('b', False)
self.newResult = Value('b', False)
+ self.cost = Value('d', 0.)
self.dataIn = Value(DataInCtype)
if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
self.dataOut = Array('d', [0] * 32 * (np.int(self.n_steps)))
@@ -128,10 +131,11 @@ class MPC_Wrapper:
else:
self.last_available_result = np.zeros((24, (np.int(self.n_steps))))
self.last_available_result[:24, 0] = np.hstack((x_init, np.array([0.0, 0.0, 8.0] * 4)))
+ self.last_cost = 0.
- def solve(self, k, xref, fsteps, gait, l_fsteps_target, oRh = np.eye(3), oTh = np.zeros((3,1)),
- position = np.zeros((3,4)), velocity = np.zeros((3,4)) , acceleration = np.zeros((3,4)), jerk = np.zeros((3,4)) ,
- dt_flying = np.zeros(4)):
+ def solve(self, k, xref, fsteps, gait, l_fsteps_target, oRh=np.eye(3), oTh=np.zeros((3, 1)),
+ position=np.zeros((3, 4)), velocity=np.zeros((3, 4)), acceleration=np.zeros((3, 4)), jerk=np.zeros((3, 4)),
+ dt_flying=np.zeros(4)):
"""Call either the asynchronous MPC or the synchronous MPC depending on the value of multiprocessing during
the creation of the wrapper
@@ -178,16 +182,17 @@ class MPC_Wrapper:
self.t_mpc_solving_duration = time() - self.t_mpc_solving_start
# Retrieve desired contact forces with through the memory shared with the asynchronous
self.last_available_result = self.convert_dataOut()
- return self.last_available_result
+ self.last_cost = self.cost.value
+ return self.last_available_result, self.last_cost
else:
- return self.last_available_result
+ return self.last_available_result, self.last_cost
else:
# Directly retrieve desired contact force of the synchronous MPC object
- return self.f_applied
+ return self.f_applied, self.last_cost
else:
# Default forces for the first iteration
self.not_first_iter = True
- return self.last_available_result
+ return self.last_available_result, self.last_cost
def run_MPC_synchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
"""Run the MPC (synchronous version) to get the desired contact forces for the feet currently in stance phase
@@ -205,7 +210,7 @@ class MPC_Wrapper:
if self.mpc_type == MPC_type.OSQP:
# OSQP MPC
self.mpc.run(np.int(k), xref.copy(), fsteps.copy())
- elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Add goal position to stop the optimisation
+ elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Add goal position to stop the optimisation
# Crocoddyl MPC
self.mpc.solve(k, xref.copy(), fsteps.copy(), l_fsteps_target, position, velocity, acceleration, jerk, oRh, oTh, dt_flying)
else:
@@ -214,9 +219,12 @@ class MPC_Wrapper:
# Output of the MPC
if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
- self.f_applied = self.mpc.get_latest_result(oRh,oTh )
+ self.f_applied = self.mpc.get_latest_result(oRh, oTh)
else:
self.f_applied = self.mpc.get_latest_result()
+
+ if self.mpc_type == MPC_type.OSQP:
+ self.last_cost = self.mpc.retrieve_cost()
def run_MPC_asynchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
"""Run the MPC (asynchronous version) to get the desired contact forces for the feet currently in stance phase
@@ -278,7 +286,7 @@ class MPC_Wrapper:
loop_mpc = MPC_crocoddyl.MPC_crocoddyl(self.params, mu=0.9, inner=False, linearModel=False)
elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Crocoddyl MPC Non-Linear with footsteps optimization
loop_mpc = MPC_crocoddyl_planner.MPC_crocoddyl_planner(self.params, mu=0.9, inner=False)
- else: # Using linear model
+ else: # Using linear model
loop_mpc = MPC_crocoddyl.MPC_crocoddyl(self.params, mu=0.9, inner=False, linearModel=True)
# Run the asynchronous MPC with the data that as been retrieved
@@ -286,9 +294,9 @@ class MPC_Wrapper:
fsteps[np.isnan(fsteps)] = 0.0
loop_mpc.run(np.int(k), xref, fsteps)
elif self.mpc_type == MPC_type.CROCODDYL_PLANNER:
- loop_mpc.solve(k, xref.copy(), fsteps.copy(), l_fsteps_target.copy(),
- position.copy(), velocity.copy(), acceleration.copy(), jerk.copy(),
- oRh.copy(), oTh.copy(), dt_flying.copy())
+ loop_mpc.solve(k, xref.copy(), fsteps.copy(), l_fsteps_target.copy(),
+ position.copy(), velocity.copy(), acceleration.copy(), jerk.copy(),
+ oRh.copy(), oTh.copy(), dt_flying.copy())
else:
loop_mpc.solve(k, xref.copy(), fsteps.copy())
@@ -301,6 +309,9 @@ class MPC_Wrapper:
else:
self.dataOut[:] = loop_mpc.get_latest_result().ravel(order='F')
+ if self.mpc_type == MPC_type.OSQP:
+ self.cost.value = loop_mpc.retrieve_cost()
+
# Set shared variable to true to signal that a new result is available
newResult.value = True
@@ -323,21 +334,21 @@ class MPC_Wrapper:
with self.dataIn.get_lock():
if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
self.dataIn.k = k
- np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps+1))[:,:] = xref
- np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))[:,:] = fsteps
- np.frombuffer(self.dataIn.l_fsteps_target).reshape((3, 4))[:,:] = l_fsteps_target
- np.frombuffer(self.dataIn.oRh).reshape((3, 3))[:,:] = oRh
- np.frombuffer(self.dataIn.oTh).reshape((3, 1))[:,:] = oTh
- np.frombuffer(self.dataIn.position).reshape((3, 4))[:,:] = position
- np.frombuffer(self.dataIn.velocity).reshape((3, 4))[:,:] = velocity
- np.frombuffer(self.dataIn.acceleration).reshape((3, 4))[:,:] = acceleration
- np.frombuffer(self.dataIn.jerk).reshape((3, 4))[:,:] = jerk
+ np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps+1))[:, :] = xref
+ np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))[:, :] = fsteps
+ np.frombuffer(self.dataIn.l_fsteps_target).reshape((3, 4))[:, :] = l_fsteps_target
+ np.frombuffer(self.dataIn.oRh).reshape((3, 3))[:, :] = oRh
+ np.frombuffer(self.dataIn.oTh).reshape((3, 1))[:, :] = oTh
+ np.frombuffer(self.dataIn.position).reshape((3, 4))[:, :] = position
+ np.frombuffer(self.dataIn.velocity).reshape((3, 4))[:, :] = velocity
+ np.frombuffer(self.dataIn.acceleration).reshape((3, 4))[:, :] = acceleration
+ np.frombuffer(self.dataIn.jerk).reshape((3, 4))[:, :] = jerk
np.frombuffer(self.dataIn.dt_flying).reshape(4)[:] = dt_flying
else:
self.dataIn.k = k
- np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps+1))[:,:] = xref
- np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))[:,:] = fsteps
+ np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps+1))[:, :] = xref
+ np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))[:, :] = fsteps
def decompress_dataIn(self, dataIn):
"""Decompress data from a C-type structure that belongs to the shared memory to retrieve data from the main control
@@ -352,14 +363,14 @@ class MPC_Wrapper:
k = self.dataIn.k
xref = np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps+1))
fsteps = np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))
- l_fsteps_target = np.frombuffer(self.dataIn.l_fsteps_target).reshape((3,4))
- oRh = np.frombuffer(self.dataIn.oRh).reshape((3,3))
- oTh = np.frombuffer(self.dataIn.oTh).reshape((3,1))
- position = np.frombuffer(self.dataIn.position).reshape((3,4))
- velocity = np.frombuffer(self.dataIn.velocity).reshape((3,4))
- acceleration = np.frombuffer(self.dataIn.acceleration).reshape((3,4))
- jerk = np.frombuffer(self.dataIn.jerk).reshape((3,4))
- dt_flying = np.frombuffer(self.dataIn.dt_flying).reshape(4)
+ l_fsteps_target = np.frombuffer(self.dataIn.l_fsteps_target).reshape((3, 4))
+ oRh = np.frombuffer(self.dataIn.oRh).reshape((3, 3))
+ oTh = np.frombuffer(self.dataIn.oTh).reshape((3, 1))
+ position = np.frombuffer(self.dataIn.position).reshape((3, 4))
+ velocity = np.frombuffer(self.dataIn.velocity).reshape((3, 4))
+ acceleration = np.frombuffer(self.dataIn.acceleration).reshape((3, 4))
+ jerk = np.frombuffer(self.dataIn.jerk).reshape((3, 4))
+ dt_flying = np.frombuffer(self.dataIn.dt_flying).reshape(4)
return k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying
diff --git a/scripts/README.md b/scripts/README.md
deleted file mode 100644
index 5de71a5b79c2c0a507b450871a3c52e77085e8b9..0000000000000000000000000000000000000000
--- a/scripts/README.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# Python Scripts
-
-* See the README in the main folder for instructions on how to use the scripts
\ No newline at end of file
diff --git a/scripts/bauzil_stairs.urdf b/scripts/bauzil_stairs.urdf
deleted file mode 100644
index 15049bb7ca61d424ad41380a737bce997228a7e5..0000000000000000000000000000000000000000
--- a/scripts/bauzil_stairs.urdf
+++ /dev/null
@@ -1,24 +0,0 @@
-<robot name="bauzil_stairs.urdf">
- <link concave="yes" name="baseLink">
- <inertial>
- <origin rpy="0 0 0" xyz="0 0 0"/>
- <mass value="0.0"/>
- <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
- </inertial>
- <visual>
- <origin rpy="0 0 -0.75" xyz="-1.2 -1.5 -0.01"/>
- <geometry>
- <mesh filename="bauzil_stairs.stl" scale="0.7 1.0 0.5"/>
- </geometry>
- <material name="red">
- <color rgba="1 0 0 1"/>
- </material>
- </visual>
- <collision concave="yes">
- <origin rpy="0 0 -0.75" xyz="-1.2 -1.5 -0.01"/>
- <geometry>
- <mesh filename="bauzil_stairs.stl" scale="0.7 1.0 0.5"/>
- </geometry>
- </collision>
- </link>
-</robot>
diff --git a/scripts/config_solo12.yaml b/scripts/config_solo12.yaml
index c721df9b3c5e2e442007a4f917325d45775e71a7..50f39f82bc1cd4ae46e3065e30a02321ca189915 100644
--- a/scripts/config_solo12.yaml
+++ b/scripts/config_solo12.yaml
@@ -10,12 +10,13 @@ robot:
true, true, true, false, false, false
]
lower_joint_limits: [
- -1.2, -1.3, -3.0, -1.2, -1.3, -3.0,
- -1.2, -1.3, -3.0, -1.2, -1.3, -3.0
+ -1.2, -2.0, -3.12, -1.2, -2.0, -3.12,
+ -1.2, -2.0, -3.12, -1.2, -2.0, -3.12
]
+
upper_joint_limits: [
- 1.2, 1.3, +3.0, +1.2, +1.3, +3.0,
- 1.2, 1.3, +3.0, +1.2, +1.3, +3.0
+ 1.2, 2., +3.12, +1.2, 2., +3.12,
+ 1.2, 2., +3.12, +1.2, 2., +3.12
]
max_joint_velocities: 80.
safety_damping: 0.5
diff --git a/scripts/solo3D/tools/heightmap_generator.py b/scripts/heightmap_generator.py
similarity index 52%
rename from scripts/solo3D/tools/heightmap_generator.py
rename to scripts/heightmap_generator.py
index b9a090d01d16f0bc8d3d7cb619389e8fbe9824b0..2551dd374243de97a21d7329f8197c4bcafa5937 100644
--- a/scripts/solo3D/tools/heightmap_generator.py
+++ b/scripts/heightmap_generator.py
@@ -1,9 +1,9 @@
import matplotlib.pyplot as plt
import os
+import numpy as np
-import sl1m.tools.plot_tools as plot
-from sl1m.tools.plot_tools import draw_whole_scene
from solo3D.tools.heightmap_tools import Heightmap
+from solo3D.tools.utils import getAllSurfacesDict_inner
from solo_rbprm.solo_abstract import Robot
@@ -16,20 +16,21 @@ 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]
+N_X = 400
+N_Y = 90
+X_BOUNDS = [-0.3, 2.5]
+Y_BOUNDS = [-0.8, 0.8]
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/"
-]
+paths = [os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/com_inequalities/feet_quasi_flat/",
+ os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/relative_effector_positions/"]
+
+COLORS = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']
# --------------------------------- METHODS ---------------------------------------------------------------
+
def init_afftool():
"""
Initialize the affordance tool and return the solo abstract rbprm builder, the surface
@@ -45,23 +46,51 @@ def init_afftool():
vf = ViewerFactory(ps)
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
- afftool.loadObstacleModel(params.environment_URDF, "environment", vf)
+ afftool.loadObstacleModel(os.environ["SOLO3D_ENV_DIR"] + params.environment_URDF, "environment", vf)
ps.selectPathValidation("RbprmPathValidation", 0.05)
return afftool
+def plot_surface(points, ax, color_id=0, alpha=1.):
+ """
+ Plot a surface
+ """
+ xs = np.append(points[0, :], points[0, 0]).tolist()
+ ys = np.append(points[1, :], points[1, 0]).tolist()
+ zs = np.append(points[2, :], points[2, 0]).tolist()
+ if color_id == -1:
+ ax.plot(xs, ys, zs)
+ else:
+ ax.plot(xs, ys, zs, color=COLORS[color_id % len(COLORS)], alpha=alpha)
+
+
+def draw_whole_scene(surface_dict, ax=None, title=None, color_id=5):
+ """
+ Plot all the potential surfaces
+ """
+ if ax is None:
+ fig = plt.figure()
+ if title is not None:
+ fig.suptitle(title, fontsize=16)
+ ax = fig.add_subplot(111, projection="3d")
+ for key in surface_dict.keys():
+ plot_surface(np.array(surface_dict[key][0]).T, ax, color_id)
+ return ax
+
# --------------------------------- MAIN ---------------------------------------------------------------
if __name__ == "__main__":
afftool = init_afftool()
affordances = afftool.getAffordancePoints('Support')
all_surfaces = getAllSurfacesDict(afftool)
+ new_surfaces = getAllSurfacesDict_inner(all_surfaces, 0.03)
heightmap = Heightmap(N_X, N_Y, X_BOUNDS, Y_BOUNDS)
heightmap.build(affordances)
- # heightmap.save_pickle(ENV_HEIGHTMAP)
- heightmap.save_binary(params.environment_heightmap)
+ heightmap.save_binary(os.environ["SOLO3D_ENV_DIR"] + params.environment_heightmap)
+ # heightmap.save_pickle(os.environ["SOLO3D_ENV_DIR"] + params.environment_heightmap + ".pickle")
- ax_heightmap = plot.plot_heightmap(heightmap)
- draw_whole_scene(all_surfaces)
- plt.show(block = True)
+ heightmap.plot()
+ ax = draw_whole_scene(all_surfaces)
+ draw_whole_scene(new_surfaces, ax, color_id=0)
+ plt.show(block=True)
\ No newline at end of file
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index e7a118c09ccd2c69b4e3a8135ad888d05d4434fb..d3b8c36c6263c1440464f57d7689f63581c4a121 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -18,20 +18,23 @@ else:
import libodri_control_interface_pywrap as oci
from solopython.utils.qualisysClient import QualisysClient
+
def get_input():
- keystrk = input()
- # thread doesn't continue until key is pressed
- # and so it remains alive
+ """
+ Thread to get the input
+ """
+ input()
+
def put_on_the_floor(device, q_init):
- """Make the robot go to the default initial position and wait for the user
+ """
+ Make the robot go to the default initial position and wait for the user
to press the Enter key to start the main control loop
Args:
device (robot wrapper): a wrapper to communicate with the robot
q_init (array): the default position of the robot
"""
-
print("PUT ON THE FLOOR.")
Kp_pos = 6.
@@ -50,11 +53,13 @@ def put_on_the_floor(device, q_init):
while i.is_alive():
device.parse_sensor_data()
device.send_command_and_wait_end_of_cycle(params.dt_wbc)
-
+
# Slow increase till 1/4th of mass is supported by each foot
- duration_increase = 2.0; # in seconds
+ duration_increase = 2.0 # in seconds
steps = int(duration_increase / params.dt_wbc)
- tau_ff = np.array([0.0, 0.022, 0.5, 0.0, 0.022, 0.5, 0.0, 0.025, 0.575, 0.0, 0.025, 0.575])
+ tau_ff = np.array([0.0, 0.022, 0.5, 0.0, 0.022, 0.5, 0.0, -0.022, -0.5, 0.0, -0.022, -0.5])
+ # tau_ff = np.array([0.0, 0.022, 0.5, 0.0, 0.022, 0.5, 0.0, 0.025, 0.575, 0.0, 0.025, 0.575])
+
for i in range(steps):
device.joints.set_torques(tau_ff * i / steps)
device.parse_sensor_data()
@@ -63,6 +68,22 @@ def put_on_the_floor(device, q_init):
print("Start the motion.")
+def check_position_error(device, controller):
+ """
+ Check the distance between current and desired position of the joints
+
+ Args:
+ device (robot wrapper): a wrapper to communicate with the robot
+ controller (array): the controller storing the desired position
+ """
+ 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)
+ return True
+ return False
+
+
def clone_movements(name_interface_clone, q_init, cloneP, cloneD, cloneQdes, cloneDQdes, cloneRunning, cloneResult):
print("-- Launching clone interface --")
@@ -72,10 +93,7 @@ def clone_movements(name_interface_clone, q_init, cloneP, cloneD, cloneQdes, clo
clone.Init(calibrateEncoders=True, q_init=q_init)
while cloneRunning.value and not clone.hardware.IsTimeout():
-
- # print(cloneP[:], cloneD[:], cloneQdes[:], cloneDQdes[:], cloneRunning.value, cloneResult.value)
if cloneResult.value:
-
clone.SetDesiredJointPDgains(cloneP[:], cloneD[:])
clone.SetDesiredJointPosition(cloneQdes[:])
clone.SetDesiredJointVelocity(cloneDQdes[:])
@@ -84,32 +102,69 @@ def clone_movements(name_interface_clone, q_init, cloneP, cloneD, cloneQdes, clo
clone.SendCommand(WaitEndOfCycle=True)
cloneResult.value = False
-
return 0
-def control_loop(name_interface_clone=None, des_vel_analysis=None):
- """Main function that calibrates the robot, get it into a default waiting position then launch
- the main control loop once the user has pressed the Enter key
+def launch_clone_process(name_interface_clone, q_init):
+ print("PASS")
+ from multiprocessing import Process, Array, Value
+ cloneP = Array('d', [0] * 12)
+ cloneD = Array('d', [0] * 12)
+ cloneQdes = Array('d', [0] * 12)
+ cloneDQdes = Array('d', [0] * 12)
+ cloneRunning = Value('b', True)
+ cloneResult = Value('b', True)
+ clone = Process(target=clone_movements, args=(name_interface_clone, q_init, cloneP,
+ cloneD, cloneQdes, cloneDQdes, cloneRunning, cloneResult))
+ clone.start()
+ print(cloneResult.value)
+
+ return cloneResult
+
+
+def damp_control(device, nb_motors):
+ """
+ Damp the control during 2.5 seconds
Args:
- name_interface_clone (string): name of the interface that will mimic the movements of the first
+ device (robot wrapper): a wrapper to communicate with the robot
+ nb_motors (int): number of motors
"""
+ t = 0.0
+ t_max = 2.5
+ while (not device.is_timeout) and (t < t_max):
+ device.parse_sensor_data() # Retrieve data from IMU and Motion capture
+
+ # Set desired quantities for the actuators
+ device.joints.set_position_gains(np.zeros(nb_motors))
+ device.joints.set_velocity_gains(0.1 * np.ones(nb_motors))
+ device.joints.set_desired_positions(np.zeros(nb_motors))
+ device.joints.set_desired_velocities(np.zeros(nb_motors))
+ device.joints.set_torques(np.zeros(nb_motors))
+
+ # Send command to the robot
+ device.send_command_and_wait_end_of_cycle(params.dt_wbc)
+ if (t % 1) < 5e-5:
+ print('IMU attitude:', device.imu.attitude_euler)
+ print('joint pos :', device.joints.positions)
+ print('joint vel :', device.joints.velocities)
+ device.robot_interface.PrintStats()
- # Check .yaml file for parameters of the controller
+ t += params.dt_wbc
- # Name of the interface that is used to communicate with the robot
- name_interface = params.interface
- # Enable or disable PyBullet GUI
+def control_loop(name_interface_clone=None, des_vel_analysis=None):
+ """
+ Main function that calibrates the robot, get it into a default waiting position then launch
+ the main control loop once the user has pressed the Enter key
+
+ Args:
+ name_interface_clone (string): name of the interface that will mimic the movements of the first
+ """
if not params.SIMULATION:
params.enable_pyb_GUI = False
- # Time variable to keep track of time
- t = 0.0
-
- # Default position after calibration
- q_init = np.array(params.q_init.tolist())
+ q_init = np.array(params.q_init.tolist()) # Default position after calibration
if params.SIMULATION and (des_vel_analysis is not None):
print("Analysis: %1.1f %1.1f %1.1f" % (des_vel_analysis[0], des_vel_analysis[1], des_vel_analysis[5]))
@@ -119,14 +174,11 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
N_steady = int(steady_state_duration / params.dt_wbc)
params.N_SIMULATION = N_analysis + N_steady
- # Run a scenario and retrieve data thanks to the logger
- controller = Controller(params, q_init, t)
+ controller = Controller(params, q_init, 0.)
if params.SIMULATION and (des_vel_analysis is not None):
controller.joystick.update_for_analysis(des_vel_analysis, N_analysis, N_steady)
- ####
-
if params.SIMULATION:
device = PyBulletSimulator()
qc = None
@@ -135,55 +187,29 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if name_interface_clone is not None:
- print("PASS")
- from multiprocessing import Process, Array, Value
- cloneP = Array('d', [0] * 12)
- cloneD = Array('d', [0] * 12)
- cloneQdes = Array('d', [0] * 12)
- cloneDQdes = Array('d', [0] * 12)
- cloneRunning = Value('b', True)
- cloneResult = Value('b', True)
- clone = Process(target=clone_movements, args=(name_interface_clone, q_init, cloneP,
- cloneD, cloneQdes, cloneDQdes, cloneRunning, cloneResult))
- clone.start()
- print(cloneResult.value)
+ cloneResult = launch_clone_process(name_interface_clone, q_init)
if params.LOGGING or params.PLOTTING:
loggerSensors = LoggerSensors(device, qualisys=qc, logSize=params.N_SIMULATION-3)
- loggerControl = LoggerControl(params.dt_wbc, params.gait.shape[0], params.type_MPC, joystick=controller.joystick,
- estimator=controller.estimator, loop=controller,
- gait=controller.gait, statePlanner=controller.statePlanner,
- footstepPlanner=controller.footstepPlanner,
- footTrajectoryGenerator=controller.footTrajectoryGenerator,
- logSize=params.N_SIMULATION-3)
-
- # Number of motors
- nb_motors = 12
+ loggerControl = LoggerControl(params, logSize=params.N_SIMULATION-3)
- # Initiate communication with the device and calibrate encoders
if params.SIMULATION:
device.Init(calibrateEncoders=True, q_init=q_init, envID=params.envID,
use_flat_plane=params.use_flat_plane, enable_pyb_GUI=params.enable_pyb_GUI, dt=params.dt_wbc)
- # ForceMonitor to display contact forces in PyBullet with red lines
- import ForceMonitor
- myForceMonitor = ForceMonitor.ForceMonitor(device.pyb_sim.robotId, device.pyb_sim.planeId)
- else:
- # Initialize the communication and the session.
+ # import ForceMonitor
+ # myForceMonitor = ForceMonitor.ForceMonitor(device.pyb_sim.robotId, device.pyb_sim.planeId)
+ else: # Initialize the communication and the session.
device.initialize(q_init[:])
device.joints.set_zero_commands()
-
device.parse_sensor_data()
-
- # Wait for Enter input before starting the control loop
put_on_the_floor(device, q_init)
# CONTROL LOOP ***************************************************
- t = 0.0
+ t = 0.0 # Time variable to keep track of time
t_max = (params.N_SIMULATION-2) * params.dt_wbc
t_log_whole = np.zeros((params.N_SIMULATION))
k_log_whole = 0
- t_start_whole = 0.0
T_whole = time.time()
dT_whole = 0.0
@@ -193,23 +219,14 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
log_Mddq_out = np.zeros((params.N_SIMULATION, 6))
log_JcTf_out = np.zeros((params.N_SIMULATION, 6))
while ((not device.is_timeout) and (t < t_max) and (not controller.error)):
-
t_start_whole = time.time()
- # Update sensor data (IMU, encoders, Motion capture)
device.parse_sensor_data()
+ if controller.compute(device, qc):
+ break
- # Desired torques
- 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 and check_position_error(device, controller):
+ break
# Set desired quantities for the actuators
device.joints.set_position_gains(controller.result.P)
@@ -232,45 +249,11 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
controller.footstepPlanner, controller.footTrajectoryGenerator,
controller.wbcWrapper, dT_whole)
-
t_end_whole = time.time()
# myForceMonitor.display_contact_forces()
- # Send command to the robot
- for i in range(1):
- device.send_command_and_wait_end_of_cycle(params.dt_wbc)
- """if (t % 1) < 5e-5:
- print('IMU attitude:', device.imu.attitude_euler)
- print('joint pos :', device.joints.positions)
- print('joint vel :', device.joints.velocities)
- device.robot_interface.PrintStats()"""
-
- """import os
- from matplotlib import pyplot as plt
- import pybullet as pyb
- if (t == 0.0):
- cpt_frames = 0
- step = 10
- if (cpt_frames % step) == 0:
- if (cpt_frames % 1000):
- print(cpt_frames)
- img = pyb.getCameraImage(width=1920, height=1080, renderer=pyb.ER_BULLET_HARDWARE_OPENGL)
- if cpt_frames == 0:
- newpath = r'/tmp/recording'
- if not os.path.exists(newpath):
- os.makedirs(newpath)
- if (int(cpt_frames/step) < 10):
- plt.imsave('/tmp/recording/frame_000'+str(int(cpt_frames/step))+'.png', img[2])
- elif int(cpt_frames/step) < 100:
- plt.imsave('/tmp/recording/frame_00'+str(int(cpt_frames/step))+'.png', img[2])
- elif int(cpt_frames/step) < 1000:
- plt.imsave('/tmp/recording/frame_0'+str(int(cpt_frames/step))+'.png', img[2])
- else:
- plt.imsave('/tmp/recording/frame_'+str(int(cpt_frames/step))+'.png', img[2])
-
- cpt_frames += 1"""
-
+ device.send_command_and_wait_end_of_cycle(params.dt_wbc) # Send command to the robot
t += params.dt_wbc # Increment loop time
dT_whole = T_whole
@@ -281,102 +264,44 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
k_log_whole += 1
# ****************************************************************
+ finished = (t >= t_max)
+ damp_control(device, 12)
- if (t >= t_max):
- finished = True
- else:
- finished = False
-
- # Stop clone interface running in parallel process
if not params.SIMULATION and name_interface_clone is not None:
- cloneResult.value = False
+ cloneResult.value = False # Stop clone interface running in parallel process
- # Stop MPC running in a parallel process
if params.enable_multiprocessing:
- print("Stopping parallel process")
+ print("Stopping parallel process MPC")
controller.mpc_wrapper.stop_parallel_loop()
- # controller.view.stop() # Stop viewer
- # DAMPING TO GET ON THE GROUND PROGRESSIVELY *********************
- t = 0.0
- t_max = 2.5
- while ((not device.is_timeout) and (t < t_max)):
-
- device.parse_sensor_data() # Retrieve data from IMU and Motion capture
+ if params.solo3D and params.enable_multiprocessing_mip:
+ print("Stopping parallel process MIP")
+ controller.surfacePlanner.stop_parallel_loop()
- # Set desired quantities for the actuators
- device.joints.set_position_gains(np.zeros(nb_motors))
- device.joints.set_velocity_gains(0.1 * np.ones(nb_motors))
- device.joints.set_desired_positions(np.zeros(nb_motors))
- device.joints.set_desired_velocities(np.zeros(nb_motors))
- device.joints.set_torques(np.zeros(nb_motors))
-
- # Send command to the robot
- device.send_command_and_wait_end_of_cycle(params.dt_wbc)
- if (t % 1) < 5e-5:
- print('IMU attitude:', device.imu.attitude_euler)
- print('joint pos :', device.joints.positions)
- print('joint vel :', device.joints.velocities)
- device.robot_interface.PrintStats()
-
- t += params.dt_wbc
-
- # FINAL SHUTDOWN *************************************************
+ # ****************************************************************
- # Whatever happened we send 0 torques to the motors.
- device.joints.set_torques(np.zeros(nb_motors))
+ # Send 0 torques to the motors.
+ device.joints.set_torques(np.zeros(12))
device.send_command_and_wait_end_of_cycle(params.dt_wbc)
if device.is_timeout:
print("Masterboard timeout detected.")
print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.")
- """from matplotlib import pyplot as plt
- N = loggerControl.tstamps.shape[0]
- t_range = np.array([k*loggerControl.dt for k in range(N)])
- plt.figure()
- plt.plot(t_range, log_Mddq[:-3, 0], "r")
- plt.plot(t_range, log_NLE[:-3, 0], "b")
- plt.plot(t_range, log_Mddq[:-3, 0] + log_NLE[:-3, 0], "violet", linestyle="--")
- plt.plot(t_range, log_JcTf[:-3, 0], "g")
- plt.plot(t_range, log_Mddq_out[:-3, 0], "darkred")
- plt.plot(t_range, log_Mddq_out[:-3, 0] + log_NLE[:-3, 0], "darkorchid", linestyle="--")
- plt.plot(t_range, log_JcTf_out[:-3, 0], "mediumblue")
- plt.plot(t_range, loggerControl.planner_gait[:, 0, 0], "k", linewidth=3)
- plt.legend(["Mddq", "NLE", "Mddq+NLE", "JcT f", "Mddq out", "Mddq out + NLE", "JcT f out", "Contact"])
- plt.show(block=True)"""
-
- # Plot recorded data
- if params.PLOTTING:
- loggerControl.plotAll(loggerSensors)
-
- # Save the logs of the Logger object
if params.LOGGING:
- loggerControl.saveAll(loggerSensors)
- print("Log saved")
+ loggerControl.saveAll(loggerSensors, "/home/odri/git/fanny/logs/data")
+
+ if params.PLOTTING:
+ loggerControl.plotAllGraphs(loggerSensors)
if params.SIMULATION and params.enable_pyb_GUI:
- # Disconnect the PyBullet server (also close the GUI)
device.Stop()
- if controller.error:
- if (controller.error_flag == 1):
- print("-- POSITION LIMIT ERROR --")
- elif (controller.error_flag == 2):
- print("-- VELOCITY TOO HIGH ERROR --")
- elif (controller.error_flag == 3):
- print("-- FEEDFORWARD TORQUES TOO HIGH ERROR --")
- print(controller.error_value)
-
print("End of script")
-
return finished, des_vel_analysis
-def main():
- """Main function
- """
-
+if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Playback trajectory to show the extent of solo12 workspace.')
parser.add_argument('-c',
'--clone',
@@ -384,11 +309,7 @@ def main():
help='Name of the clone interface that will reproduce the movement of the first one \
(use ifconfig in a terminal), for instance "enp1s0"')
- os.nice(-20) # Â Set the process to highest priority (from -20 highest to +20 lowest)
+ # os.nice(-20) # Â Set the process to highest priority (from -20 highest to +20 lowest)
f, v = control_loop(parser.parse_args().clone) # , np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
print(f, v)
quit()
-
-
-if __name__ == "__main__":
- main()
diff --git a/scripts/place_com_with_invkin.py b/scripts/place_com_with_invkin.py
index fc2090dd516a8b42361e2968941b1273071867bd..ec936ced6b0e2bba89d456e1c7a8f6ab7bbab7ff 100644
--- a/scripts/place_com_with_invkin.py
+++ b/scripts/place_com_with_invkin.py
@@ -6,7 +6,7 @@ import pinocchio as pin
# Parameters of the InvkinÂ
l = 0.1946 * 2
L = 0.14695 * 2
-h = 0.218
+h = 0.18
q_init = [0.0, 0.7, -1.4, -0.0, 0.7, -1.4, 0.0, 0.7, -1.4, -0.0, 0.7, -1.4]
# Load robot model and data
@@ -45,7 +45,7 @@ while np.any(np.abs(e_basispos) > 0.001):
# Get data required by IK with Pinocchio
pos = solo.com()
rot = solo.data.oMf[BASE_ID].rotation
- Jbasis = pin.getFrameJacobian( solo.model, solo.data, BASE_ID, pin.LOCAL_WORLD_ALIGNED)[:3]
+ Jbasis = pin.getFrameJacobian(solo.model, solo.data, BASE_ID, pin.LOCAL_WORLD_ALIGNED)[:3]
Jwbasis = pin.getFrameJacobian(solo.model, solo.data, BASE_ID, pin.LOCAL_WORLD_ALIGNED)[3:]
for i_ee in range(4):
idx = int(foot_ids[i_ee])
@@ -67,9 +67,9 @@ while np.any(np.abs(e_basispos) > 0.001):
q = pin.integrate(solo.model, q, 0.01 * np.linalg.pinv(J) @ x_err)
# Compute final position of CoM
-q[7:] = np.array([0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407])
+#q[7:] = np.array([0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407])
pin.forwardKinematics(solo.model, solo.data, q, np.zeros(
- solo.model.nv), np.zeros(solo.model.nv))
+ solo.model.nv), np.zeros(solo.model.nv))
pos = solo.com()
for i_ee in range(4):
idx = int(foot_ids[i_ee])
@@ -84,4 +84,3 @@ if ('viewer' in solo.viz.__dict__):
solo.viewer.gui.addFloor('world/floor')
solo.viewer.gui.setRefreshIsSynchronous(False)
solo.display(q)
-
diff --git a/scripts/solo3D/StatePlanner.py b/scripts/solo3D/StatePlanner.py
deleted file mode 100644
index eb8f4e851cbb88bc49139e7afcfe57b485abe356..0000000000000000000000000000000000000000
--- a/scripts/solo3D/StatePlanner.py
+++ /dev/null
@@ -1,172 +0,0 @@
-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
index ac022b0fed447b50787e456c08714630385d6ec2..bdb87adf257fd52dc4de3701d7338da0b7e9bc69 100644
--- a/scripts/solo3D/SurfacePlanner.py
+++ b/scripts/solo3D/SurfacePlanner.py
@@ -1,48 +1,42 @@
import pinocchio as pin
import numpy as np
import os
-from time import perf_counter as clock
+import copy
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 solo3D.tools.utils import getAllSurfacesDict_inner
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/"
-]
+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):
+ def __init__(self, params):
"""
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.plot = False
+
+ self.use_heuristique = params.use_heuristic
+ self.step_duration = params.T_gait/2
+ self.shoulders = np.reshape(params.footsteps_under_shoulders, (3, 4), order="F")
self.solo_abstract = SoloAbstract()
self.solo_abstract.setJointBounds("root_joint", [-5., 5., -5., 5., 0.241, 1.5])
@@ -55,10 +49,10 @@ class SurfacePlanner:
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.afftool.loadObstacleModel(os.environ["SOLO3D_ENV_DIR"] + params.environment_URDF, "environment", self.vf)
self.ps.selectPathValidation("RbprmPathValidation", 0.05)
- self.all_surfaces = getAllSurfacesDict(self.afftool)
+ self.all_surfaces = getAllSurfacesDict_inner(getAllSurfacesDict(self.afftool), margin=0.02)
self.potential_surfaces = []
@@ -71,13 +65,6 @@ class SurfacePlanner:
: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
@@ -90,7 +77,6 @@ class SurfacePlanner:
gait = np.roll(gait, -2, axis=0)
-
return gait
def compute_step_length(self, o_v_ref):
@@ -99,49 +85,61 @@ class SurfacePlanner:
: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
+ step_length = o_v_ref * self.step_duration
+ return np.array([step_length[i] for i in range(2)])
- return np.array([step_length[i][0] for i in range(2)])
-
- def compute_effector_positions(self, configs):
+ def compute_effector_positions(self, configs, h_v_ref):
"""
- Compute the step_length used for the cost
- :param o_v_ref: desired velocity
- :return: desired step_length
+ Compute the desired effector positions in 2D
+ :param configs the list of configurations
+ :param h_v_ref, Array (x3) the desired velocity in horizontal frame
+ :param yaw, Array (x3) yaw of the horizontal frame wrt the world frame
"""
- effector_positions = np.zeros((4, self.pb.n_phases, 2))
+ effector_positions = [[] for _ in range(4)]
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)
+ for foot in range(4):
+ if foot in phase.moving:
+ rpy = pin.rpy.matrixToRpy(pin.Quaternion(configs[phase.id][3:7].copy()).toRotationMatrix())
+ hRb = pin.rpy.rpyToMatrix(np.array([rpy[0], rpy[1], 0.]))
+ wRh = pin.rpy.rpyToMatrix(np.array([0., 0., rpy[2]]))
+ heuristic = wRh @ (0.5 * self.step_duration * copy.deepcopy(h_v_ref)) + wRh @ hRb @ copy.deepcopy(self.shoulders[:, foot])
+ effector_positions[foot].append(np.array(configs[phase.id][:2] + heuristic[:2]))
+ else:
+ effector_positions[foot].append(None)
return effector_positions
+ def compute_shoulder_positions(self, configs):
+ """
+ Compute the shoulder positions
+ :param configs the list of configurations
+ """
+ shoulder_positions = np.zeros((4, self.pb.n_phases, 3))
+ for phase in self.pb.phaseData:
+ for foot in phase.moving:
+ R = pin.Quaternion(configs[phase.id][3:7]).toRotationMatrix()
+ shoulder_positions[foot][phase.id] = R @ self.shoulders[:, foot] + configs[phase.id][:3]
+ return shoulder_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
+ :return: a list of surface candidates and a boolean set to false if one foot hase no potential surface
"""
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]
+ 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)
+ surfaces_names = self.solo_abstract.clientRbprm.rbprm.getCollidingObstacleAtConfig(config.tolist(), rom)
for name in surfaces_names:
surfaces.append(self.all_surfaces[name][0])
@@ -162,109 +160,86 @@ class SurfacePlanner:
def retrieve_surfaces(self, surfaces, indices=None):
"""
- Get the surface vertices, inequalities and selected surface indices if need be
+ Get all the potential surfaces as vertices and as inequalities for the first step of each foot
+ and get the selected surface indices if need be
+ return vertices a list of all potential surfaces vertices for each foot's first step
+ return inequalities a list of all potential surfaces inequalities for each foot's first step
+ return indices the selected surface indices for each foot's first step
"""
vertices = []
- surfaces_inequalities = []
- if indices is not None:
- surface_indices = []
- else:
- surface_indices = None
+ inequalities = []
+ selected_surfaces_indices = []
+
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])
+ inequalities.append(self.pb.phaseData[0].S[first_phase_i])
if indices is not None:
- surface_indices.append(indices[0][first_phase_i])
+ selected_surfaces_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])
+ inequalities.append(self.pb.phaseData[1].S[second_phase_i])
if indices is not None:
- surface_indices.append(indices[1][second_phase_i])
+ selected_surfaces_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
+ return vertices, inequalities, selected_surfaces_indices
- def run(self, configs, gait_in, current_contacts, o_v_ref):
+ def run(self, configs, gait_in, current_contacts, h_v_ref):
"""
- Select the nex surfaces to use
- :param xref: successive states
- :param gait: a gait matrix
+ Select the next surfaces to use
+ :param configs: successive states
+ :param gait_in: a gait matrix
:param current_contacts: the initial_contacts to use in the computation
- :param o_v_ref: the desired velocity for the cost
+ :param h_v: Array (x3) the current velocity for the cost, in horizontal frame
+ :param h_v_ref: Array (x3) the desired velocity for the cost, in horizontal frame
: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()
-
+ surfaces, empty_list = self.get_potential_surfaces(configs, gait)
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])
+ vertices, inequalities, _ = self.retrieve_surfaces(surfaces)
+ return vertices, inequalities, None, None, False
- 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
+ self.pb.generate_problem(R, surfaces, gait, initial_contacts, c0=None, com=False)
+ shoulder_positions = self.compute_shoulder_positions(configs)
+ if self.use_heuristique:
+ effector_positions = self.compute_effector_positions(configs, h_v_ref)
+ costs = {"effector_positions_xy": [1., effector_positions], "effector_positions_3D": [0.1, shoulder_positions]}
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)
+ step_length = self.compute_step_length(h_v_ref[:2])
+ costs = {"step_length": [1.0, step_length], "effector_positions_3D": [0.1, shoulder_positions]}
+ result = solve_MIP(self.pb, costs=costs, com=False)
+
+ if result.success:
+ if self.plot:
+ import matplotlib.pyplot as plt
+ import sl1m.tools.plot_tools as plot
+ ax = plot.draw_whole_scene(self.all_surfaces)
+ plot.plot_planner_result(result.all_feet_pos, effector_positions=effector_positions, coms=configs, ax=ax, show=True)
+
+ vertices, inequalities, indices = self.retrieve_surfaces(surfaces, result.surface_indices)
+ return vertices, inequalities, indices, result.all_feet_pos, True
+
+ if self.plot:
+ import matplotlib.pyplot as plt
+ import sl1m.tools.plot_tools as plot
+ 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")
+ vertices, inequalities, _ = self.retrieve_surfaces(surfaces)
+ return vertices, inequalities, None, None, False
diff --git a/scripts/solo3D/SurfacePlannerWrapper.py b/scripts/solo3D/SurfacePlannerWrapper.py
index 587193a160b7711b4bbca01cdcfbea3c1d208151..47f8b9b3f71b903087bc6eeabcd6a72e9f0e7002 100644
--- a/scripts/solo3D/SurfacePlannerWrapper.py
+++ b/scripts/solo3D/SurfacePlannerWrapper.py
@@ -1,26 +1,22 @@
from solo3D.SurfacePlanner import SurfacePlanner
-from multiprocessing import Process, Lock
-from multiprocessing.sharedctypes import Value, Array
-from ctypes import Structure, c_double
+from multiprocessing import Process
+from multiprocessing.sharedctypes import Value
import ctypes
+import time
+import copy
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_ROWS = N_VERTICES_MAX + 2
N_SURFACE_MAX = 10
-N_SURFACE_CONFIG = 3
-N_gait = 100
-N_POTENTIAL_SURFACE = 5
+N_POTENTIAL_SURFACE = 7
N_FEET = 4
-N_PHASE = 3
-class SurfaceDataCtype(Structure):
+class SurfaceDataCtype(ctypes.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]
@@ -33,296 +29,217 @@ class SurfaceDataCtype(Structure):
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)]
+ _fields_ = [('A', ctypes.c_double * 3 * N_ROWS), ('b', ctypes.c_double * N_ROWS),
+ ('vertices', ctypes.c_double * 3 * N_VERTICES_MAX), ('on', ctypes.c_bool)]
-class DataOutCtype(Structure):
+class DataOutCtype(ctypes.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
'''
+ params = lqrw.Params()
_fields_ = [('potentialSurfaces', SurfaceDataCtype * N_POTENTIAL_SURFACE * N_FEET),
- ('selectedSurfaces', SurfaceDataCtype * N_FEET), ('all_feet', ctypes.c_double * 12 * N_PHASE),
- ('success', ctypes.c_bool)]
+ ('selectedSurfaces', SurfaceDataCtype * N_FEET), ('all_feet', ctypes.c_double * 12 * params.number_steps),
+ ('success', ctypes.c_bool), ('t_mip', ctypes.c_float)]
-class DataInCtype(Structure):
+class DataInCtype(ctypes.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)]
+ params = lqrw.Params()
+ _fields_ = [('gait', ctypes.c_int64 * 4 * int(params.gait.shape[0])), ('configs', ctypes.c_double * 7 * params.number_steps),
+ ('h_v_ref', ctypes.c_double * 3), ('contacts', ctypes.c_double * 12)]
-class SurfacePlanner_Wrapper():
- ''' Wrapper for the class SurfacePlanner for the paralellisation
+class Surface_planner_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]]
+ self.params = params
+ self.n_gait = int(params.gait.shape[0])
+ # 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
+ # Results used by controller
+ self.mip_success = False
+ self.t_mip = 0.
+ self.mip_iteration = 0
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
+ # When synchronous, values are stored to be used by controller only at the next flying phase
self.mip_success_syn = False
+ self.mip_iteration_syn = 0
+ self.potential_surfaces_syn = lqrw.SurfaceVectorVector()
+ self.selected_surfaces_syn = lqrw.SurfaceVector()
+ self.all_feet_pos_syn = []
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)
+ if self.multiprocessing:
+ self.new_data = Value('b', False)
+ self.new_result = Value('b', True)
self.running = Value('b', True)
-
- # Data Out :
- self.dataOut = Value(DataOutCtype)
- # Data IN :
- self.dataIn = Value(DataInCtype)
-
+ self.data_out = Value(DataOutCtype)
+ self.data_in = Value(DataInCtype)
+ p = Process(target=self.asynchronous_process)
+ p.start()
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 = []
+ self.planner = SurfacePlanner(params)
+
+ self.initialized = False
- def run(self, configs, gait_in, current_contacts, o_v_ref):
+ def run(self, configs, gait_in, current_contacts, h_v_ref):
+ '''
+ Either call synchronous or asynchronous planner
+ '''
if self.multiprocessing:
- self.run_asynchronous(configs, gait_in, current_contacts, o_v_ref)
+ self.run_asynchronous(configs, gait_in, current_contacts, h_v_ref)
else:
- self.run_synchronous(configs, gait_in, current_contacts, o_v_ref)
+ self.run_synchronous(configs, gait_in, current_contacts, h_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)
+ def run_synchronous(self, configs, gait, current_contacts, h_v_ref):
+ '''
+ Call the planner and store the result in syn variables
+ '''
+ t_start = time.time()
+ vertices, inequalities, indices, self.all_feet_pos_syn, success = self.planner.run(configs, gait, current_contacts, h_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()
+ self.potential_surfaces_syn = lqrw.SurfaceVectorVector()
+ for foot, foot_surfaces in enumerate(inequalities):
+ potential_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))
+ potential_surfaces.append(lqrw.Surface(S, s, vertices[foot][i].T))
+ self.potential_surfaces_syn.append(potential_surfaces)
- # 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
+ for foot, foot_inequalities in enumerate(inequalities):
+ S, s = foot_inequalities[indices[foot]]
+ self.selected_surfaces_syn.append(lqrw.Surface(S, s, vertices[foot][indices[foot]].T))
+ self.t_mip = time.time() - t_start
+
+ def run_asynchronous(self, configs, gait_in, current_contacts, h_v_ref_in):
+ '''
+ Compress the data and send them for asynchronous process
+ '''
+ for i, config in enumerate(configs):
+ data_config = np.frombuffer(self.data_in.configs[i])
+ data_config[:] = config[:]
+ gait = np.frombuffer(self.data_in.gait).reshape((self.n_gait, 4))
+ gait[:, :] = gait_in
+ h_v_ref = np.frombuffer(self.data_in.h_v_ref)
+ h_v_ref[:] = h_v_ref_in[:]
+ contact = np.frombuffer(self.data_in.contacts).reshape((3, 4))
+ contact[:, :] = current_contacts[:, :]
+ self.new_data.value = True
+
+ def asynchronous_process(self):
+ '''
+ Asynchronous process created during initialization
+ '''
+ planner = SurfacePlanner(self.params)
+ while self.running.value:
+ if self.new_data.value:
+ self.new_data.value = False
+ t_start = time.time()
+
+ configs = [np.frombuffer(config) for config in self.data_in.configs]
+ gait = np.frombuffer(self.data_in.gait).reshape((self.n_gait, 4))
+ h_v_ref = np.frombuffer(self.data_in.h_v_ref).reshape((3))
+ contacts = np.frombuffer(self.data_in.contacts).reshape((3, 4))
+
+ vertices, inequalities, indices, _, success = planner.run(configs, gait, contacts, h_v_ref)
+
+ t = time.time() - t_start
+ self.compress_result(vertices, inequalities, indices, success, t)
+ self.new_result.value = True
+
+ def compress_result(self, vertices, inequalities, indices, success, t):
+ '''
+ Store the planner result in data_out
+ '''
+ self.data_out.success = success
+ self.data_out.t_mip = t
+ for foot, foot_inequalities in enumerate(inequalities):
+ i = 0
+ for i, (S, s) in enumerate(foot_inequalities):
+ A = np.frombuffer(self.data_out.potentialSurfaces[foot][i].A).reshape((N_ROWS, 3))
+ A[:, :] = S[:, :]
+ b = np.frombuffer(self.data_out.potentialSurfaces[foot][i].b)
+ b[:] = s[:]
+ v = np.frombuffer(self.data_out.potentialSurfaces[foot][i].vertices).reshape((N_VERTICES_MAX, 3))
+ v[:, :] = vertices[foot][i].T[:, :]
+ self.data_out.potentialSurfaces[foot][i].on = True
+
+ for j in range(i + 1, N_POTENTIAL_SURFACE):
+ self.data_out.potentialSurfaces[foot][j].on = False
- def update_latest_results(self):
- ''' Update latest results : 2 list
+ if success:
+ for foot, index in enumerate(indices):
+ A = np.frombuffer(self.data_out.selectedSurfaces[foot].A).reshape((N_ROWS, 3))
+ A[:, :] = inequalities[foot][index][0][:, :]
+ b = np.frombuffer(self.data_out.selectedSurfaces[foot].b)
+ b[:] = inequalities[foot][index][1][:]
+ v = np.frombuffer(self.data_out.selectedSurfaces[foot].vertices).reshape((N_VERTICES_MAX, 3))
+ v = vertices[foot][index].T[:, :]
+ self.data_out.selectedSurfaces[foot].on = True
+
+ def get_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
+ if self.new_result.value:
+ self.new_result.value = False
+
+ self.mip_success = self.data_out.success
+ self.t_mip = self.data_out.t_mip
+ self.mip_iteration += 1
+
+ self.potential_surfaces = lqrw.SurfaceVectorVector()
+ for foot_surfaces in self.data_out.potentialSurfaces:
+ potential_surfaces = lqrw.SurfaceVector()
+ for s in foot_surfaces:
+ if s.on:
+ potential_surfaces.append(lqrw.Surface(np.array(s.A), np.array(s.b), np.array(s.vertices)))
+ self.potential_surfaces.append(potential_surfaces)
+
+ self.selected_surfaces = lqrw.SurfaceVector()
+ if self.data_out.success:
+ for s in self.data_out.selectedSurfaces:
+ self.selected_surfaces.append(lqrw.Surface(np.array(s.A), np.array(s.b), np.array(s.vertices)))
+ else:
+ print("Error: No new MIP result available \n")
+ pass
else:
- # Results have been already updated
self.mip_success = self.mip_success_syn
self.mip_iteration = self.mip_iteration_syn
-
+ self.potential_surfaces = self.potential_surfaces_syn
if self.mip_success:
self.selected_surfaces = self.selected_surfaces_syn
- self.all_feet_pos = self.all_feet_pos_syn.copy()
+ self.all_feet_pos = copy.deepcopy(self.all_feet_pos_syn)
+
+ return not self.mip_success
def stop_parallel_loop(self):
- """Stop the infinite loop in the parallel process to properly close the simulation
"""
-
+ 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
deleted file mode 100644
index f19cd9c432320fe05e6e7c3217b689c9d7fd7f5e..0000000000000000000000000000000000000000
--- a/scripts/solo3D/tools/Surface.py
+++ /dev/null
@@ -1,279 +0,0 @@
-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_tools.py b/scripts/solo3D/tools/heightmap_tools.py
index b095a7f41603ee35b3053a68cca346509a688935..5251efd3ddad58874a3e98349609f18e71f47dbc 100644
--- a/scripts/solo3D/tools/heightmap_tools.py
+++ b/scripts/solo3D/tools/heightmap_tools.py
@@ -1,12 +1,11 @@
-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
+import libquadruped_reactive_walking as lqrw
+
+
+COLORS = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']
class MapHeader(ctypes.Structure):
@@ -29,15 +28,13 @@ class Heightmap:
: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))
+ self.z = np.zeros((n_x, n_y))
def save_pickle(self, filename):
filehandler = open(filename, 'wb')
@@ -50,7 +47,7 @@ class Heightmap:
- filename (str) : name of the file saved.
"""
- arr_bytes = self.zv.astype(ctypes.c_double).tobytes()
+ arr_bytes = self.z.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)
@@ -59,6 +56,17 @@ class Heightmap:
f.write(h_bytes)
f.write(arr_bytes)
+ def build_from_fit(self, fit):
+ """
+ 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(2):
+ for j in range(2):
+ self.z[i, j] = fit[0] * self.x[i] + fit[1] * self.y[j] + fit[2]
+
def build(self, affordances):
"""
Build the heightmap and return it
@@ -66,10 +74,11 @@ class Heightmap:
affordances until one is found
:param affordances list of affordances
"""
+ last_z = 0.
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.])
+ p1 = np.array([self.x[i], self.y[j], -1.])
+ p2 = np.array([self.x[i], self.y[j], 10.])
segment = np.array([p1, p2])
fcl_segment = convex(segment, [0, 1, 0])
@@ -83,15 +92,33 @@ class Heightmap:
intersections.append(get_point_intersect_line_triangle(segment, triangle)[2])
if len(intersections) != 0:
- self.zv[i, j] = np.max(np.array(intersections))
+ self.z[i, j] = np.max(np.array(intersections))
+ last_z = self.z[i, j]
+ else:
+ self.z[i, j] = last_z
- def map_index(self, x, y):
+ def plot(self, alpha=1., ax=None):
"""
- Get the i, j indices of a given position in the heightmap
+ Plot the heightmap
"""
- i = np.searchsorted(self.x, x) - 1
- j = np.searchsorted(self.y, y) - 1
- return i, j
+ import matplotlib.pyplot as plt
+
+ if ax is None:
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection="3d")
+ i = 0
+ if alpha != 1.:
+ i = 1
+
+ xv, yv = np.meshgrid(self.x, self.y, sparse=False, indexing='ij')
+ ax.plot_surface(xv, yv, self.z, color=COLORS[i], alpha=alpha)
+
+ ax.set_xlabel("x")
+ ax.set_ylabel("y")
+ ax.set_zlabel("z")
+ ax.set_zlim([np.min(self.z), np.max(self.z) + 1.])
+
+ return ax
def affordance_to_convex(affordance):
diff --git a/scripts/solo3D/tools/pyb_environment_3D.py b/scripts/solo3D/tools/pyb_environment_3D.py
index 2fa072815cc0ec29ce2eb0cb16b955bec5f14236..1c12887cdb5ee904d4e319d732a20948dfec20e2 100644
--- a/scripts/solo3D/tools/pyb_environment_3D.py
+++ b/scripts/solo3D/tools/pyb_environment_3D.py
@@ -1,6 +1,7 @@
import numpy as np
import pybullet as pyb
import pybullet_data
+import os
class PybEnvironment3D():
''' Class to vizualise the 3D environment and foot trajectory and in PyBullet simulation.
@@ -17,7 +18,7 @@ class PybEnvironment3D():
- footTrajectoryGenerator: Foot trajectory class (Bezier version).
"""
self.enable_pyb_GUI = params.enable_pyb_GUI
- self.URDF = params.environment_URDF
+ self.URDF = os.environ["SOLO3D_ENV_DIR"] + params.environment_URDF
self.params = params
# Solo3D python class
@@ -43,12 +44,8 @@ class PybEnvironment3D():
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):
@@ -69,16 +66,16 @@ class PybEnvironment3D():
- 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]),
+ for step in range(len(all_feet_pos[0])):
+ for foot in range(len(all_feet_pos)):
+ if all_feet_pos[foot][step] is None:
+ pyb.resetBasePositionAndOrientation(int(self.sl1m_Ids_target[step, foot]),
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],
+ pyb.resetBasePositionAndOrientation(int(self.sl1m_Ids_target[step, foot]),
+ posObj=all_feet_pos[foot][step],
ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
return 0
diff --git a/scripts/solo3D/tools/readme.txt b/scripts/solo3D/tools/readme.txt
deleted file mode 100644
index d44f77a6b9889316803bae0104380e10b3a4e928..0000000000000000000000000000000000000000
--- a/scripts/solo3D/tools/readme.txt
+++ /dev/null
@@ -1,13 +0,0 @@
-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
index f49704944bf37950d3b8ac59e4ffd6c0edeae352..7abe0152253f0c475d4b71b1d19df4fd9b6421db 100644
--- a/scripts/solo3D/tools/utils.py
+++ b/scripts/solo3D/tools/utils.py
@@ -1,4 +1,6 @@
import numpy as np
+from scipy.spatial import ConvexHull
+
def EulerToQuaternion(roll_pitch_yaw):
roll, pitch, yaw = roll_pitch_yaw
@@ -72,3 +74,191 @@ def quaternionToRPY(quat):
rotateZ = np.arctan2(rotateZa0, rotateZa1)
return np.array([[rotateX], [rotateY], [rotateZ]])
+
+def getAllSurfacesDict_inner(all_surfaces, margin):
+ '''
+ Computes the inner vertices of the given convex surface, with a margin.
+ Args :
+ - all_surfaces : Dictionary containing the surface vertices, normal and name.
+ - margin : (float) margin in m
+ Returns :
+ - New dictionnary with inner vertices
+ '''
+
+ all_names = []
+ surfaces = []
+ for name_surface in all_surfaces :
+ vertices = order(np.array(all_surfaces.get(name_surface)[0]))
+ ineq_inner, ineq_inner_vect, normal = compute_inner_inequalities(vertices, margin)
+ vertices_inner = compute_inner_vertices(vertices, ineq_inner, ineq_inner_vect )
+
+ # Save inner vertices
+ all_names.append(name_surface)
+ surfaces.append((vertices_inner.tolist(), normal.tolist()))
+
+ surfaces_dict = dict(zip(all_names, surfaces))
+ return surfaces_dict
+
+def norm(sq):
+ """ Computes b=norm
+"""
+ cr = np.cross(sq[2] - sq[0], sq[1] - sq[0])
+ return np.abs(cr / np.linalg.norm(cr))
+
+
+def order(vertices, method="convexHull"):
+ """" Order the array of vertice in counterclock wise using convex Hull method
+"""
+ if len(vertices) <= 3:
+ return 0
+ v = np.unique(vertices, axis=0)
+ n = 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)
+ vert = v[h.vertices]
+ else:
+ mean = np.mean(c, axis=0)
+ d = c - mean
+ s = np.arctan2(d[:, 0], d[:, 1])
+ vert = v[np.argsort(s)]
+
+ return vert
+
+
+def compute_inner_inequalities(vertices, margin):
+ """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 ]
+ ... ]]
+ """
+ nb_vert = vertices.shape[0]
+
+ # Computes normal surface
+ S_normal = np.cross(vertices[0, :] - vertices[1, :], vertices[0, :] - vertices[2, :])
+ if S_normal @ np.array([0., 0., 1.]) < 0.: # Check orientation of the normal
+ S_normal = -S_normal
+
+ normal = S_normal / np.linalg.norm(S_normal)
+
+ ineq_inner = np.zeros((nb_vert + 1, 3))
+ ineq_vect_inner = np.zeros((nb_vert + 1))
+
+ ineq_inner[-1, :] = normal
+ ineq_vect_inner[-1] = -(-normal[0] * vertices[0, 0] - normal[1] * vertices[0, 1] - normal[2] * vertices[0, 2])
+
+ for i in range(nb_vert):
+
+ if i < nb_vert - 1:
+ AB = vertices[i, :] - vertices[i + 1, :]
+ else:
+ AB = vertices[i, :] - vertices[0, :] # last point of the list with first
+
+ n_plan = np.cross(AB, 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 = vertices[i, :] + margin * n_plan
+
+ # Create the parallel plan that pass trhough M
+ ineq_inner[i, :] = -np.array([n_plan[0], n_plan[1], n_plan[2]])
+ ineq_vect_inner[i] = -n_plan[0] * M[0] - n_plan[1] * M[1] - n_plan[2] * M[2]
+
+ return ineq_inner, ineq_vect_inner, normal
+
+
+def compute_inner_vertices( vertices, ineq_inner, ineq_vect_inner):
+ """" 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 = vertices.shape[0]
+
+ # P = np.array([a,b,c,d]) , (Plan) ax + by + cz + d = 0
+ P_normal = np.zeros(4)
+ P_normal[:3] = ineq_inner[-1, :]
+ P_normal[-1] = -ineq_vect_inner[-1]
+
+ P1, P2 = np.zeros(4), np.zeros(4)
+
+ for i in range(nb_vert):
+ if i < nb_vert - 1:
+ P1[:3], P2[:3] = ineq_inner[i, :], ineq_inner[i + 1, :]
+ P1[-1], P2[-1] = -ineq_vect_inner[i], -ineq_vect_inner[i + 1]
+
+ A, B = plane_intersect(P1, P2)
+ S_inner.append(LinePlaneCollision(P_normal, A, B))
+ else:
+ P1[:3], P2[:3] = ineq_inner[i, :], ineq_inner[0, :]
+ P1[-1], P2[-1] = -ineq_vect_inner[i], -ineq_vect_inner[0]
+
+ A, B = plane_intersect(P1, P2)
+ S_inner.append(LinePlaneCollision(P_normal, A, B))
+
+ vertices_inner = np.array(S_inner)
+ return vertices_inner
+
+
+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
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index b08efd3cd20a8ef3ca34b0bf89be0220e0055377..22f9a2ab069f94591a9c57ba8b4a53b8658b9a0d 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -111,7 +111,8 @@ void Estimator::initialize(Params& params) {
_1Mi_ = pinocchio::SE3(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0), Vector3(0.1163, 0.0, 0.02));
- q_security_ = (Vector3(M_PI * 0.4, M_PI * 80 / 180, M_PI)).replicate<4, 1>();
+ q_security_ = (Vector3(1.2, 2.1, 3.14)).replicate<4, 1>();
+// q_security_ = (Vector3(M_PI * 0.4, M_PI * 80 / 180, M_PI)).replicate<4, 1>();
q_FK_(6, 0) = 1.0; // Last term of the quaternion
q_filt_(6, 0) = 1.0; // Last term of the quaternion
@@ -138,7 +139,7 @@ void Estimator::initialize(Params& params) {
}
void Estimator::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
- Vector3 const& baseOrientation, VectorN const& dummyPos) {
+ Vector3 const& baseOrientation, VectorN const& q_perfect) {
// Linear acceleration of the trunk (base frame)
IMU_lin_acc_ = baseLinearAcceleration;
@@ -154,7 +155,7 @@ 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_RPY_.tail(1) = q_perfect.tail(1); // Yaw angle from motion capture
}
IMU_ang_pos_ =
@@ -264,7 +265,7 @@ Vector3 Estimator::BaseVelocityFromKinAndIMU(int contactFrameId) {
void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN const& baseLinearAcceleration,
VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q_mes,
- VectorN const& v_mes, VectorN const& dummyPos, Vector3 const& b_baseVel) {
+ VectorN const& v_mes, VectorN const& q_perfect, Vector3 const& b_baseVel_perfect) {
feet_status_ = gait.block(0, 0, 1, 4);
feet_goals_ = goals;
@@ -274,7 +275,7 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
}
// Update IMU data
- get_data_IMU(baseLinearAcceleration, baseAngularVelocity, baseOrientation, dummyPos);
+ get_data_IMU(baseLinearAcceleration, baseAngularVelocity, baseOrientation, q_perfect);
// Angular position of the trunk
Vector4 filt_ang_pos = IMU_ang_pos_.coeffs();
@@ -315,7 +316,7 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
// Base velocity estimated by FK, estimated by motion capture
if (solo3D) {
- FK_lin_vel_ = b_baseVel;
+ FK_lin_vel_ = b_baseVel_perfect;
}
// Rotation matrix to go from base frame to world frame
@@ -345,8 +346,8 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
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));
+ // offset_.tail(3) << -0.0155;
+ filt_lin_pos = filter_xyz_pos_.compute(q_perfect.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));
}
@@ -354,23 +355,23 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
// Output filtered position vector (19 x 1)
q_filt_.head(3) = filt_lin_pos;
if (perfect_estimator || solo3D) {
- q_filt_(2, 0) = dummyPos(2, 0) - 0.0155; // Minus feet radius
+ q_filt_(2, 0) = q_perfect(2, 0); // Minus feet radius
}
q_filt_.block(3, 0, 4, 1) = filt_ang_pos;
q_filt_.tail(12) = actuators_pos_; // Actuators pos are already directly from PyBullet
// Output filtered velocity vector (18 x 1)
// Linear velocities directly from PyBullet if perfect estimator
- v_filt_.head(3) = perfect_estimator ? b_baseVel : b_filt_lin_vel_;
+ v_filt_.head(3) = perfect_estimator ? b_baseVel_perfect : b_filt_lin_vel_;
v_filt_.block(3, 0, 3, 1) = filt_ang_vel; // Angular velocities are already directly from PyBullet
v_filt_.tail(12) = actuators_vel_; // Actuators velocities are already directly from PyBullet
vx_queue_.pop_back();
vy_queue_.pop_back();
vz_queue_.pop_back();
- vx_queue_.push_front(perfect_estimator ? b_baseVel(0) : b_filt_lin_vel_(0));
- vy_queue_.push_front(perfect_estimator ? b_baseVel(1) : b_filt_lin_vel_(1));
- vz_queue_.push_front(perfect_estimator ? b_baseVel(2) : b_filt_lin_vel_(2));
+ vx_queue_.push_front(perfect_estimator ? b_baseVel_perfect(0) : b_filt_lin_vel_(0));
+ vy_queue_.push_front(perfect_estimator ? b_baseVel_perfect(1) : b_filt_lin_vel_(1));
+ vz_queue_.push_front(perfect_estimator ? b_baseVel_perfect(2) : b_filt_lin_vel_(2));
v_filt_bis_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.0) / N_queue_;
v_filt_bis_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.0) / N_queue_;
v_filt_bis_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.0) / N_queue_;
@@ -415,10 +416,13 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
int Estimator::security_check(VectorN const& tau_ff) {
if (((q_filt_.tail(12).cwiseAbs()).array() > q_security_.array()).any()) { // Test position limits
+ std::cout << "Position limit error " << ((q_filt_.tail(12).cwiseAbs()).array() > q_security_.array()).transpose() << std::endl;
return 1;
- } else if (((v_secu_.cwiseAbs()).array() > 50.0).any()) { // Test velocity limits
+ } else if (((v_secu_.cwiseAbs()).array() > 100.0).any()) { // Test velocity limits
+ std::cout << "Velocity limit error " << ((v_secu_.cwiseAbs()).array() > 100.0).transpose() << std::endl;
return 2;
} else if (((tau_ff.cwiseAbs()).array() > 8.0).any()) { // Test feedforward torques limits
+ std::cout << "feedforward limit error " << ((tau_ff.cwiseAbs()).array() > 8.0).transpose() << std::endl;
return 3;
}
return 0;
diff --git a/src/FootTrajectoryGeneratorBezier.cpp b/src/FootTrajectoryGeneratorBezier.cpp
index e9032d24e2f262e35765be6548bb0a6ddad9b21e..9943fa26ff1aca9c4e501b92a3634b99f580cc4f 100644
--- a/src/FootTrajectoryGeneratorBezier.cpp
+++ b/src/FootTrajectoryGeneratorBezier.cpp
@@ -1,9 +1,9 @@
#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()
@@ -30,8 +30,8 @@ FootTrajectoryGeneratorBezier::FootTrajectoryGeneratorBezier()
acceleration_base_(Matrix34::Zero()),
intersectionPoint_(Vector2::Zero()),
ineq_vector_{Vector4::Zero()},
+ useBezier(true),
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;
@@ -55,6 +55,7 @@ void FootTrajectoryGeneratorBezier::initialize(Params& params, Gait& gaitIn, Sur
N_samples_ineq = N_samples_ineq_in;
degree = degree_in;
res_size = dim * (degree + 1 - 6);
+ useBezier = params.use_bezier;
P_ = MatrixN::Zero(res_size, res_size);
q_ = VectorN::Zero(res_size);
@@ -67,10 +68,10 @@ void FootTrajectoryGeneratorBezier::initialize(Params& params, Gait& gaitIn, Sur
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());
+ 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);
@@ -197,14 +198,6 @@ void FootTrajectoryGeneratorBezier::updatePolyCoeff_Z(int const& i_foot, Vector3
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);
@@ -268,7 +261,8 @@ Vector3 FootTrajectoryGeneratorBezier::evaluatePoly(int const& i_foot, int const
return vector;
}
-void FootTrajectoryGeneratorBezier::updateFootPosition(int const& k, int const& i_foot, Vector3 const& targetFootstep) {
+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_;
@@ -278,8 +272,9 @@ void FootTrajectoryGeneratorBezier::updateFootPosition(int const& k, int const&
if (t0 < t1 - lockTime_) {
// compute reference polynoms coefficients
if (t0s[i_foot] < 10e-4 || k == 0) {
+ double height_ = std::max(position_(2, i_foot), targetFootstep[2]);
// Update Z coefficients only at the beginning of the flying phase
- updatePolyCoeff_Z(i_foot, position_.col(i_foot), targetFootstep, t1, h + targetFootstep[2]);
+ updatePolyCoeff_Z(i_foot, position_.col(i_foot), targetFootstep, t1, h + height_);
// Initale velocity and acceleration nulle
updatePolyCoeff_XY(i_foot, position_.col(i_foot), Vector3::Zero(), Vector3::Zero(), targetFootstep, t0, t1);
@@ -297,18 +292,8 @@ void FootTrajectoryGeneratorBezier::updateFootPosition(int const& k, int const&
// 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;
-
+ pastSurface_[i_foot].getHeight(targetFootstep.head(2))) >= 10e-3) {
int nb_vert = newSurface_[i_foot].vertices_.rows();
MatrixN vert = newSurface_[i_foot].vertices_;
@@ -499,12 +484,15 @@ void FootTrajectoryGeneratorBezier::updateFootPosition(int const& k, int const&
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);
+ if (useBezier) {
+ 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);
+ } else {
+ 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);
+ }
}
}
@@ -551,7 +539,57 @@ void FootTrajectoryGeneratorBezier::update(int k, MatrixN const& targetFootstep,
// Update desired position, velocities and accelerations for flying feet
for (int i = 0; i < (int)feet.size(); i++) {
- position_.col(feet[i]) = position_FK_.col(feet[i]);
+ // position_.col(feet[i]) = position_FK_.col(feet[i]);
+ updateFootPosition(k, feet[i], targetFootstep.col(feet[i]));
+ }
+ return;
+}
+
+void FootTrajectoryGeneratorBezier::updateDebug(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];
+ }
+ }
+ }
+
+ // Update feet position using estimated state, by FK
+ // update_position_FK(q);
+
+ // Update desired position, velocities and accelerations for flying feet
+ 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;
@@ -668,23 +706,23 @@ void FootTrajectoryGeneratorBezier::get_intersect_segment(Vector2 a1, Vector2 a2
}
Eigen::MatrixXd FootTrajectoryGeneratorBezier::getFootPositionBaseFrame(const Eigen::Matrix<double, 3, 3>& R,
- const Eigen::Matrix<double, 3, 1>& T) {
+ 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) {
+ 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) {
+ 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
index d55d9c97ea5c41e3253e8a4497a592f2efeafabf..c4f8b00cd10677077e8814662091e3b6eaa06176 100644
--- a/src/FootstepPlannerQP.cpp
+++ b/src/FootstepPlannerQP.cpp
@@ -33,12 +33,14 @@ FootstepPlannerQP::FootstepPlannerQP()
d_{VectorN::Zero(0)},
x{VectorN::Zero(N)},
surfaceStatus_(false),
- surfaceIteration_(0){
+ useSL1M(true),
+ surfaceIteration_(0) {
// Empty
}
void FootstepPlannerQP::initialize(Params& params, Gait& gaitIn, Surface initialSurface_in) {
params_ = ¶ms;
+ useSL1M = params.use_sl1m;
dt = params.dt_mpc;
dt_wbc = params.dt_wbc;
h_ref = params.h_ref;
@@ -74,7 +76,7 @@ void FootstepPlannerQP::initialize(Params& params, Gait& gaitIn, Surface initial
// QP initialization
qp.reset(N, 0, M);
weights_.setZero(N);
- weights_ << 1000., 1000., 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;;
+ weights_ << 1000., 1000., 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)
@@ -100,11 +102,9 @@ void FootstepPlannerQP::initialize(Params& params, Gait& gaitIn, Surface initial
}
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) {
+ Vector6 const& b_vref) {
if (q.rows() != 18) {
- throw std::runtime_error("q should be a vector of size 18 (pos+RPY+mot)");
+ throw std::runtime_error("q should be a vector of size 18 (base position + base RPY + joint)");
}
// Update location of feet in stance phase (for those which just entered stance phase)
@@ -113,20 +113,18 @@ MatrixN FootstepPlannerQP::updateFootsteps(bool refresh, int k, VectorN const& q
}
// Compute location of footsteps
- return computeTargetFootstep(k, q.head(6), b_v, b_vref, potentialSurfaces, surfaces, surfaceStatus,
- surfaceIteration);
+ return computeTargetFootstep(k, q.head(6), b_v, b_vref);
}
-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) {
- // Update intern parameters
+void FootstepPlannerQP::updateSurfaces(SurfaceVectorVector const& potentialSurfaces, SurfaceVector const& surfaces,
+ bool const surfaceStatus, int const surfaceIteration) {
surfaceStatus_ = surfaceStatus;
surfaceIteration_ = surfaceIteration;
surfaces_ = surfaces;
potentialSurfaces_ = potentialSurfaces;
+}
+MatrixN FootstepPlannerQP::computeTargetFootstep(int k, Vector6 const& q, Vector6 const& b_v, Vector6 const& b_vref) {
// Rotation matrix along z axis
RPY_ = q.tail(3);
double c = std::cos(RPY_(2));
@@ -137,8 +135,16 @@ MatrixN FootstepPlannerQP::computeTargetFootstep(int k, Vector6 const& q, Vector
q_tmp = q.head(3);
q_tmp(2) = 0.0;
+ // ! b_vref and b_v corresponds to h_v, velocities in horizontal frame
+ // b_v given in horizontal frame, b_vref given in base frame
+ Vector3 RP_ = RPY_;
+ RP_[2] = 0; // Yaw taken into account later
+ Vector6 h_vref;
+ h_vref.head(3) = pinocchio::rpy::rpyToMatrix(RP_) * b_vref.head(3);
+ h_vref.tail(3) = pinocchio::rpy::rpyToMatrix(RP_) * b_vref.tail(3);
+
// Compute the desired location of footsteps over the prediction horizon
- computeFootsteps(k, b_v, b_vref);
+ computeFootsteps(k, b_v, h_vref);
// Update desired location of footsteps on the ground
updateTargetFootsteps();
@@ -191,8 +197,7 @@ void FootstepPlannerQP::computeFootsteps(int k, Vector6 const& b_v, Vector6 cons
// 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++)
- {
+ 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) {
@@ -235,7 +240,7 @@ void FootstepPlannerQP::computeFootsteps(int k, Vector6 const& b_v, Vector6 cons
{
if (t0s[j] < 10e-4 and k % k_mpc == 0) // Beginning of flying phase
{
- if (surfaceStatus_) {
+ if (surfaceStatus_ && useSL1M) {
selectedSurfaces_[j] = surfaces_[j];
} else {
selectedSurfaces_[j] = selectSurfaceFromPoint(heuristic_fb_, phase, j);
@@ -245,7 +250,7 @@ void FootstepPlannerQP::computeFootsteps(int k, Vector6 const& b_v, Vector6 cons
optimVector_.push_back(optim_data);
} else {
Surface sf_ = Surface();
- if (surfaceStatus_) {
+ if (surfaceStatus_ && useSL1M) {
sf_ = surfaces_[j];
} else {
sf_ = selectSurfaceFromPoint(heuristic_fb_, phase, j);
@@ -274,10 +279,10 @@ void FootstepPlannerQP::computeFootsteps(int k, Vector6 const& b_v, Vector6 cons
// 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].constant_term, id_l, optimVector_[id_l].Rz_tmp);
- foot++;
+ for (uint id_l = 0; id_l < optimVector_.size(); id_l++) {
+ iStart = surfaceInequalities(iStart, optimVector_[id_l].surface, optimVector_[id_l].constant_term, id_l,
+ optimVector_[id_l].Rz_tmp);
+ foot++;
}
status = qp.solve_quadprog(P_, q_, C_, d_, G_, h_, x); // solve QP
@@ -292,7 +297,6 @@ void FootstepPlannerQP::computeFootsteps(int k, Vector6 const& b_v, Vector6 cons
// Offset to the future position
q_dxdy << dx(i - 1, 0), dy(i - 1, 0), 0.0;
-
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);
@@ -308,15 +312,15 @@ void FootstepPlannerQP::computeFootsteps(int k, Vector6 const& b_v, Vector6 cons
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);
+ b_footsteps_[i].col(foot) = b_footsteps_[i - 1].col(foot);
}
}
}
}
-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
+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
@@ -325,8 +329,8 @@ void FootstepPlannerQP::computeNextFootstep(int i, int j, Vector6 const& b_v, Ve
// Add feedback term
footstep += params_->k_feedback * b_v.head(3);
- if (feedback_term){
- footstep += - params_->k_feedback * b_vref.head(3);
+ if (feedback_term) {
+ footstep += -params_->k_feedback * b_vref.head(3);
}
// Add centrifugal term
@@ -363,7 +367,8 @@ void FootstepPlannerQP::updateTargetFootsteps() {
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_.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
@@ -379,7 +384,7 @@ void FootstepPlannerQP::updateNewContact(Vector18 const& q) {
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
+ currentFootstep_(2, i) = footsteps_[1](2, i); // Z from the height map
}
}
}
@@ -419,11 +424,10 @@ Surface FootstepPlannerQP::selectSurfaceFromPoint(Vector3 const& point, int phas
double sfHeight = 0.;
bool surfaceFound = false;
- Surface sf;
+ Surface sf = initialSurface_;
- if (surfaceIteration_) {
+ if (surfaceIteration_ > 0) {
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));
@@ -434,37 +438,41 @@ Surface FootstepPlannerQP::selectSurfaceFromPoint(Vector3 const& point, int phas
}
}
}
+ }
- 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
+ // The vertices has been ordered previously counter-clock wise, using qHull methods.
+ // We could use hpp_fcl to compute this distance.
+ Pair A = {0., 0.};
+ Pair B = {0., 0.};
+ Pair E = {point(0), point(1)};
+ double distance = 100.0;
+ double distance_tmp = 0.;
+ if (not surfaceFound) {
+ if (surfaceIteration_ > 0) {
+ SurfaceVector potentialSurfaces = potentialSurfaces_[moving_foot_index];
+ for (uint i = 0; i < potentialSurfaces.size(); i++) {
+ MatrixN vertices = potentialSurfaces[i].getVertices();
+ for (uint j = 0; j < vertices.rows(); j++) {
+ A.F = vertices(j, 0);
+ A.S = vertices(j, 1);
+ B.F = vertices((j + 1) % vertices.rows(), 0);
+ B.S = vertices((j + 1) % vertices.rows(), 1);
+ distance_tmp = minDistance(A, B, E);
+ if (distance_tmp < distance) {
+ distance = distance_tmp;
+ sf = potentialSurfaces[i];
+ }
+ }
+ }
}
- } else {
- sf = initialSurface_;
}
return sf;
}
-int FootstepPlannerQP::surfaceInequalities(int i_start, Surface const& surface, Vector3 const& next_ft, int id_l, Matrix3 Rz_tmp) {
+int FootstepPlannerQP::surfaceInequalities(int i_start, Surface const& surface, Vector3 const& next_ft, int id_l,
+ Matrix3 Rz_tmp) {
int n_rows = int(surface.getA().rows());
- MatrixN mat_tmp = MatrixN::Zero(n_rows,3);
+ MatrixN mat_tmp = MatrixN::Zero(n_rows, 3);
mat_tmp = surface.getA() * Rz * Rz_tmp;
G_.block(i_start, 0, n_rows, 2) = params_->k_feedback * mat_tmp.block(0, 0, n_rows, 2);
G_.block(i_start, 2 + 3 * id_l, n_rows, 3) = -surface.getA();
@@ -486,3 +494,60 @@ MatrixN FootstepPlannerQP::vectorToMatrix(std::vector<Matrix34> const& array) {
}
return M;
}
+
+// Function to return the minimum distance
+// between a line segment AB and a point E
+// https://www.geeksforgeeks.org/minimum-distance-from-a-point-to-the-line-segment-using-vectors/
+double FootstepPlannerQP::minDistance(Pair const& A, Pair const& B, Pair const& E) {
+ // vector AB
+ Pair AB = {B.F - A.F, B.S - A.S};
+ // AB.F = B.F - A.F;
+ // AB.S = B.S - A.S;
+
+ // vector BP
+ Pair BE = {E.F - B.F, E.S - B.S};
+ // BE.F = E.F - B.F;
+ // BE.S = E.S - B.S;
+
+ // vector AP
+ Pair AE = {E.F - A.F, E.S - A.S};
+ // AE.F = E.F - A.F, AE.S = E.S - A.S;
+
+ // Variables to store dot product
+ double AB_BE, AB_AE;
+
+ // Calculating the dot product
+ AB_BE = (AB.F * BE.F + AB.S * BE.S);
+ AB_AE = (AB.F * AE.F + AB.S * AE.S);
+
+ // Minimum distance from
+ // point E to the line segment
+ double reqAns = 0;
+
+ // Case 1
+ if (AB_BE > 0) {
+ // Finding the magnitude
+ double y = E.S - B.S;
+ double x = E.F - B.F;
+ reqAns = sqrt(x * x + y * y);
+ }
+
+ // Case 2
+ else if (AB_AE < 0) {
+ double y = E.S - A.S;
+ double x = E.F - A.F;
+ reqAns = sqrt(x * x + y * y);
+ }
+
+ // Case 3
+ else {
+ // Finding the perpendicular distance
+ double x1 = AB.F;
+ double y1 = AB.S;
+ double x2 = AE.F;
+ double y2 = AE.S;
+ double mod = sqrt(x1 * x1 + y1 * y1);
+ reqAns = abs(x1 * y2 - y1 * x2) / mod;
+ }
+ return reqAns;
+}
diff --git a/src/Heightmap.cpp b/src/Heightmap.cpp
index 1a8bbea5a754b6fa74eefd9687d018eee1b78c63..2ebb340afc9a17d125e9d3881b0bfcf15f40c0c1 100644
--- a/src/Heightmap.cpp
+++ b/src/Heightmap.cpp
@@ -1,93 +1,76 @@
#include "qrw/Heightmap.hpp"
-Heightmap::Heightmap() {
+Heightmap::Heightmap()
+ : z_(),
+ fit_(VectorN::Zero(3)),
+ p(),
+ A_(MatrixN::Ones(p.heightmap_fit_size * p.heightmap_fit_size, 3)),
+ b_(VectorN::Zero(p.heightmap_fit_size * p.heightmap_fit_size)) {
// empty
}
-void Heightmap::initialize(const std::string& file_name) {
+void Heightmap::initialize(const std::string &fileName) {
// Open the binary file
- std::ifstream iF(file_name, std::ios::in | std::ios::out | std::ios::binary);
- if (!iF) {
+ std::ifstream file(fileName, std::ios::in | std::ios::out | std::ios::binary);
+ if (!file) {
throw std::runtime_error("Error while opening heighmap binary file");
}
// Extract header from binary file
- iF.read(reinterpret_cast<char*>(&header_), sizeof header_);
+ file.read(reinterpret_cast<char *>(&map_), sizeof map_);
// 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);
+ z_ = MatrixN::Zero(map_.size_x, map_.size_y);
- 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.3;
- FIT_SIZE_Y = 0.3;
- FIT_NX = 16;
- FIT_NY = 6;
+ dx_ = std::abs((map_.x_init - map_.x_end) / (map_.size_x - 1));
+ dy_ = std::abs((map_.y_init - map_.y_end) / (map_.size_y - 1));
+ // Read the file and extract heightmap matrix
int i = 0;
int j = 0;
double read;
- // Read the file and extract heightmap matrix
- while (i < header_.size_x && !iF.eof()) {
+ while (i < map_.size_x && !file.eof()) {
j = 0;
- while (j < header_.size_y && !iF.eof()) {
- iF.read(reinterpret_cast<char*>(&read), sizeof read);
+ while (j < map_.size_y && !file.eof()) {
+ file.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::xIndex(double x) {
+ return (x < map_.x_init || x > map_.x_end) ? -1 : (int)std::round((x - map_.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_);
- }
+int Heightmap::yIndex(double y) {
+ return (y < map_.y_init || y > map_.y_end) ? -1 : (int)std::round((y - map_.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);
- }
+double Heightmap::getHeight(double x, double y) {
+ int iX = xIndex(x);
+ int iY = yIndex(y);
+ return (iX == -1 || iY == -1) ? 0. : z_(iX, iY);
}
-Vector3 Heightmap::compute_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);
+Vector3 Heightmap::fitSurface_(double x, double y) {
+ VectorN xVector = VectorN::LinSpaced(p.heightmap_fit_size, x - p.heightmap_fit_length, x + p.heightmap_fit_length);
+ VectorN yVector = VectorN::LinSpaced(p.heightmap_fit_size, y - p.heightmap_fit_length, y + p.heightmap_fit_length);
- int i_pb = 0;
- for (int i = 0; i < FIT_NX; i++) {
- for (int j = 0; j < FIT_NY; j++) {
- A.block(i_pb, 0, 1, 3) << x_surface[i],y_surface[j], 1.0;
- b.block(i_pb, 0, 1, 1) << get_height(x_surface[i],y_surface[j]);
- i_pb += 1;
+ int index = 0;
+ for (int i = 0; i < p.heightmap_fit_size; i++) {
+ for (int j = 0; j < p.heightmap_fit_size; j++) {
+ A_.block(index, 0, 1, 2) << xVector[i], yVector[j];
+ b_(index) = getHeight(xVector[i], yVector[j]);
+ index++;
}
}
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);
- return surface_eq;
-}
+ P_ = A_.transpose() * A_;
+ q_ = -A_.transpose() * b_;
+ status = qp.solve_quadprog(P_, q_, C_, d_, G_, h_, fit_);
+
+ return fit_;
+}
\ No newline at end of file
diff --git a/src/Joystick.cpp b/src/Joystick.cpp
index 32912237af7bd6d75f2e529e445173e595a55d3c..d29885ebdca28953a6bd07758b542a655600db82 100644
--- a/src/Joystick.cpp
+++ b/src/Joystick.cpp
@@ -30,29 +30,14 @@ void Joystick::initialize(Params& params) {
}
}
-VectorN Joystick::handle_v_switch_py(double k, VectorN const& k_switch_py, MatrixN const& v_switch_py) {
- int i = 1;
- while ((i < k_switch_py.rows()) && k_switch_py[i] <= k) {
- i++;
- }
- if (i != k_switch_py.rows()) {
- double ev = k - k_switch_py[i - 1];
- double t1 = k_switch_py[i] - k_switch_py[i - 1];
- A3_ = 2 * (v_switch_py.col(i - 1) - v_switch_py.col(i)) / pow(t1, 3);
- A2_ = (-3.0 / 2.0) * t1 * A3_;
- v_ref_ = v_switch_py.col(i - 1) + A2_ * pow(ev, 2) + A3_ * pow(ev, 3);
- }
- return v_ref_;
-}
-
void Joystick::handle_v_switch(int k) {
int i = 1;
- while ((i < k_switch.cols()) && k_switch(0, i) <= k) {
+ while (i < k_switch.size() && k_switch(i) <= k) {
i++;
}
- if (i != k_switch.cols()) {
- double ev = k - k_switch(0, i - 1);
- double t1 = k_switch(0, i) - k_switch(0, i - 1);
+ if (i != k_switch.size()) {
+ double ev = k - k_switch(i - 1);
+ double t1 = k_switch(i) - k_switch(i - 1);
A3_ = 2 * (v_switch.col(i - 1) - v_switch.col(i)) / pow(t1, 3);
A2_ = (-3.0 / 2.0) * t1 * A3_;
v_ref_ = v_switch.col(i - 1) + A2_ * pow(ev, 2) + A3_ * pow(ev, 3);
@@ -116,45 +101,6 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static) {
gamepad.w_yaw = -event.value / 32767.0;
}
}
- // printf("Start:%d Stop:%d Vx:%f \tVy:%f
- // \tWyaw:%f\n",gamepad.start,gamepad.select,gamepad.v_x,gamepad.v_y,gamepad.w_yaw);
-
- /*if (k < 2000)
- {
- int a = 1;
- }
- else if (k < 4000)
- {
- gamepad.v_x = 0.4;
- gamepad.v_y = 0.4;
- }
- else if (k < 7000)
- {
- gamepad.v_x = 0.0;
- gamepad.v_y = 0.0;
- }
- else if (k < 10000)
- {
- gamepad.L1 = 1;
- gamepad.v_x = std::sin(2 * M_PI * (k - 7000) / 3000);
- gamepad.v_y = std::sin(2 * M_PI * (k - 7000) / 3000);
- gamepad.v_z = 0.7 * std::sin(2 * M_PI * (k - 7000) / 3000);
- gamepad.w_yaw = 0.7 * std::sin(2 * M_PI * (k - 7000) / 3000);
- }
- else if (k < 20000)
- {
- gamepad.L1 = 0;
- gamepad.v_x = 0.0;
- gamepad.v_y = 0.5;
- gamepad.v_z = 0.0;
- gamepad.w_yaw = -0.2;
- }
- else if (k < 22000)
- {
- gamepad.v_x = 0.0;
- gamepad.v_y = 0.0;
- gamepad.w_yaw = 0.0;
- }*/
// Remember when L1 was pressed for the last time
if (gamepad.L1 == 1) {
@@ -192,12 +138,6 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static) {
// Joystick code
joystick_code_ = 0;
- /*
- if (gamepad.cross == 1) {joystick_code_ = 1;}
- else if (gamepad.circle == 1) {joystick_code_ = 2;}
- else if (gamepad.triangle == 1) {joystick_code_ = 3;}
- else if (gamepad.square == 1) {joystick_code_ = 4;}
- */
if (params_->DEMONSTRATION) {
if (!getL1() && (k % k_mpc == 0) && (k > static_cast<int>(std::round(1.0 / params_->dt_wbc)))) {
@@ -212,7 +152,7 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static) {
lock_gp = true;
lock_time_static_ = std::chrono::system_clock::now();
} else if (switch_static &&
- (std::abs(v_gp_(0, 0)) > v_up || std::abs(v_gp_(1, 0)) > v_up || std::abs(v_gp_(5, 0)) > v_up)) {
+ (std::abs(v_gp_(0, 0)) > v_up || std::abs(v_gp_(1, 0)) > v_up || std::abs(v_gp_(5, 0)) > v_up)) {
switch_static = false;
lock_gp = true;
lock_time_static_ = std::chrono::system_clock::now();
@@ -229,7 +169,8 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static) {
// Lock gamepad value during switching or after L1 is pressed
if ((lock_gp && ((std::chrono::duration<double>)(std::chrono::system_clock::now() - lock_time_static_)).count() <
lock_duration_) ||
- (((std::chrono::duration<double>)(std::chrono::system_clock::now() - lock_time_L1_)).count() < lock_duration_)) {
+ (((std::chrono::duration<double>)(std::chrono::system_clock::now() - lock_time_L1_)).count() <
+ lock_duration_)) {
gp_alpha_vel = 0.0;
gp_alpha_pos = params_->gp_alpha_pos;
} else if (lock_gp) {
@@ -257,50 +198,8 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static) {
void Joystick::update_v_ref_predefined(int k, int velID) {
// Initialization of velocity profile during first call
if (k == 0) {
- MatrixN t_switch;
- switch (velID) {
- case 0:
- t_switch = MatrixN::Zero(1, 2);
- t_switch << 0, 1;
- v_switch = MatrixN::Zero(6, 2);
- break;
- case 6:
- t_switch = MatrixN::Zero(1, 9);
- t_switch << 0, 1, 2, 3, 4, 6, 7, 8, 9;
- v_switch = MatrixN::Zero(6, 9);
- v_switch.row(0) << 0.0, 0.0, 0.25, 0.5, 0.8, 0.8, 0.5, 0.25, 0.0;
- break;
- case 7:
- t_switch = MatrixN::Zero(1, 9);
- t_switch << 0, 1, 3, 4, 6, 7, 9, 10, 11;
- v_switch = MatrixN::Zero(6, 9);
- v_switch.row(0) << 0.0, 0.25, 0.5, 0.5, 0.5, 0.5, 0.5, 0.25, 0.0;
- v_switch.row(5) << 0.0, 0.8, -0.8, -0.8, 0.8, 0.8, -0.8, 0.0, 0.0;
- break;
- case 8:
- t_switch = MatrixN::Zero(1, 9);
- t_switch << 0, 1, 3, 4, 6, 7, 9, 10, 11;
- v_switch = MatrixN::Zero(6, 9);
- v_switch.row(0) << 0.0, 0.4, 0.8, 0.8, 0.4, 0.0, 0.0, 0.0, 0.0;
- v_switch.row(5) << 0.0, 0.8, -1.2, 0.8, 0.0, 0.0, 0.0, 0.0, 0.0;
- break;
- case 9:
- t_switch = MatrixN::Zero(1, 9);
- t_switch << 0, 2.5, 6.5, 9;
- v_switch = MatrixN::Zero(6, 9);
- v_switch.row(0) << 0.0, 0.6, 0.6, 0.0;
- v_switch.row(5) << 0.0, 0.4, 0.4, 0.0;
- break;
- case 10:
- t_switch = MatrixN::Zero(1, 7);
- t_switch << 0, 2, 4, 6, 8, 10, 15;
- v_switch = MatrixN::Zero(6, 7);
- v_switch.row(0) << 0.0, 0.4, 0.8, 1.0, 1.0, 1.0, 1.0;
- break;
- default:
- throw std::runtime_error("Unknown velocity ID for the polynomial interpolation.");
- }
- k_switch = (t_switch / dt_wbc).cast<int>();
+ v_switch = params_->v_switch;
+ k_switch = (params_->t_switch / dt_wbc).cast<int>();
}
handle_v_switch(k); // Polynomial interpolation to generate the velocity profile
}
diff --git a/src/MPC.cpp b/src/MPC.cpp
index f89ae53f43de6d4c455e5b4386c577ee0767fd19..92963183e6c189f0488f05db0d205317cd2b295b 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -475,6 +475,16 @@ int MPC::update_NK() {
return 0;
}
+float MPC::retrieve_cost() {
+ // Cost function is x^T P x + q^T x
+ // Here P is a diagonal matrix and q = 0
+ float cost = 0.0;
+ for (int i = 0; i < 2 * 12 * n_steps; i++) {
+ cost += (workspce->solution->x)[i] * P->x[i] * (workspce->solution->x)[i];
+ }
+ return cost;
+}
+
int MPC::call_solver(int k) {
// Initial guess for forces (mass evenly supported by all legs in contact)
warmxf.block(0, 0, 12 * (n_steps - 1), 1) = x.block(12, 0, 12 * (n_steps - 1), 1);
diff --git a/src/Params.cpp b/src/Params.cpp
index 37b720725b526413c2bedc44d852295a6f4c6af6..4bd9bae1470d3176c4b22d0ec6326704a09a55db 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -31,6 +31,8 @@ Params::Params()
gp_alpha_vel(0.0),
gp_alpha_pos(0.0),
+ t_switch_vec(1, 0.0), // Fill with zeros, will be filled with values later
+ v_switch_vec(6, 0.0), // Fill with zeros, will be filled with values later
fc_v_esti(0.0),
@@ -158,6 +160,14 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "gp_alpha_pos");
gp_alpha_pos = robot_node["gp_alpha_pos"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "t_switch");
+ t_switch_vec = robot_node["t_switch"].as<std::vector<double> >();
+ convert_t_switch();
+
+ assert_yaml_parsing(robot_node, "robot", "v_switch");
+ v_switch_vec = robot_node["v_switch"].as<std::vector<double> >();
+ convert_v_switch();
+
assert_yaml_parsing(robot_node, "robot", "fc_v_esti");
fc_v_esti = robot_node["fc_v_esti"].as<double>();
@@ -226,6 +236,27 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "environment_heightmap");
environment_heightmap = robot_node["environment_heightmap"].as<std::string>();
+
+ assert_yaml_parsing(robot_node, "robot", "heightmap_fit_length");
+ heightmap_fit_length = robot_node["heightmap_fit_length"].as<double>();
+
+ assert_yaml_parsing(robot_node, "robot", "heightmap_fit_size");
+ heightmap_fit_size = robot_node["heightmap_fit_size"].as<int>();
+
+ assert_yaml_parsing(robot_node, "robot", "number_steps");
+ number_steps = robot_node["number_steps"].as<int>();
+
+ assert_yaml_parsing(robot_node, "robot", "max_velocity");
+ max_velocity = robot_node["max_velocity"].as<std::vector<double> >();
+
+ assert_yaml_parsing(robot_node, "robot", "use_bezier");
+ use_bezier = robot_node["use_bezier"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "use_sl1m");
+ use_sl1m = robot_node["use_sl1m"].as<bool>();
+
+ assert_yaml_parsing(robot_node, "robot", "use_heuristic");
+ use_sl1m = robot_node["use_heuristic"].as<bool>();
}
void Params::convert_gait_vec() {
@@ -247,7 +278,7 @@ void Params::convert_gait_vec() {
T_gait = N_gait * dt_mpc;
// Resize gait matrix
- gait = Eigen::MatrixXd::Zero(N_gait * N_periods, 4);
+ gait = MatrixN::Zero(N_gait * N_periods, 4);
// Fill gait matrix
int k = 0;
@@ -263,3 +294,39 @@ void Params::convert_gait_vec() {
gait.block(i * N_gait, 0, N_gait, 4) = gait.block(0, 0, N_gait, 4);
}
}
+
+void Params::convert_t_switch() {
+ // Resize t_switch matrix
+ t_switch = VectorN::Zero(t_switch_vec.size());
+
+ // Fill t_switch matrix
+ for (uint i = 0; i < t_switch_vec.size(); i++) {
+ t_switch(i) = t_switch_vec[i];
+ }
+}
+
+void Params::convert_v_switch() {
+ if (v_switch_vec.size() % 6 != 0) {
+ throw std::runtime_error(
+ "v_switch matrix in yaml is not in the correct format. It should have six "
+ "lines, containing the values switch values for each coordinate of the velocity.");
+ }
+
+ if (v_switch_vec.size() / 6 != t_switch_vec.size()) {
+ throw std::runtime_error(
+ "v_switch matrix in yaml is not in the correct format. the same number of colums as t_switch.");
+ }
+
+ int n_col = v_switch_vec.size() / 6;
+
+ // Resize v_switch matrix
+ v_switch = MatrixN::Zero(6, n_col);
+
+ // Fill v_switch matrix
+ for (uint i = 0; i < 6; i++) {
+ for (uint j = 0; j < n_col; j++) {
+ v_switch(i, j) = v_switch_vec[n_col * i + j];
+ }
+ }
+}
+
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 8c1b2ba34c2d08fa6d5ecf938d381297cdbb810f..4ea574fde7c03be41044e27c0bf008029f42c6e3 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -700,11 +700,11 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// Compute the inverse dynamics, aka the joint torques according to the current state of the system,
// the desired joint accelerations and the external forces, using the Recursive Newton Euler Algorithm.
// Result is stored in data_.tau
- // pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_cmd_);
- // Vector12 f_compensation = Vector12::Zero();
+ pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_cmd_);
+ Vector12 f_compensation = Vector12::Zero();
// FORCE COMPENSATION TERM
- Vector18 ddq_test = Vector18::Zero();
+ /*Vector18 ddq_test = Vector18::Zero();
ddq_test.head(6) = ddq_cmd_.head(6);
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_test);
Vector6 RNEA_without_joints = data_.tau.head(6);
@@ -712,7 +712,7 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
Vector6 RNEA_NLE = data_.tau.head(6);
RNEA_NLE(2, 0) -= 9.81 * data_.mass[0];
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_cmd_);
- Vector12 f_compensation = pseudoInverse(Jc_.transpose()) * (data_.tau.head(6) - RNEA_without_joints + RNEA_NLE);
+ Vector12 f_compensation = pseudoInverse(Jc_.transpose()) * (data_.tau.head(6) - RNEA_without_joints + RNEA_NLE);*/
/*std::cout << "M inertia" << std::endl;
std::cout << data_.M << std::endl;*/
@@ -737,15 +737,15 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// std::cout << "M : " << std::endl << data_.M.block(0, 0, 3, 18) << std::endl;
// std::cout << "ddq: " << std::endl << ddq_cmd_.transpose() << std::endl;
-
+
/*
std::cout << "-- BEFORE QP PROBLEM --" << std::endl;
std::cout << "M ddq_u: " << std::endl << (data_.M.block(0, 0, 3, 6) * ddq_cmd_.head(6)).transpose() << std::endl;
- std::cout << "M ddq_a: " << std::endl << (data_.M.block(0, 6, 3, 12) * ddq_cmd_.tail(12)).transpose() << std::endl;
+ std::cout << "M ddq_a: " << std::endl << (data_.M.block(0, 6, 3, 12) * ddq_cmd_.tail(12)).transpose() << std::endl;
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
std::cout << "Non linear effects: " << std::endl << data_.tau.head(6).transpose() << std::endl;
- std::cout << "JcT f_cmd + f_comp: " << std::endl << (Jc_.transpose() * (f_cmd + f_compensation)).transpose() << std::endl;
- std::cout << "JcT f_comp: " << std::endl << (Jc_.transpose() * (f_compensation)).transpose() << std::endl;
+ std::cout << "JcT f_cmd + f_comp: " << std::endl << (Jc_.transpose() * (f_cmd + f_compensation)).transpose() <<
+ std::endl; std::cout << "JcT f_comp: " << std::endl << (Jc_.transpose() * (f_compensation)).transpose() << std::endl;
*/
// Solve the QP problem
@@ -760,15 +760,15 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// DEBUG INERTIA AND NON LINEAR EFFECTS
Vector6 left = data_.M.block(0, 0, 6, 6) * box_qp_->get_ddq_res() - Jc_.transpose() * box_qp_->get_f_res();
- Vector6 right = - data_.tau.head(6) + Jc_.transpose() * (f_cmd + f_compensation);
+ Vector6 right = -data_.tau.head(6) + Jc_.transpose() * (f_cmd + f_compensation);
Vector6 tmp_RNEA = data_.tau.head(6);
- //std::cout << "RNEA: " << std::endl << data_.tau.head(6).transpose() << std::endl;
- //std::cout << "left: " << std::endl << left.transpose() << std::endl;
- //std::cout << "right: " << std::endl << right.transpose() << std::endl;
- //std::cout << "M: " << std::endl << data_.M.block(0, 0, 6, 6) << std::endl;
- //std::cout << "JcT: " << std::endl << Jc_.transpose() << std::endl;
- //std::cout << "M: " << std::endl << data_.M.block(0, 0, 3, 18) << std::endl;
+ // std::cout << "RNEA: " << std::endl << data_.tau.head(6).transpose() << std::endl;
+ // std::cout << "left: " << std::endl << left.transpose() << std::endl;
+ // std::cout << "right: " << std::endl << right.transpose() << std::endl;
+ // std::cout << "M: " << std::endl << data_.M.block(0, 0, 6, 6) << std::endl;
+ // std::cout << "JcT: " << std::endl << Jc_.transpose() << std::endl;
+ // std::cout << "M: " << std::endl << data_.M.block(0, 0, 3, 18) << std::endl;
/*
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
Vector6 tmp_NLE = data_.tau.head(6);
@@ -839,13 +839,14 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
/*
std::cout << "-- AFTER QP PROBLEM --" << std::endl;
- std::cout << "M ddq_u: " << std::endl << (data_.M.block(0, 0, 3, 6) * ddq_with_delta_.head(6)).transpose() << std::endl;
- std::cout << "M ddq_a: " << std::endl << (data_.M.block(0, 6, 3, 12) * ddq_with_delta_.tail(12)).transpose() << std::endl;
- pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
- std::cout << "Non linear effects: " << std::endl << data_.tau.head(6).transpose() << std::endl;
- std::cout << "JcT f_cmd: " << std::endl << (Jc_.transpose() * f_with_delta_).transpose() << std::endl;
-
- std::cout << "LEFT " << (tmp_RNEA.head(3) + data_.M.block(0, 0, 3, 6) * box_qp_->get_ddq_res()).transpose() << std::endl;
+ std::cout << "M ddq_u: " << std::endl << (data_.M.block(0, 0, 3, 6) * ddq_with_delta_.head(6)).transpose() <<
+ std::endl; std::cout << "M ddq_a: " << std::endl << (data_.M.block(0, 6, 3, 12) *
+ ddq_with_delta_.tail(12)).transpose() << std::endl; pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_,
+ VectorN::Zero(model_.nv)); std::cout << "Non linear effects: " << std::endl << data_.tau.head(6).transpose() <<
+ std::endl; std::cout << "JcT f_cmd: " << std::endl << (Jc_.transpose() * f_with_delta_).transpose() << std::endl;
+
+ std::cout << "LEFT " << (tmp_RNEA.head(3) + data_.M.block(0, 0, 3, 6) * box_qp_->get_ddq_res()).transpose() <<
+ std::endl;
*/
// Increment log counter
diff --git a/src/StatePlanner3D.cpp b/src/StatePlanner3D.cpp
index 7ab6700f9e9bbb005e235d83b24b2ba727b898f1..ef1cc62f6c528695b45e3efdb1a43b6bf41d6f31 100644
--- a/src/StatePlanner3D.cpp
+++ b/src/StatePlanner3D.cpp
@@ -1,93 +1,108 @@
#include "qrw/StatePlanner3D.hpp"
-StatePlanner3D::StatePlanner3D() : dt_(0.0), h_ref_(0.0), n_steps_(0), RPY_(Vector3::Zero()) {
+StatePlanner3D::StatePlanner3D()
+ : nStates_(0),
+ dt_(0.),
+ referenceHeight_(0.),
+ maxVelocity_(),
+ rpy_(Vector3::Zero()),
+ Rz_(Matrix3::Zero()),
+ DxDy_(Vector3::Zero()),
+ referenceStates_(),
+ dtVector_(),
+ heightmap_(),
+ rpyMap_(Vector3::Zero()),
+ fit_(Vector3::Zero()),
+ nSteps_(0),
+ stepDuration_(0.),
+ configs_(),
+ config_(Vector7::Zero()),
+ rpyConfig_(Vector3::Zero()) {
// Empty
}
void StatePlanner3D::initialize(Params& params) {
+ nStates_ = static_cast<int>(params.gait.rows());
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);
- Rz = Matrix3::Zero();
- q_dxdy = Vector3::Zero();
- mean_surface = Vector3::Zero();
+ referenceHeight_ = params.h_ref;
+ maxVelocity_ = params.max_velocity;
+ referenceStates_ = MatrixN::Zero(12, 1 + nStates_);
+ dtVector_ = VectorN::LinSpaced(nStates_, dt_, static_cast<double>(nStates_) * dt_);
+ heightmap_.initialize(std::getenv("SOLO3D_ENV_DIR") + params.environment_heightmap);
+ nSteps_ = params.number_steps;
+ stepDuration_ = params.T_gait / 2;
+ configs_ = MatrixN::Zero(7, params.number_steps);
}
-void StatePlanner3D::computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, int is_new_step) {
+void StatePlanner3D::updateSurface(VectorN const& q, Vector6 const& vRef) {
+ fit_ = heightmap_.fitSurface_(q(0), q(1)); // Update surface equality before new step
+ rpyMap_(0) = std::atan2(fit_(1), 1.);
+ rpyMap_(1) = -std::atan2(fit_(0), 1.);
+
+ computeConfigurations(q, vRef);
+}
+
+void StatePlanner3D::computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vRef) {
if (q.rows() != 6) {
- throw std::runtime_error("q should be a vector of size 6");
- }
- if (is_new_step) {
- mean_surface = heightmap_.compute_mean_surface(q(0), q(1)); // Update surface equality before new step
- rpy_map(0) = -std::atan2(mean_surface(1), 1.);
- rpy_map(1) = -std::atan2(mean_surface(0), 1.);
- compute_configurations(q, vref);
+ throw std::runtime_error("StatePlanner3D::computeReferenceStates: q should be a vector of size 6");
}
- RPY_ = q.tail(3);
- double c = std::cos(RPY_(2));
- double s = std::sin(RPY_(2));
- Rz.topLeftCorner<2, 2>() << c, -s, s, c;
+ rpy_ = q.tail(3);
+ double c = std::cos(rpy_(2));
+ double s = std::sin(rpy_(2));
+ Rz_.topLeftCorner<2, 2>() << c, -s, s, c;
// 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_.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) {
+ for (int i = 0; i < nStates_; 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);
+ (vRef(0) * std::sin(vRef(5) * dtVector_(i)) + vRef(1) * (std::cos(vRef(5) * dtVector_(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);
+ (vRef(1) * std::sin(vRef(5) * dtVector_(i)) - vRef(0) * (std::cos(vRef(5) * dtVector_(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) = vRef(0) * dtVector_(i);
+ referenceStates_(1, 1 + i) = vRef(1) * dtVector_(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_(5, 1 + i) = vRef(5) * dtVector_(i);
referenceStates_(6, 1 + i) =
- vref(0) * std::cos(referenceStates_(5, 1 + i)) - vref(1) * std::sin(referenceStates_(5, 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));
+ 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);
+ referenceStates_(11, 1 + i) = vRef(5);
// Update according to heightmap
- q_dxdy(0) = referenceStates_(0, i + 1);
- q_dxdy(1) = referenceStates_(1, i + 1);
- q_dxdy = Rz * q_dxdy + q.head(3); // world frame
+ DxDy_(0) = referenceStates_(0, i + 1);
+ DxDy_(1) = referenceStates_(1, i + 1);
+ DxDy_ = Rz_ * DxDy_ + q.head(3); // world frame
- referenceStates_(2, 1 + i) = mean_surface(0) * q_dxdy(0) + mean_surface(1) * q_dxdy(1) + mean_surface(2) + h_ref_;
+ referenceStates_(2, 1 + i) = fit_(0) * DxDy_(0) + fit_(1) * DxDy_(1) + fit_(2) + referenceHeight_;
- 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]);
+ referenceStates_(3, 1 + i) = 1.0 * (rpyMap_[0] * std::cos(-rpy_[2]) - rpyMap_[1] * std::sin(-rpy_[2]));
+ referenceStates_(4, 1 + i) = 1.0 * (rpyMap_[0] * std::sin(-rpy_[2]) + rpyMap_[1] * std::cos(-rpy_[2]));
}
// Update velocities according to heightmap
- for (int i = 0; i < n_steps_; i++) {
+ for (int i = 0; i < nStates_; i++) {
if (i == 0) {
- referenceStates_(8, 1 + i) = std::max(std::min((referenceStates_(2, 1) - q[2]) / dt_, v_max_z), -v_max_z);
- referenceStates_(9, 1 + i) = std::max(std::min((referenceStates_(3, 1) - RPY_[0]) / dt_, v_max), -v_max);
- referenceStates_(10, 1 + i) = std::max(std::min((referenceStates_(4, 1) - RPY_[1]) / dt_, v_max), -v_max);
+ referenceStates_(8, 1 + i) =
+ std::max(std::min((referenceStates_(2, 1) - q[2]) / dt_, maxVelocity_[2]), -maxVelocity_[2]);
+ referenceStates_(9, 1 + i) =
+ std::max(std::min((referenceStates_(3, 1) - rpy_[0]) / dt_, maxVelocity_[0]), -maxVelocity_[0]);
+ referenceStates_(10, 1 + i) =
+ std::max(std::min((referenceStates_(4, 1) - rpy_[1]) / dt_, maxVelocity_[1]), -maxVelocity_[1]);
} else {
referenceStates_(9, 1 + i) = 0.;
referenceStates_(10, 1 + i) = 0.;
@@ -96,49 +111,28 @@ void StatePlanner3D::computeReferenceStates(VectorN const& q, Vector6 const& v,
}
}
-void StatePlanner3D::compute_configurations(VectorN const& q, Vector6 const& vref) {
- pinocchio::SE3::Quaternion quat_;
- Vector3 rpy_map_tmp = Vector3::Zero(3); // Tmp vector3 to recompute the orientation of the configuration
- Vector3 mean_surface_tmp = Vector3::Zero(3); // Tmp vector3 to recompute the mean surface
- 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);
+void StatePlanner3D::computeConfigurations(VectorN const& q, Vector6 const& vRef) {
+ for (int i = 0; i < nSteps_; i++) {
+ double dt_config = stepDuration_ * (i + 2); // Delay of 2 phase of contact for MIP
+
+ Vector2 dxdy;
+
+ if (std::abs(vRef(5)) >= 0.001) {
+ dxdy(0) = (vRef(0) * std::sin(vRef(5) * dt_config) + vRef(1) * (std::cos(vRef(5) * dt_config) - 1.0)) / vRef(5);
+ dxdy(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;
+ dxdy(0) = vRef(0) * dt_config;
+ dxdy(1) = vRef(1) * dt_config;
}
+ configs_(0, i) = std::cos(q(5)) * dxdy(0) - std::sin(q(5)) * dxdy(1); // Yaw rotation for dx
+ configs_(1, i) = std::sin(q(5)) * dxdy(0) + std::cos(q(5)) * dxdy(1); // Yaw rotation for dy
+ configs_.block(0, i, 2, 1) += q.head(2); // Add initial position
- Vector3 rpy_config = Vector3::Zero();
- rpy_config(2) = q(5) + vref(5) * dt_config;
-
- // Update according to heightmap
+ configs_(2, i) = fit_(0) * configs_(0, i) + fit_(1) * configs_(1, i) + fit_(2) + referenceHeight_;
+ rpyConfig_(2) = q(5) + vRef(5) * dt_config;
+ rpyConfig_(0) = rpyMap_(0) * std::cos(rpyConfig_(2)) - rpyMap_(1) * std::sin(rpyConfig_(2));
+ rpyConfig_(1) = rpyMap_(0) * std::sin(rpyConfig_(2)) + rpyMap_(1) * std::cos(rpyConfig_(2));
- // Use the mean surface computed for the current position
- // config_(2) = mean_surface(0) * config_(0) + mean_surface(1) * config_(1) +
- // mean_surface(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]);
-
- // Rcomputed the mean surface for the estimated next position
- mean_surface_tmp = heightmap_.compute_mean_surface(config_(0), config_(1));
- config_(2) = mean_surface(0) * config_(0) + mean_surface(1) * config_(1) + mean_surface(2) + h_ref_;
-
- rpy_map_tmp(0) = -std::atan2(mean_surface_tmp(1), 1.);
- rpy_map_tmp(1) = -std::atan2(mean_surface_tmp(0), 1.);
- rpy_config(0) = rpy_map_tmp[0] * std::cos(rpy_config[2]) - rpy_map_tmp[1] * std::sin(rpy_config[2]);
- rpy_config(1) = rpy_map_tmp[0] * std::sin(rpy_config[2]) + rpy_map_tmp[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_;
+ configs_.block(3, i, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(rpyConfig_)).coeffs();
}
}
diff --git a/src/walk_parameters.yaml b/src/walk_parameters.yaml
index f64a56f876842c79c6f48318996ca9f1cc912888..219cf5df12a8d64656b03b266fbc11dc535eb5e5 100644
--- a/src/walk_parameters.yaml
+++ b/src/walk_parameters.yaml
@@ -1,40 +1,52 @@
robot:
# General parameters
config_file: config_solo12.yaml # Name of the yaml file containing hardware information
- interface: enp2s0 # Name of the communication interface (check with ifconfig)
- DEMONSTRATION: false # Enable/disable demonstration functionalities
- SIMULATION: false # Enable/disable PyBullet simulation or running on real robot
+
+ interface: eth0 # Name of the communication inerface (check with ifconfig)
LOGGING: true # Enable/disable logging during the experiment
PLOTTING: false # Enable/disable automatic plotting at the end of the experiment
+ DEMONSTRATION: false # Enable/disable demonstration functionalities
+ SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
+ enable_pyb_GUI: true # Enable/disable PyBullet GUI
envID: 0 # Identifier of the environment to choose in which one the simulation will happen
use_flat_plane: true # If True the ground is flat, otherwise it has bumps
predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
- velID: 6 # Identifier of the reference velocity profile to choose which one will be sent to the robot
- N_SIMULATION: 11000 # Number of simulated wbc time steps
- enable_pyb_GUI: false # Enable/disable PyBullet GUI
- enable_corba_viewer: true # Enable/disable Corba Viewer
+ velID: 10 # Identifier of the reference velocity profile to choose which one will be sent to the robot
+ N_SIMULATION: 180000 # Number of simulated wbc time steps
+ enable_corba_viewer: false # Enable/disable Corba Viewer
enable_multiprocessing: true # 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
- # q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
+ # q_init: [ 0.00208551, 0.97841023, -1.77335038, 0.0020868, 0.97951833, -1.77534163, 0.00208551, 0.97841023, -1.77335038, 0.0020868, 0.97951833, -1.77534163]
+ # q_init: [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
+ q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
dt_wbc: 0.001 # Time step of the whole body control
dt_mpc: 0.02 # Time step of the model predictive control
- type_MPC: 3 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
+ type_MPC: 0 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
+# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
Kp_main: [3.0, 3.0, 3.0] # Proportional gains for the PD+
+# Kd_main: [0., 0., 0.] # Derivative gains for the PD+
Kd_main: [0.3, 0.3, 0.3] # Derivative gains for the PD+
+# Kff_main: 0.0 # Feedforward torques multiplier for the PD+
Kff_main: 1.0 # Feedforward torques multiplier for the PD+
# Parameters of Gait
N_periods: 1
- gait: [12, 1, 0, 0, 1,
- 12, 0, 1, 1, 0] # Initial gait matrix
+ gait: [8, 1, 0, 0, 1,
+ 8, 0, 1, 1, 0] # Initial gait matrix
# Parameters of Joystick
gp_alpha_vel: 0.003 #Â Coefficient of the low pass filter applied to gamepad velocity
gp_alpha_pos: 0.005 #Â Coefficient of the low pass filter applied to gamepad position
+ t_switch: [ 0, 1, 11, 12, 13, 15, 16, 117]
+ v_switch: [0.0, 0.15, 0.15, 0.0, 0.0, 0.0, 0.0, 0.1,
+ 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, 0.6, 0.6, 0.0, 0.0]
# Parameters of Estimator
fc_v_esti: 50.0 # Cut frequency for the low pass that filters the estimated base velocity
@@ -70,7 +82,14 @@ robot:
Fz_min: 0.0 # Minimal vertical contact force [N]
# Parameters fro solo3D simulation
- solo3D: false # 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: true # Activation of the 3D environment, and corresponding planner blocks
+ enable_multiprocessing_mip: true # Enable/disable running the MIP in another process in parallel of the main loop
+ environment_URDF: "/short_bricks/short_bricks.urdf"
+ environment_heightmap: "/short_bricks/short_bricks.bin"
+ heightmap_fit_length: 0.2 # Length of half the heightmap fit in a direction
+ heightmap_fit_size: 5 # Number of points on each axis in the heightmap fit
+ number_steps: 3 # Number of steps to ptimize with the MIP
+ max_velocity: [0.4, 0.4, 0.1] # Maximum velocity of the base
+ use_bezier: false # Use Bezier to plan trajectories, otherwise use simple 6d polynomial curve.
+ use_sl1m: true # Use SL1M to select the surfaces, otherwise use Raibert heuristic projection in 2D.
+ use_heuristic: true # Use heuristic as SL1M cost.