From 80dedbb0cc997c9e09daa12fc63b58aae4d1c8b6 Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Fri, 11 Mar 2022 13:38:29 +0000
Subject: [PATCH] Clean and refactor gait class
---
README.md | 2 +-
config/walk_parameters.yaml | 2 +-
include/qrw/Gait.hpp | 84 +++---
.../Solo3D/FootTrajectoryGeneratorBezier.hpp | 2 -
python/Gait.cpp | 17 +-
.../Solo3D/FootTrajectoryGeneratorBezier.cpp | 5 +-
scripts/Controller.py | 42 +--
scripts/solo3D/pyb_environment_3D.py | 2 +-
scripts/tools/LoggerControl.py | 4 +-
src/Controller.cpp | 14 +-
src/FootTrajectoryGenerator.cpp | 4 +-
src/FootstepPlanner.cpp | 18 +-
src/Gait.cpp | 266 ++++++++----------
src/Solo3D/FootTrajectoryGeneratorBezier.cpp | 67 +----
src/Solo3D/FootstepPlannerQP.cpp | 6 +-
src/WbcWrapper.cpp | 2 +-
16 files changed, 228 insertions(+), 309 deletions(-)
diff --git a/README.md b/README.md
index 54b9d904..c23a2ce9 100644
--- a/README.md
+++ b/README.md
@@ -72,7 +72,7 @@ Implementation of a reactive walking controller for quadruped robots. Architectu
* If `predefined_vel = True` the robot follows the reference velocity pattern velID. Velocity patterns are defined in `Joystick.py`, you can modify them or add new ones. Each profile defines forward, lateral and yaw velocities that should be reached at the associated loop iterations (in `self.k_switch`). There is an automatic interpolation between milestones to have a smooth reference velocity command.
-* You can define a new gait in `src/Planner.cpp` using `create_trot` or `create_walk` as models. Create a new function (like `create_bounding` for a bounding gait) and call it inside the Planner constructor before `create_gait_f()`.
+* You can define a new gait in `src/Planner.cpp` using `createTrot` or `createWalk` as models. Create a new function (like `createBounding` for a bounding gait) and call it inside the Planner constructor before `create_gait_f()`.
* You can modify the swinging feet apex height in `include/quadruped-reactive-control/Planner.hpp` with `maxHeight_` or the lock time before touchdown with `lockTime_` (to lock the target location on the ground before touchdown).
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index b8a25b29..1d09a607 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -24,7 +24,7 @@ robot:
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+
diff --git a/include/qrw/Gait.hpp b/include/qrw/Gait.hpp
index 9b4f0b7a..ec141cb0 100644
--- a/include/qrw/Gait.hpp
+++ b/include/qrw/Gait.hpp
@@ -43,19 +43,44 @@ class Gait {
////////////////////////////////////////////////////////////////////////////////////////////////
///
- /// \brief Compute the remaining and total duration of a swing phase or a stance phase based
+ /// \brief Compute the total duration of a swing phase or a stance phase based
/// on the content of the gait matrix
///
/// \param[in] i Considered phase (row of the gait matrix)
/// \param[in] j Considered foot (col of the gait matrix)
- /// \param[in] value 0.0 for swing phase detection, 1.0 for stance phase detection
///
////////////////////////////////////////////////////////////////////////////////////////////////
- double getPhaseDuration(int i, int j, double value);
+ double getPhaseDuration(int i, int j);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute the duration of a swing phase or a stance phase since its start based
+ /// on the content of the gait matrix
+ /// \details We suppose that the phase starts after the start of past gait matrix
+ ///
+ /// \param[in] i Considered phase (row of the gait matrix)
+ /// \param[in] j Considered foot (col of the gait matrix)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ double getElapsedTime(int i, int j);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute the remaining duration of a swing phase or a stance phase based
+ /// on the content of the gait matrix
+ /// \details We suppose that the end of the phase is reached before the end of desiredGait matrix
+ ///
+ /// \param[in] i Considered phase (row of the gait matrix)
+ /// \param[in] j Considered foot (col of the gait matrix)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ double getRemainingTime(int i, int j);
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Handle the joystick code to trigger events (change of gait for instance)
+ /// \details We suppose that the end of the phase is reached before the end of the desiredGait
+ /// matrix and the phase starts after the start of past gait matrix
///
/// \param[in] k Numero of the current loop
/// \param[in] k_mpc Number of loop per mpc time step
@@ -77,7 +102,7 @@ class Gait {
/// \param[in] joystickCode Integer to trigger events with the joystick
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void updateGait(int const k, int const k_mpc, int const joystickCode);
+ void update(int const k, int const k_mpc, int const joystickCode);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -86,38 +111,34 @@ class Gait {
////////////////////////////////////////////////////////////////////////////////////////////////
void rollGait();
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Update the gait matrix externally (directly set the gait matrix)
- ///
- /// \param[in] gaitMatrix Gait matrix that should be used for the incoming timesteps
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- bool setGait(MatrixN const& gaitMatrix);
-
MatrixN getPastGait() { return pastGait_; }
MatrixN getCurrentGait() { return currentGait_; }
double getCurrentGaitCoeff(int i, int j) { return currentGait_(i, j); }
MatrixN getDesiredGait() { return desiredGait_; }
- double getRemainingTime() { return remainingTime_; }
- bool getIsStatic() { return is_static_; }
- VectorN getQStatic() { return q_static_; }
+ bool getIsStatic() { return isStatic_; }
bool isNewPhase() { return newPhase_; }
private:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a static gait with all legs in stance phase
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void createStatic();
+
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Create a slow walking gait, raising and moving only one foot at a time
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void create_walk();
+ void createWalk();
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Create a trot gait with diagonaly opposed legs moving at the same time
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void create_trot();
+ void createTrot();
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -125,55 +146,46 @@ class Gait {
/// 4-stance phases
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void create_walking_trot();
+ void createWalkingTrot();
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Create a pacing gait with legs on the same side (left or right) moving at the same time
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void create_pacing();
+ void createPacing();
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Create a bounding gait with legs on the same side (front or hind) moving at the same time
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void create_bounding();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a static gait with all legs in stance phase
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void create_static();
+ void createBounding();
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Create a transverse gallop gait
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void create_transverse_gallop();
+ void createTransverseGallop();
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Create a custom gallop gait
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void create_custom_gallop();
+ void createCustomGallop();
MatrixN pastGait_; // Past gait
MatrixN currentGait_; // Current and future gait
MatrixN desiredGait_; // Future desired gait
double dt_; // Time step of the contact sequence (time step of the MPC)
+ int nRows_; // number of rows in the gait matrix
- double remainingTime_; // Remaining time till the end of the current stance/swing phase
-
- bool newPhase_; // Flag to indicate that the contact status has changed
- bool is_static_; // Flag to indicate that all feet are in an infinite stance phase
- int switch_to_gait_; // Memory to store joystick code if the user wants to change the gait pattern
- VectorN q_static_; // Configuration vector used during static phases (4 feet in stance)
+ bool newPhase_; // Flag to indicate that the contact status has changed
+ bool isStatic_; // Flag to indicate that all feet are in an infinite stance phase
+ int switchToGait_; // Memory to store joystick code if the user wants to change the gait pattern
};
#endif // GAIT_H_INCLUDED
diff --git a/include/qrw/Solo3D/FootTrajectoryGeneratorBezier.hpp b/include/qrw/Solo3D/FootTrajectoryGeneratorBezier.hpp
index 80b89b3d..53e31b85 100644
--- a/include/qrw/Solo3D/FootTrajectoryGeneratorBezier.hpp
+++ b/include/qrw/Solo3D/FootTrajectoryGeneratorBezier.hpp
@@ -78,8 +78,6 @@ 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);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/python/Gait.cpp b/python/Gait.cpp
index 55e0e894..e22f8a7f 100644
--- a/python/Gait.cpp
+++ b/python/Gait.cpp
@@ -8,20 +8,19 @@ struct GaitVisitor : public bp::def_visitor<GaitVisitor<Gait>> {
void visit(PyClassGait& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def("getPastGait", &Gait::getPastGait, "Get currentGait_ matrix.\n")
- .def("getCurrentGait", &Gait::getCurrentGait, "Get currentGait_ matrix.\n")
- .def("getDesiredGait", &Gait::getDesiredGait, "Get currentGait_ matrix.\n")
- .def("isNewPhase", &Gait::isNewPhase, "Get newPhase_ boolean.\n")
- .def("getIsStatic", &Gait::getIsStatic, "Get is_static_ boolean.\n")
+ .def("get_past_gait", &Gait::getPastGait, "Get past gait matrix.\n")
+ .def("get_gait_matrix", &Gait::getCurrentGait, "Get gait matrix.\n")
+ .def("get_desired_gait", &Gait::getDesiredGait, "Get desired gait matrix.\n")
+ .def("is_new_step", &Gait::isNewPhase, "True if new phase of the gait.\n")
+ .def("is_static", &Gait::getIsStatic, "True if static gait.\n")
.def("initialize", &Gait::initialize, bp::args("params"), "Initialize Gait from Python.\n")
- // Update current gait matrix from Python
- .def("updateGait", &Gait::updateGait, bp::args("k", "k_mpc", "joystickCode"),
+ .def("update", &Gait::update, bp::args("k", "k_mpc", "joystickCode"),
"Update current gait matrix from Python.\n")
- // Set current gait matrix from Python
- .def("setGait", &Gait::setGait, bp::args("gaitMatrix"), "Set current gait matrix from Python.\n");
+ .add_property("matrix",
+ bp::make_function(&Gait::getCurrentGait, bp::return_value_policy<bp::return_by_value>()));
}
static void expose() { bp::class_<Gait>("Gait", bp::no_init).def(GaitVisitor<Gait>()); }
diff --git a/python/Solo3D/FootTrajectoryGeneratorBezier.cpp b/python/Solo3D/FootTrajectoryGeneratorBezier.cpp
index 16a09922..11ce47ed 100644
--- a/python/Solo3D/FootTrajectoryGeneratorBezier.cpp
+++ b/python/Solo3D/FootTrajectoryGeneratorBezier.cpp
@@ -31,10 +31,7 @@ struct FootTrajectoryGeneratorBezierVisitor
bp::return_value_policy<bp::return_by_value>()))
.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");
+ "Compute target location of footsteps from Python.\n");
}
static void expose() {
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 079fb719..8e1d9af8 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -257,7 +257,7 @@ class Controller:
"""
t_start = time.time()
- self.joystick.update_v_ref(self.k, self.velID, self.gait.getIsStatic())
+ self.joystick.update_v_ref(self.k, self.velID, self.gait.is_static())
q_perfect = np.zeros(6)
b_baseVel_perfect = np.zeros(3)
@@ -290,7 +290,7 @@ class Controller:
self.last_b_vel = b_baseVel_perfect
self.n_nan = 0
- self.estimator.run(self.gait.getCurrentGait(),
+ self.estimator.run(self.gait.matrix,
self.footTrajectoryGenerator.getFootPosition(),
device.imu.linear_acceleration,
device.imu.gyroscope,
@@ -331,9 +331,9 @@ class Controller:
t_filter = time.time()
self.t_filter = t_filter - t_start
- self.gait.updateGait(self.k, self.k_mpc, self.joystick.getJoystickCode())
+ self.gait.update(self.k, self.k_mpc, self.joystick.getJoystickCode())
- self.update_mip = self.k % self.k_mpc == 0 and self.gait.isNewPhase()
+ self.update_mip = self.k % self.k_mpc == 0 and self.gait.is_new_step()
if self.solo3D:
if self.update_mip:
self.statePlanner.updateSurface(self.q_filter[:6, :1], self.vref_filt_mpc[:6, :1])
@@ -352,11 +352,11 @@ class Controller:
xref = self.statePlanner.getReferenceStates()
fsteps = self.footstepPlanner.getFootsteps()
- cgait = self.gait.getCurrentGait()
+ gait_matrix = self.gait.matrix
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.run(configs, gait_matrix, 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)
@@ -370,14 +370,14 @@ class Controller:
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.mpc_wrapper.solve(self.k, xref, fsteps, gait_matrix, 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.mpc_wrapper.solve(self.k, xref, fsteps, gait_matrix, np.zeros((3, 4)))
except ValueError:
print("MPC Problem")
self.x_f_mpc, self.mpc_cost = self.mpc_wrapper.get_latest_result()
@@ -388,9 +388,9 @@ class Controller:
# If the MPC optimizes footsteps positions then we use them
if self.k > 100 and self.type_MPC == 3:
for foot in range(4):
- if cgait[0, foot] == 0:
+ if gait_matrix[0, foot] == 0:
id = 0
- while cgait[id, foot] == 0:
+ while gait_matrix[id, foot] == 0:
id += 1
self.o_targetFootstep[:2, foot] = self.x_f_mpc[24 + 2*foot:24+2*foot+2, id+1]
@@ -401,12 +401,12 @@ class Controller:
self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep)
if not self.error and not self.joystick.getStop():
- if self.DEMONSTRATION and self.gait.getIsStatic():
+ if self.DEMONSTRATION and self.gait.is_static():
hRb = np.eye(3)
# Desired position, orientation and velocities of the base
self.xgoals[:6, 0] = np.zeros((6,))
- if self.DEMONSTRATION and self.joystick.getL1() and self.gait.getIsStatic():
+ if self.DEMONSTRATION and self.joystick.getL1() and self.gait.is_static():
self.p_ref[:, 0] = self.joystick.getPRef()
self.xgoals[[3, 4], 0] = self.p_ref[[3, 4], 0]
self.h_ref = self.p_ref[2, 0]
@@ -416,7 +416,7 @@ class Controller:
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():
+ if self.DEMONSTRATION and self.gait.is_static():
self.x_f_mpc[12:24, 0] = [0.0, 0.0, 9.81 * 2.5 / 4.0] * 4
# Update configuration vector for wbc with filtered roll and pitch and reference angular positions of previous loop
@@ -447,7 +447,7 @@ class Controller:
# 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, :]]),
+ (self.x_f_mpc[12:24, 0:1]).copy(), np.array([gait_matrix[0, :]]),
self.feet_p_cmd,
self.feet_v_cmd,
self.feet_a_cmd,
@@ -484,7 +484,7 @@ class Controller:
if self.SIMULATION:
self.pybEnvironment3D.update(self.k)
- self.pyb_debug(device, fsteps, cgait, xref)
+ self.pyb_debug(device, fsteps, gait_matrix, xref)
self.t_loop = time.time() - t_start
self.k += 1
@@ -499,7 +499,7 @@ class Controller:
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):
+ def pyb_debug(self, device, fsteps, gait_matrix, xref):
if self.k > 1 and self.enable_pyb_GUI:
# Display desired feet positions in WBC as green spheres
@@ -517,12 +517,12 @@ class Controller:
for i in range(4):
j = 0
cpt = 1
- status = cgait[0, i]
- while cpt < cgait.shape[0] and j < device.pyb_sim.ftps_Ids.shape[1]:
- while cpt < cgait.shape[0] and cgait[cpt, i] == status:
+ status = gait_matrix[0, i]
+ while cpt < gait_matrix.shape[0] and j < device.pyb_sim.ftps_Ids.shape[1]:
+ while cpt < gait_matrix.shape[0] and gait_matrix[cpt, i] == status:
cpt += 1
- if cpt < cgait.shape[0]:
- status = cgait[cpt, i]
+ if cpt < gait_matrix.shape[0]:
+ status = gait_matrix[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], [oTh_pyb[2, 0]]])
pyb.resetBasePositionAndOrientation(
diff --git a/scripts/solo3D/pyb_environment_3D.py b/scripts/solo3D/pyb_environment_3D.py
index 1c12887c..de7c5e9f 100644
--- a/scripts/solo3D/pyb_environment_3D.py
+++ b/scripts/solo3D/pyb_environment_3D.py
@@ -84,7 +84,7 @@ class PybEnvironment3D():
''' Update the target trajectory for current and next phases. Hide the unnecessary spheres.
'''
- gait = self.gait.getCurrentGait()
+ gait = self.gait.get_gait_matrix()
fsteps = self.footStepPlanner.getFootsteps()
for j in range(4):
diff --git a/scripts/tools/LoggerControl.py b/scripts/tools/LoggerControl.py
index 667173c9..984193fb 100644
--- a/scripts/tools/LoggerControl.py
+++ b/scripts/tools/LoggerControl.py
@@ -184,12 +184,12 @@ class LoggerControl():
self.planner_xref[self.i] = statePlanner.getReferenceStates()
self.planner_fsteps[self.i] = footstepPlanner.getFootsteps()
self.planner_target_fsteps[self.i] = footstepPlanner.getTargetFootsteps()
- self.planner_gait[self.i] = gait.getCurrentGait()
+ self.planner_gait[self.i] = gait.get_gait_matrix()
self.planner_goals[self.i] = footTrajectoryGenerator.getFootPosition()
self.planner_vgoals[self.i] = footTrajectoryGenerator.getFootVelocity()
self.planner_agoals[self.i] = footTrajectoryGenerator.getFootAcceleration()
self.planner_jgoals[self.i] = footTrajectoryGenerator.getFootJerk()
- self.planner_is_static[self.i] = gait.getIsStatic()
+ self.planner_is_static[self.i] = gait.is_static()
self.planner_h_ref[self.i] = controller.h_ref
# Logging from model predictive control
diff --git a/src/Controller.cpp b/src/Controller.cpp
index 6404dd5f..2f60c8c1 100644
--- a/src/Controller.cpp
+++ b/src/Controller.cpp
@@ -1,11 +1,10 @@
#include "qrw/Controller.hpp"
+#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/crba.hpp"
-#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/math/rpy.hpp"
#include "pinocchio/parsers/urdf.hpp"
-#include "pinocchio/algorithm/compute-all-terms.hpp"
-#include "pinocchio/algorithm/frames.hpp"
Controller::Controller()
: P(Vector12::Zero()),
@@ -73,7 +72,7 @@ void Controller::compute(FakeRobot* robot) {
estimator.updateReferenceState(joystick.getVRef());
// Update gait
- gait.updateGait(k, k_mpc, joystick.getJoystickCode());
+ gait.update(k, k_mpc, joystick.getJoystickCode());
// Quantities go through a 1st order low pass filter with fc = 15 Hz (avoid >25Hz foldback)
q_filt_mpc.head(6) = filter_mpc_q.filter(estimator.getQReference().head(6), true);
@@ -136,7 +135,7 @@ void Controller::compute(FakeRobot* robot) {
// Update velocity vector for wbc
dq_wbc.head(6) = estimator.getVEstimate().head(6); // Velocities in base frame (not horizontal frame!)
- dq_wbc.tail(12) = wbcWrapper.get_vdes(); // with reference angular velocities of previous loop
+ dq_wbc.tail(12) = wbcWrapper.get_vdes(); // with reference angular velocities of previous loop
// Desired position, orientation and velocities of the base
xgoals.tail(6) = vref_filt_mpc; // Velocities (in horizontal frame!)
@@ -252,13 +251,14 @@ void Controller::security_check() {
if (((estimator.getQEstimate().tail(12).cwiseAbs()).array() > q_security_.array()).any()) {
std::cout << "Position limit error "
- << ((estimator.getQEstimate().tail(12).cwiseAbs()).array() > q_security_.array()).transpose() << std::endl;
+ << ((estimator.getQEstimate().tail(12).cwiseAbs()).array() > q_security_.array()).transpose()
+ << std::endl;
error_flag = 1;
} else if (((estimator.getVSecurity().cwiseAbs()).array() > 100.0).any()) {
std::cout << "Velocity limit error " << ((estimator.getVSecurity().cwiseAbs()).array() > 100.0).transpose()
<< std::endl;
error_flag = 2;
- } else if (((tau_ff.cwiseAbs()).array() > 8.0).any()) {
+ } else if (((tau_ff.cwiseAbs()).array() > 8.0).any()) {
std::cout << "Feedforward limit error " << ((tau_ff.cwiseAbs()).array() > 8.0).transpose() << std::endl;
error_flag = 3;
} else {
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index ea66aab7..08880bec 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -193,8 +193,8 @@ void FootTrajectoryGenerator::update(int k, MatrixN const &targetFootstep) {
// For each foot in swing phase get remaining duration of the swing phase
for (int i = 0; i < 4; i++) {
if (feet(0, i) == 0) {
- t_swing[i] = gait_->getPhaseDuration(0, i, 0.0); // 0.0 for swing phase
- double value = t_swing[i] - (gait_->getRemainingTime() * k_mpc - ((k + 1) % k_mpc)) * dt_wbc - dt_wbc;
+ t_swing[i] = gait_->getPhaseDuration(0, i);
+ double value = gait_->getElapsedTime(0, i) - dt_wbc;
t0s[i] = std::max(0.0, value);
}
}
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index fda686c3..ad6227f0 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -1,11 +1,11 @@
+#include "qrw/FootstepPlanner.hpp"
+
#include <example-robot-data/path.hpp>
-#include "pinocchio/math/rpy.hpp"
-#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/frames.hpp"
-
-#include "qrw/FootstepPlanner.hpp"
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/parsers/urdf.hpp"
FootstepPlanner::FootstepPlanner()
: gait_(NULL),
@@ -52,8 +52,7 @@ void FootstepPlanner::initialize(Params& params, Gait& gaitIn) {
Rz(2, 2) = 1.0;
// Path to the robot URDF
- const std::string filename =
- std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf");
+ const std::string filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf");
// Build model from urdf (base is not free flyer)
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
@@ -141,14 +140,14 @@ void FootstepPlanner::computeFootsteps(int k, Vector6 const& b_v, Vector6 const&
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) {
+ if (gait(i - 1, j) > 0 && gait(i, j) > 0) {
footsteps_[i].col(j) = footsteps_[i - 1].col(j);
}
}
// Feet that were in swing phase and are now in stance phase need to be updated
for (int j = 0; j < 4; j++) {
- if ((1 - gait(i - 1, j)) * gait(i, j) > 0) {
+ if (gait(i - 1, j) == 0 && gait(i, j) > 0) {
// Offset to the future position
q_dxdy << dx(i - 1, 0), dy(i - 1, 0), 0.0;
@@ -168,8 +167,7 @@ void FootstepPlanner::computeFootsteps(int k, Vector6 const& b_v, Vector6 const&
void FootstepPlanner::computeNextFootstep(int i, int j, Vector6 const& b_v, Vector6 const& b_vref) {
nextFootstep_ = Matrix34::Zero();
-
- double t_stance = gait_->getPhaseDuration(i, j, 1.0); // 1.0 for stance phase
+ double t_stance = gait_->getPhaseDuration(i, j);
// Disable heuristic terms if gait is going to switch to static so that feet land at vertical of shoulders
if (!gait_->getIsStatic()) {
diff --git a/src/Gait.cpp b/src/Gait.cpp
index 59497da0..f4888264 100644
--- a/src/Gait.cpp
+++ b/src/Gait.cpp
@@ -6,245 +6,211 @@ Gait::Gait()
: pastGait_(),
currentGait_(),
desiredGait_(),
- dt_(0.0),
- remainingTime_(0.0),
+ dt_(0.),
+ nRows_(0),
newPhase_(false),
- is_static_(false),
- switch_to_gait_(0),
- q_static_(VectorN::Zero(19)) {
+ isStatic_(false),
+ switchToGait_(0) {
// Empty
}
void Gait::initialize(Params& params) {
dt_ = params.dt_mpc;
+ nRows_ = params.gait.rows();
- pastGait_ = MatrixN::Zero(params.gait.rows(), 4);
- currentGait_ = MatrixN::Zero(params.gait.rows(), 4);
- desiredGait_ = MatrixN::Zero(params.gait.rows(), 4);
+ pastGait_ = MatrixN::Zero(nRows_, 4);
+ currentGait_ = MatrixN::Zero(nRows_, 4);
+ desiredGait_ = MatrixN::Zero(nRows_, 4);
- // Fill desired gait matrix with yaml gait
- desiredGait_ = params.gait;
-
- // Fill currrent gait matrix
- currentGait_ = desiredGait_;
- pastGait_ = desiredGait_.colwise().reverse();
+ currentGait_ = params.gait;
+ desiredGait_ = currentGait_;
+ pastGait_ = currentGait_;
}
-void Gait::create_walk() {
+void Gait::createStatic() { desiredGait_.setOnes(); }
+
+void Gait::createWalk() {
// Number of timesteps in 1/4th period of gait
- long int N = currentGait_.rows() / 4;
+ long int N = nRows_ / 4;
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+ desiredGait_ = MatrixN::Zero(nRows_, 4);
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 0.0, 1.0, 0.0;
+ RowVector4 sequence;
+ sequence << 1., 0., 1., 0.;
desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 1.0, 0.0, 0.0, 1.0;
+ sequence << 1., 0., 0., 1.;
desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 0.0, 1.0;
+ sequence << 0., 1., 0., 1.;
desiredGait_.block(2 * N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 1.0, 0.0;
+ sequence << 0., 1., 1., 0.;
desiredGait_.block(3 * N, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_trot() {
- // Number of timesteps in a half period of gait
- long int N = currentGait_.rows() / 2;
-
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+void Gait::createTrot() {
+ long int N = nRows_ / 2;
+ desiredGait_ = MatrixN::Zero(nRows_, 4);
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 0.0, 0.0, 1.0;
+ RowVector4 sequence;
+ sequence << 1., 0., 0., 1.;
desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 1.0, 0.0;
+ sequence << 0., 1., 1., 0.;
desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_walking_trot() {
- // Number of timesteps in a half period of gait
- long int N = currentGait_.rows() / 2;
-
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+void Gait::createWalkingTrot() {
+ long int N = nRows_ / 2;
+ desiredGait_ = MatrixN::Zero(nRows_, 4);
long int M = 8;
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 0.0, 0.0, 1.0;
+ RowVector4 sequence;
+ sequence << 1., 0., 0., 1.;
desiredGait_.block(0, 0, N - M, 4) = sequence.colwise().replicate(N);
- sequence << 1.0, 1.0, 1.0, 1.0;
+ sequence << 1., 1., 1., 1.;
desiredGait_.block(N - M, 0, M, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 1.0, 0.0;
+ sequence << 0., 1., 1., 0.;
desiredGait_.block(N, 0, N - M, 4) = sequence.colwise().replicate(N);
- sequence << 1.0, 1.0, 1.0, 1.0;
+ sequence << 1., 1., 1., 1.;
desiredGait_.block(2 * N - M, 0, M, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_pacing() {
- // Number of timesteps in a half period of gait
- long int N = currentGait_.rows() / 2;
-
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+void Gait::createPacing() {
+ long int N = nRows_ / 2;
+ desiredGait_ = MatrixN::Zero(nRows_, 4);
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 0.0, 1.0, 0.0;
+ RowVector4 sequence;
+ sequence << 1., 0., 1., 0.;
desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 0.0, 1.0;
+ sequence << 0., 1., 0., 1.;
desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_bounding() {
- // Number of timesteps in a half period of gait
- long int N = currentGait_.rows() / 2;
-
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+void Gait::createBounding() {
+ long int N = nRows_ / 2;
+ desiredGait_ = MatrixN::Zero(nRows_, 4);
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 1.0, 0.0, 0.0;
+ RowVector4 sequence;
+ sequence << 1., 1., 0., 0.;
desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 0.0, 1.0, 1.0;
+ sequence << 0., 0., 1., 1.;
desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_static() { desiredGait_.setOnes(); }
+void Gait::createTransverseGallop() {
+ long int N = nRows_ / 4;
+ desiredGait_ = MatrixN::Zero(nRows_, 4);
-void Gait::create_transverse_gallop() {
- // Number of timesteps in a half period of gait
- long int N = currentGait_.rows() / 4;
-
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
-
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 0.0, 0.0, 1.0, 0.0;
+ RowVector4 sequence;
+ sequence << 0., 0., 1., 0.;
desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 1.0, 0.0, 0.0, 0.0;
+ sequence << 1., 0., 0., 0.;
desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 0.0, 0.0, 1.0;
+ sequence << 0., 0., 0., 1.;
desiredGait_.block(2 * N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 0.0, 0.0;
+ sequence << 0., 1., 0., 0.;
desiredGait_.block(3 * N, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_custom_gallop() {
- // Number of timesteps in a half period of gait
- long int N = currentGait_.rows() / 4;
-
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+void Gait::createCustomGallop() {
+ long int N = nRows_ / 4;
+ desiredGait_ = MatrixN::Zero(nRows_, 4);
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 0.0, 1.0, 0.0;
+ RowVector4 sequence;
+ sequence << 1., 0., 1., 0.;
desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 1.0, 0.0, 0.0, 1.0;
+ sequence << 1., 0., 0., 1.;
desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 0.0, 1.0;
+ sequence << 0., 1., 0., 1.;
desiredGait_.block(2 * N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 1.0, 0.0;
+ sequence << 0., 1., 1., 0.;
desiredGait_.block(3 * N, 0, N, 4) = sequence.colwise().replicate(N);
}
-double Gait::getPhaseDuration(int i, int j, double value) {
- double t_phase = 1;
- int a = i;
+double Gait::getPhaseDuration(int i, int j) { return getElapsedTime(i, j) + getRemainingTime(i, j); }
- // Looking for the end of the swing/stance phase in currentGait_
- while ((i + 1 < currentGait_.rows()) && (currentGait_(i + 1, j) == value)) {
- i++;
- t_phase++;
+double Gait::getRemainingTime(int i, int j) {
+ double state = currentGait_(i, j);
+ double nPhase = 1;
+ int row = i;
+
+ while ((row < nRows_ - 1) && (currentGait_(row + 1, j) == state)) {
+ row++;
+ nPhase++;
}
- // If we reach the end of currentGait_ we continue looking for the end of the swing/stance phase in desiredGait_
- if (i + 1 == currentGait_.rows()) {
- int k = 0;
- while ((k < desiredGait_.rows()) && (desiredGait_(k, j) == value)) {
- k++;
- t_phase++;
+
+ if (row == nRows_ - 1) {
+ row = 0;
+ while ((row < nRows_) && (desiredGait_(row, j) == state)) {
+ row++;
+ nPhase++;
}
}
- // We suppose that we found the end of the swing/stance phase either in currentGait_ or desiredGait_
+ return nPhase * dt_;
+}
- remainingTime_ = t_phase;
+double Gait::getElapsedTime(int i, int j) {
+ double state = currentGait_(i, j);
+ double nPhase = 0;
+ int row = i;
- // Looking for the beginning of the swing/stance phase in currentGait_
- while ((a > 0) && (currentGait_(a - 1, j) == value)) {
- a--;
- t_phase++;
+ while ((row > 0) && (currentGait_(row - 1, j) == state)) {
+ row--;
+ nPhase++;
}
- // If we reach the end of currentGait_ we continue looking for the beginning of the swing/stance phase in pastGait_
- if (a == 0) {
- while ((a < pastGait_.rows()) && (pastGait_(a, j) == value)) {
- a++;
- t_phase++;
+
+ if (row == 0) {
+ row = nRows_;
+ while ((row > 0) && (pastGait_(row - 1, j) == state)) {
+ row--;
+ nPhase++;
}
}
- // We suppose that we found the beginning of the swing/stance phase either in currentGait_ or pastGait_
-
- // TODO: Handle infinite swing / stance phases
-
- return t_phase * dt_; // Take into account time step value
+ return nPhase * dt_;
}
-void Gait::updateGait(int const k, int const k_mpc, int const joystickCode) {
+void Gait::update(int const k, int const k_mpc, int const joystickCode) {
changeGait(k, k_mpc, joystickCode);
if (k % k_mpc == 0 && k > 0) rollGait();
}
bool Gait::changeGait(int const k, int const k_mpc, int const code) {
- if (code != 0 && switch_to_gait_ == 0) {
- switch_to_gait_ = code;
+ if (code != 0 && switchToGait_ == 0) {
+ switchToGait_ = code;
}
- if (switch_to_gait_ != 0 && ((k - k_mpc) % (k_mpc * currentGait_.rows() / 2) == 0)) {
- is_static_ = false;
- switch (switch_to_gait_) {
+ if (switchToGait_ != 0 && ((k - k_mpc) % (k_mpc * nRows_ / 2) == 0)) {
+ isStatic_ = false;
+ switch (switchToGait_) {
/*case 1:
- create_pacing();
+ createPacing();
break;
case 2:
- create_bounding();
+ createBounding();
break;*/
case 3:
- create_trot();
+ createTrot();
break;
case 1:
- is_static_ = true;
- create_static();
+ isStatic_ = true;
+ createStatic();
break;
}
- switch_to_gait_ = 0;
+ switchToGait_ = 0;
}
- return is_static_;
+ return isStatic_;
}
void Gait::rollGait() {
- // Transfer current gait into past gait
- for (long int m = pastGait_.rows() - 1; m > 0; m--) // TODO: Find optimized circular shift function
- {
- pastGait_.row(m).swap(pastGait_.row(m - 1));
- }
- pastGait_.row(0) = currentGait_.row(0);
+ // Age past gait
+ pastGait_.topRows(nRows_ - 1) = pastGait_.bottomRows(nRows_ - 1);
+ pastGait_.row(nRows_ - 1) = currentGait_.row(0);
- // Entering new contact phase, store positions of feet that are now in contact
- if (!currentGait_.row(0).isApprox(currentGait_.row(1))) {
- newPhase_ = true;
- } else {
- newPhase_ = false;
- }
+ newPhase_ = !currentGait_.row(0).isApprox(currentGait_.row(1));
// Age current gait
- for (int index = 1; index < currentGait_.rows(); index++) {
- currentGait_.row(index - 1).swap(currentGait_.row(index));
- }
-
- // Insert a new line from desired gait into current gait
- currentGait_.row(currentGait_.rows() - 1) = desiredGait_.row(0);
+ currentGait_.topRows(nRows_ - 1) = currentGait_.bottomRows(nRows_ - 1);
+ currentGait_.row(nRows_ - 1) = desiredGait_.row(0);
// Age desired gait
- for (int index = 1; index < currentGait_.rows(); index++) {
- desiredGait_.row(index - 1).swap(desiredGait_.row(index));
- }
-}
-
-bool Gait::setGait(MatrixN const& gaitMatrix) {
- std::cout << "Gait matrix received by setGait:" << std::endl;
- std::cout << gaitMatrix << std::endl;
-
- // Todo: Check if the matrix is a static gait (only ones)
- return false;
-}
+ desiredGait_.topRows(nRows_ - 1) = desiredGait_.bottomRows(nRows_ - 1);
+ desiredGait_.row(nRows_ - 1) = currentGait_.row(nRows_ - 1);
+}
\ No newline at end of file
diff --git a/src/Solo3D/FootTrajectoryGeneratorBezier.cpp b/src/Solo3D/FootTrajectoryGeneratorBezier.cpp
index 381b39bf..3fba2885 100644
--- a/src/Solo3D/FootTrajectoryGeneratorBezier.cpp
+++ b/src/Solo3D/FootTrajectoryGeneratorBezier.cpp
@@ -1,12 +1,12 @@
+#include "qrw/Solo3D/FootTrajectoryGeneratorBezier.hpp"
+
+#include <chrono>
#include <example-robot-data/path.hpp>
-#include "pinocchio/math/rpy.hpp"
-#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/frames.hpp"
-#include "qrw/Solo3D/FootTrajectoryGeneratorBezier.hpp"
-
-#include <chrono>
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/parsers/urdf.hpp"
using namespace std::chrono;
@@ -88,8 +88,7 @@ void FootTrajectoryGeneratorBezier::initialize(Params& params, Gait& gaitIn, Sur
z_margin_ = z_margin_in;
// Path to the robot URDF
- const std::string filename =
- std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf");
+ const std::string filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf");
// Build model from urdf (base is not free flyer)
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
@@ -516,8 +515,8 @@ void FootTrajectoryGeneratorBezier::update(int k, MatrixN const& targetFootstep,
// 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;
+ t_swing[i] = gait_->getPhaseDuration(0, feet[j]);
+ double value = gait_->getElapsedTime(0, feet[j]) - dt_wbc;
t0s[i] = std::max(0.0, value);
}
} else {
@@ -551,56 +550,6 @@ void FootTrajectoryGeneratorBezier::update(int k, MatrixN const& targetFootstep,
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;
-}
-
void FootTrajectoryGeneratorBezier::update_position_FK(VectorN const& q) {
// Get position of the feet in world frame, using estimated state q
q_FK_.head(3) = q.head(3);
diff --git a/src/Solo3D/FootstepPlannerQP.cpp b/src/Solo3D/FootstepPlannerQP.cpp
index c4195bf0..5b3fb8b0 100644
--- a/src/Solo3D/FootstepPlannerQP.cpp
+++ b/src/Solo3D/FootstepPlannerQP.cpp
@@ -329,7 +329,7 @@ void FootstepPlannerQP::computeNextFootstep(int i, int j, Vector6 const& b_v, Ve
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
+ double t_stance = gait_->getPhaseDuration(i, j); // 1.0 for stance phase
// Add symmetry term
footstep = t_stance * 0.5 * b_v.head(3);
@@ -411,8 +411,8 @@ void FootstepPlannerQP::update_remaining_time(int k) {
// For each foot in swing phase get remaining duration of the swing phase
for (int foot = 0; foot < (int)feet_.size(); foot++) {
int i = feet_[foot];
- t_swing[i] = gait_->getPhaseDuration(0, feet_[foot], 0.0); // 0.0 for swing phase
- double value = t_swing[i] - (gait_->getRemainingTime() * k_mpc - ((k + 1) % k_mpc)) * dt_wbc - dt_wbc;
+ t_swing[i] = gait_->getPhaseDuration(0, feet_[foot]);
+ double value = gait_->getElapsedTime(0, feet_[foot]) - dt_wbc;
t0s[i] = std::max(0.0, value);
}
} else {
diff --git a/src/WbcWrapper.cpp b/src/WbcWrapper.cpp
index f1e69a8c..b9262796 100644
--- a/src/WbcWrapper.cpp
+++ b/src/WbcWrapper.cpp
@@ -10,7 +10,7 @@
WbcWrapper::WbcWrapper()
: M_(Eigen::Matrix<double, 18, 18>::Zero()),
Jc_(Eigen::Matrix<double, 12, 6>::Zero()),
- k_since_contact_(Eigen::Matrix<double, 1, 4>::Zero()),
+ k_since_contact_(RowVector4::Zero()),
bdes_(Vector7::Zero()),
qdes_(Vector12::Zero()),
vdes_(Vector12::Zero()),
--
GitLab