From b939677c2da7cf395b860099f8c679bbdae6061e Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Fri, 18 Dec 2020 17:31:52 +0100
Subject: [PATCH] Implement change of gait with joystick for C++ Planner
---
.../quadruped-reactive-walking/Planner.hpp | 6 +-
python/gepadd.cpp | 2 +-
scripts/Planner.py | 7 +-
src/Planner.cpp | 107 +++++++++++++++++-
4 files changed, 116 insertions(+), 6 deletions(-)
diff --git a/include/quadruped-reactive-walking/Planner.hpp b/include/quadruped-reactive-walking/Planner.hpp
index 70dd1a15..db403bda 100644
--- a/include/quadruped-reactive-walking/Planner.hpp
+++ b/include/quadruped-reactive-walking/Planner.hpp
@@ -132,8 +132,12 @@ class Planner {
int create_walk();
int create_trot();
+ int create_pacing();
+ int create_bounding();
+ int create_static();
int create_gait_f();
int roll(int k);
+ int handle_joystick(int code, const Eigen::MatrixXd &q);
int compute_footsteps(Eigen::MatrixXd q_cur, Eigen::MatrixXd v_cur, Eigen::MatrixXd v_ref);
double get_stance_swing_duration(int i, int j, double value);
int compute_next_footstep(int i, int j);
@@ -141,7 +145,7 @@ class Planner {
int update_target_footsteps();
int update_trajectory_generator(int k, double h_estim);
int run_planner(int k, const Eigen::MatrixXd &q, const Eigen::MatrixXd &v, const Eigen::MatrixXd &b_vref,
- double h_estim, double z_average);
+ double h_estim, double z_average, int joystick_code);
// Accessors (to retrieve C data from Python)
Eigen::MatrixXd get_xref();
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index c399c67b..da855671 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -53,7 +53,7 @@ struct PlannerPythonVisitor : public bp::def_visitor<PlannerPythonVisitor<Planne
//.add_property("xref", &Planner::get_xref)
// Run Planner from Python
- .def("run_planner", &Planner::run_planner, bp::args("k", "q", "v", "b_vref", "h_estim", "z_average"),
+ .def("run_planner", &Planner::run_planner, bp::args("k", "q", "v", "b_vref", "h_estim", "z_average", "joystick_code"),
"Run Planner from Python.\n");
}
diff --git a/scripts/Planner.py b/scripts/Planner.py
index 53ae9dc2..1c27557a 100644
--- a/scripts/Planner.py
+++ b/scripts/Planner.py
@@ -571,20 +571,25 @@ class PyPlanner:
self.new_desired_gait = self.one_swing_gait()
self.is_static = False"""
+ joystick_code = 0
if joystick is not None:
if joystick.northButton:
+ joystick_code = 1
self.new_desired_gait = self.pacing_gait()
self.is_static = False
joystick.northButton = False
elif joystick.eastButton:
+ joystick_code = 2
self.new_desired_gait = self.bounding_gait()
self.is_static = False
joystick.eastButton = False
elif joystick.southButton:
+ joystick_code = 3
self.new_desired_gait = self.trot_gait()
self.is_static = False
joystick.southButton = False
elif joystick.westButton:
+ joystick_code = 4
self.new_desired_gait = self.static_gait()
self.is_static = True
self.q_static[0:7, 0:1] = q.copy()
@@ -617,7 +622,7 @@ class PyPlanner:
# Update desired location of footsteps on the ground
# self.update_target_footsteps()
- self.Cplanner.run_planner(k, q, v, b_vref, np.double(h_estim), np.double(z_average))
+ self.Cplanner.run_planner(k, q, v, b_vref, np.double(h_estim), np.double(z_average), joystick_code)
# Update trajectory generator (3D pos, vel, acc)
# self.update_trajectory_generator(k, h_estim, q)
diff --git a/src/Planner.cpp b/src/Planner.cpp
index 365f1c30..e92da2ed 100644
--- a/src/Planner.cpp
+++ b/src/Planner.cpp
@@ -102,6 +102,69 @@ int Planner::create_trot() {
return 0;
}
+int Planner::create_pacing() {
+ /* Create a pacing gait with legs on the same side (left or right) moving at the same time */
+
+ // Number of timesteps in a half period of gait
+ int N = (int)std::lround(0.5 * T_gait / dt);
+
+ gait_f_des = Eigen::Matrix<double, N0_gait, 5>::Zero();
+ gait_f_des.block(0, 0, 2, 1) << N, N;
+ fsteps.block(0, 0, 2, 1) = gait_f_des.block(0, 0, 2, 1);
+
+ // Set stance and swing phases
+ // Coefficient (i, j) is equal to 0.0 if the j-th feet is in swing phase during the i-th phase
+ // Coefficient (i, j) is equal to 1.0 if the j-th feet is in stance phase during the i-th phase
+ gait_f_des(0, 1) = 1.0;
+ gait_f_des(0, 3) = 1.0;
+ gait_f_des(1, 2) = 1.0;
+ gait_f_des(1, 4) = 1.0;
+
+ return 0;
+}
+
+int Planner::create_bounding() {
+ /* Create a bounding gait with legs on the same side (front or hind) moving at the same time */
+
+ // Number of timesteps in a half period of gait
+ int N = (int)std::lround(0.5 * T_gait / dt);
+
+ gait_f_des = Eigen::Matrix<double, N0_gait, 5>::Zero();
+ gait_f_des.block(0, 0, 2, 1) << N, N;
+ fsteps.block(0, 0, 2, 1) = gait_f_des.block(0, 0, 2, 1);
+
+ // Set stance and swing phases
+ // Coefficient (i, j) is equal to 0.0 if the j-th feet is in swing phase during the i-th phase
+ // Coefficient (i, j) is equal to 1.0 if the j-th feet is in stance phase during the i-th phase
+ gait_f_des(0, 1) = 1.0;
+ gait_f_des(0, 2) = 1.0;
+ gait_f_des(1, 3) = 1.0;
+ gait_f_des(1, 4) = 1.0;
+
+ return 0;
+}
+
+int Planner::create_static() {
+ /* Create a static gait with all legs in stance phase */
+
+ // Number of timesteps in a half period of gait
+ int N = (int)std::lround(T_gait / dt);
+
+ gait_f_des = Eigen::Matrix<double, N0_gait, 5>::Zero();
+ gait_f_des(0, 0) = N;
+ fsteps(0, 0) = gait_f_des(0, 0);
+
+ // Set stance and swing phases
+ // Coefficient (i, j) is equal to 0.0 if the j-th feet is in swing phase during the i-th phase
+ // Coefficient (i, j) is equal to 1.0 if the j-th feet is in stance phase during the i-th phase
+ gait_f_des(0, 1) = 1.0;
+ gait_f_des(0, 2) = 1.0;
+ gait_f_des(0, 3) = 1.0;
+ gait_f_des(0, 4) = 1.0;
+
+ return 0;
+}
+
int Planner::create_gait_f() {
/* Initialize content of the gait matrix based on the desired gait, the gait period and
the length of the prediciton horizon */
@@ -400,11 +463,13 @@ int Planner::getRefStates(Eigen::MatrixXd q, Eigen::MatrixXd v, Eigen::MatrixXd
}
if (is_static) {
+ Eigen::Matrix<double, 3, 1> RPY;
+ Eigen::Quaterniond quat(q_static(6, 0), q_static(3, 0), q_static(4, 0), q_static(5, 0)); // w, x, y, z
+ RPY << pinocchio::rpy::matrixToRpy(quat.toRotationMatrix());
for (int i = 0; i < n_steps; i++) {
xref.block(0, 1 + i, 3, 1) = q_static.block(0, 0, 3, 1);
+ xref.block(3, 1 + i, 3, 1) = RPY;
}
- xref.block(3, 1, 3, n_steps) = Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(
- 3, n_steps); // (utils_mpc.quaternionToRPY(self.q_static[3:7, 0])).reshape((3, 1))
}
return 0;
@@ -514,7 +579,7 @@ int Planner::update_trajectory_generator(int k, double h_estim) {
}
int Planner::run_planner(int k, const Eigen::MatrixXd &q, const Eigen::MatrixXd &v, const Eigen::MatrixXd &b_vref_in,
- double h_estim, double z_average) {
+ double h_estim, double z_average, int joystick_code) {
/* Run the planner for one iteration of the main control loop
Args:
@@ -524,6 +589,7 @@ int Planner::run_planner(int k, const Eigen::MatrixXd &q, const Eigen::MatrixXd
b_vref_in (6x1 array): desired velocity vector of the flying base in base frame (linear and angular stacked)
h_estim (double): estimated height of the base
z_average (double): average height of feet currently in stance phase
+ joystick_code (int): integer to trigger events with the joystick
*/
// Get the reference velocity in world frame (given in base frame)
@@ -536,6 +602,9 @@ int Planner::run_planner(int k, const Eigen::MatrixXd &q, const Eigen::MatrixXd
vref_in.block(0, 0, 3, 1) = R_2 * b_vref_in.block(0, 0, 3, 1);
vref_in.block(3, 0, 3, 1) = b_vref_in.block(3, 0, 3, 1);
+ // Handle joystick events
+ handle_joystick(joystick_code, q);
+
// Move one step further in the gait
if (k % k_mpc == 0) {
roll(k);
@@ -636,6 +705,38 @@ int Planner::roll(int k) {
return 0;
}
+int Planner::handle_joystick(int code, const Eigen::MatrixXd &q) {
+ /* Handle the joystick code to trigger events (change of gait for instance)
+
+ Args:
+ code (int): integer to trigger events with the joystick
+ q (7x1 array): current position vector of the flying base in world frame (linear and angular stacked)
+ */
+
+ if (code == 0) {
+ return 0;
+ }
+ else if (code == 1) {
+ create_pacing();
+ is_static = false;
+ }
+ else if (code == 2) {
+ create_bounding();
+ is_static = false;
+ }
+ else if (code == 3) {
+ create_trot();
+ is_static = false;
+ }
+ else if (code == 4) {
+ create_static();
+ q_static.block(0, 0, 7, 1) = q.block(0, 0, 7, 1);
+ is_static = true;
+ }
+
+ return 0;
+}
+
// Trajectory generator functions (output reference pos, vel and acc of feet in swing phase)
TrajGen::TrajGen() {}
--
GitLab