diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index 71591ef8aec83dd4d98caf4c2801bd06478f7f12..894f79bbe1b85d3a694c170a4d376dfb6e41247a 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -33,6 +33,7 @@ public:
/// \brief Default constructor
///
/// \param[in] dt_in Time step of the contact sequence (time step of the MPC)
+ /// \param[in] dt_wbc_in Time step of whole body control
/// \param[in] T_mpc_in MPC period (prediction horizon)
/// \param[in] h_ref_in Reference height for the trunk
/// \param[in] shoulderIn Position of shoulders in local frame
@@ -40,6 +41,7 @@ public:
///
////////////////////////////////////////////////////////////////////////////////////////////////
void initialize(double dt_in,
+ double dt_wbc_in,
double T_mpc_in,
double h_ref_in,
MatrixN const& shouldersIn,
@@ -53,18 +55,35 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
~FootstepPlanner() {}
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Refresh footsteps locations (computation and update of relevant matrices)
+ ///
+ /// \param[in] refresh true if we move one step further in the gait
+ /// \param[in] k number of remaining wbc time step for the current mpc time step (wbc frequency is higher so there are inter-steps)
+ /// \param[in] q current position vector of the flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] b_v current velocity vector of the flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ MatrixN updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref);
+
+ MatrixN getFootsteps();
+ MatrixN getTargetFootsteps();
+ MatrixN getRz();
+
+private:
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute the desired location of footsteps and update relevant matrices
///
- /// \param[in] q current position vector of the flying base in world frame (linear and angular stacked)
- /// \param[in] v current velocity vector of the flying base in world frame (linear and angular stacked)
- /// \param[in] b_vref desired velocity vector of the flying base in base frame (linear and angular stacked)
+ /// \param[in] k number of remaining wbc time step for the current mpc time step (wbc frequency is higher so there are inter-steps)
+ /// \param[in] q current position vector of the flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] b_v current velocity vector of the flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
///
////////////////////////////////////////////////////////////////////////////////////////////////
- MatrixN computeTargetFootstep(VectorN const& q,
- Vector6 const& v,
- Vector6 const& b_vref);
+ MatrixN computeTargetFootstep(int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -73,22 +92,17 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
void updateNewContact();
- MatrixN getFootsteps();
- MatrixN getTargetFootsteps();
-
-private:
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute a X by 13 matrix containing the remaining number of steps of each phase of the gait (first column)
/// and the [x, y, z]^T desired position of each foot for each phase of the gait (12 other columns).
/// For feet currently touching the ground the desired position is where they currently are.
///
- /// \param[in] q current position vector of the flying base in world frame(linear and angular stacked)
- /// \param[in] v current velocity vector of sthe flying base in world frame(linear and angular stacked)
- /// \param[in] vref desired velocity vector of the flying base in world frame(linear and angular stacked)
+ /// \param[in] b_v current velocity vector of sthe flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void computeFootsteps(VectorN const& q, Vector6 const& v, Vector6 const& vref);
+ void computeFootsteps(int k, Vector6 const& b_v, Vector6 const& b_vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -96,11 +110,13 @@ private:
///
/// \param[in] i considered phase (row of the gait matrix)
/// \param[in] j considered foot (col of the gait matrix)
+ /// \param[in] b_v current velocity vector of sthe flying base in horizontal frame (linear and angular stacked)
+ /// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
///
/// \retval Matrix with the next footstep positions
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void computeNextFootstep(int i, int j);
+ void computeNextFootstep(int i, int j, Vector6 const& b_v, Vector6 const& b_vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -114,6 +130,7 @@ private:
Gait* gait_; // Gait object to hold the gait informations
double dt; // Time step of the contact sequence (time step of the MPC)
+ double dt_wbc; // Time step of the whole body control
double T_gait; // Gait period
double T_mpc; // MPC period (prediction horizon)
double h_ref; // Reference height for the trunk
@@ -128,12 +145,13 @@ private:
// Constant sized matrices
Matrix34 shoulders_; // Position of shoulders in local frame
- Matrix34 currentFootstep_; // Feet matrix in world frame
- Matrix34 nextFootstep_; // Feet matrix in world frame
- Matrix34 targetFootstep_;
+ Matrix34 currentFootstep_; // Feet matrix
+ Matrix34 nextFootstep_; // Temporary matrix to perform computations
+ Matrix34 targetFootstep_; // In horizontal frame
+ Matrix34 o_targetFootstep_; // targetFootstep_ in world frame
std::vector<Matrix34> footsteps_;
- Matrix3 Rz; // Predefined matrices for compute_footstep function
+ MatrixN Rz; // Rotation matrix along z axis
VectorN dt_cum;
VectorN yaws;
VectorN dx;
@@ -143,8 +161,6 @@ private:
Vector3 q_dxdy;
Vector3 RPY_;
Eigen::Quaterniond quat_;
- Vector3 b_v;
- Vector6 b_vref;
};
#endif // FOOTSTEPPLANNER_H_INCLUDED
diff --git a/include/qrw/StatePlanner.hpp b/include/qrw/StatePlanner.hpp
index 0bca1cf85c98105c3c0f12f8df8101d88ecf8349..a6100b3118bd1274526d85576e8bc2fc2e2cf3a1 100644
--- a/include/qrw/StatePlanner.hpp
+++ b/include/qrw/StatePlanner.hpp
@@ -46,9 +46,9 @@ public:
/// linear velocity and angular velocity vertically stacked. The first column contains
/// the current state while the remaining N columns contains the desired future states.
///
- /// \param[in] q current position vector of the flying base in world frame (linear and angular stacked)
- /// \param[in] v current velocity vector of the flying base in world frame (linear and angular stacked)
- /// \param[in] vref desired velocity vector of the flying base in world frame (linear and angular stacked)
+ /// \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
///
////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 97690c70b42a64d9ff25e8e89652e07b22e4fc6d..d963f2c635f46adaf321edb3e904c50f9c46c498 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -1,7 +1,6 @@
#include "qrw/gepadd.hpp"
#include "qrw/InvKin.hpp"
#include "qrw/MPC.hpp"
-// #include "qrw/Planner.hpp"
#include "qrw/StatePlanner.hpp"
#include "qrw/Gait.hpp"
#include "qrw/FootstepPlanner.hpp"
@@ -121,15 +120,14 @@ struct FootstepPlannerPythonVisitor : public bp::def_visitor<FootstepPlannerPyth
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
.def("getFootsteps", &FootstepPlanner::getFootsteps, "Get footsteps_ matrix.\n")
+ .def("getRz", &FootstepPlanner::getRz, "Get rotation along z matrix.\n")
- .def("initialize", &FootstepPlanner::initialize, bp::args("dt_in", "T_mpc_in", "h_ref_in", "shouldersIn", "gaitIn", "N_gait"),
+ .def("initialize", &FootstepPlanner::initialize, bp::args("dt_in", "dt_wbc_in", "T_mpc_in", "h_ref_in", "shouldersIn", "gaitIn", "N_gait"),
"Initialize FootstepPlanner from Python.\n")
// Compute target location of footsteps from Python
- .def("computeTargetFootstep", &FootstepPlanner::computeTargetFootstep, bp::args("q", "v", "b_vref"),
- "Compute target location of footsteps from Python.\n")
-
- .def("updateNewContact", &FootstepPlanner::updateNewContact, "Refresh feet position when entering a new contact phase.\n");
+ .def("updateFootsteps", &FootstepPlanner::updateFootsteps, bp::args("refresh", "k", "q", "b_v", "b_vref"),
+ "Update and compute location of footsteps from Python.\n");
}
diff --git a/scripts/Controller.py b/scripts/Controller.py
index c9c2ec9ed4e0f33a01f43a7c80d1a4242f2454c2..cf5766158266e9b311b6d718d1ea6266059514f4 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -131,7 +131,7 @@ class Controller:
shoulders[0, :] = [0.1946, 0.1946, -0.1946, -0.1946]
shoulders[1, :] = [0.14695, -0.14695, 0.14695, -0.14695]
self.footstepPlanner = lqrw.FootstepPlanner()
- self.footstepPlanner.initialize(dt_mpc, T_mpc, self.h_ref, shoulders.copy(), self.gait, N_gait)
+ self.footstepPlanner.initialize(dt_mpc, dt_wbc, T_mpc, self.h_ref, shoulders.copy(), self.gait, N_gait)
self.footTrajectoryGenerator = lqrw.FootTrajectoryGenerator()
self.footTrajectoryGenerator.initialize(0.05, 0.07, self.fsteps_init.copy(), shoulders.copy(),
@@ -169,6 +169,10 @@ class Controller:
self.qmes12 = np.zeros((19, 1))
self.vmes12 = np.zeros((18, 1))
+ self.v_estim = np.zeros((18, 1))
+ self.yaw_estim = 0.0
+ self.RPY_filt = np.zeros(3)
+
self.error_flag = 0
self.q_security = np.array([np.pi*0.4, np.pi*80/180, np.pi] * 4)
@@ -201,71 +205,60 @@ class Controller:
# Process state estimator
self.estimator.run_filter(self.k, self.gait.getCurrentGait(),
device, self.footTrajectoryGenerator.getFootPosition())
+
t_filter = time.time()
# Update state for the next iteration of the whole loop
- if self.k > 1:
- self.q[:, 0] = self.estimator.q_filt[:, 0]
- oMb = pin.SE3(pin.Quaternion(self.q[3:7, 0:1]), self.q[0:3, 0:1])
- self.v[0:3, 0:1] = oMb.rotation @ self.estimator.v_filt[0:3, 0:1]
- self.v[3:6, 0:1] = oMb.rotation @ self.estimator.v_filt[3:6, 0:1]
- self.v[6:, 0] = self.estimator.v_filt[6:, 0]
-
- # Update estimated position of the robot
- self.v_estim[0:3, 0:1] = oMb.rotation.transpose() @ self.joystick.v_ref[0:3, 0:1]
- self.v_estim[3:6, 0:1] = oMb.rotation.transpose() @ self.joystick.v_ref[3:6, 0:1]
- if not self.gait.getIsStatic():
- self.q_estim[:, 0] = pin.integrate(self.solo.model,
- self.q, self.v_estim * self.myController.dt)
- self.yaw_estim = (utils_mpc.quaternionToRPY(self.q_estim[3:7, 0]))[2, 0]
- else:
- self.planner.q_static[:] = pin.integrate(self.solo.model,
- self.planner.q_static, self.v_estim * self.myController.dt)
- self.planner.RPY_static[:, 0:1] = utils_mpc.quaternionToRPY(self.planner.q_static[3:7, 0])
- else:
- self.yaw_estim = 0.0
- self.q_estim = self.q.copy()
- oMb = pin.SE3(pin.Quaternion(self.q[3:7, 0:1]), self.q[0:3, 0:1])
- self.v_estim = self.v.copy()
+ self.v_estim[0:3, 0] = self.joystick.v_ref[0:3, 0] # TODO: Joystick velocity given in base frame and not
+ self.v_estim[3:6, 0] = self.joystick.v_ref[3:6, 0] # in horizontal frame (case of non flat ground)
+ self.v_estim[6:, 0] = 0.0
+ if not self.gait.getIsStatic():
+ # Integration to get evolution of perfect x, y and yaw
+ self.q[3:7, 0] = self.estimator.EulerToQuaternion([0.0, 0.0, self.yaw_estim]) # Remove pitch and roll
+ self.q[:, 0] = pin.integrate(self.solo.model, self.q, self.v_estim * self.myController.dt)
- # Update gait
- self.gait.updateGait(self.k, self.k_mpc, self.q[0:7, 0:1], self.joystick.joystick_code)
+ # Mix perfect x and y with height measurement
+ self.q[2, 0] = self.estimator.q_filt[2, 0]
- # Update footsteps if new contact phase
- if(self.k % self.k_mpc == 0 and self.k != 0 and self.gait.isNewPhase()):
- self.footstepPlanner.updateNewContact()
+ # Mix perfect yaw with pitch and roll measurements
+ self.yaw_estim = (utils_mpc.quaternionToRPY(self.q[3:7, 0]))[2, 0]
+ self.RPY_filt = utils_mpc.quaternionToRPY(self.estimator.q_filt[3:7, 0])
+ self.q[3:7, 0] = self.estimator.EulerToQuaternion([self.RPY_filt[0], self.RPY_filt[1], self.yaw_estim])
- """// Get the reference velocity in world frame (given in base frame)
- Eigen::Quaterniond quat(q(6, 0), q(3, 0), q(4, 0), q(5, 0)); // w, x, y, z
- RPY << pinocchio::rpy::matrixToRpy(quat.toRotationMatrix());
- double c = std::cos(RPY(2, 0));
- double s = std::sin(RPY(2, 0));
- R_2.block(0, 0, 2, 2) << c, -s, s, c;
- R_2(2, 2) = 1.0;
- 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);"""
+ # Actuators measurements
+ self.q[7:, 0] = self.estimator.q_filt[7:, 0]
- o_v_ref = np.zeros((6, 1))
- o_v_ref[0:3, 0:1] = oMb.rotation @ self.joystick.v_ref[0:3, 0:1]
- o_v_ref[3:6, 0:1] = oMb.rotation @ self.joystick.v_ref[3:6, 0:1]
+ # Velocities are the one estimated by the estimator
+ self.v = self.estimator.v_filt.copy()
+ else:
+ pass
+ # TODO: Adapt static mode to new version of the code
+ # self.planner.q_static[:] = pin.integrate(self.solo.model,
+ # self.planner.q_static, self.v_estim * self.myController.dt)
+ # self.planner.RPY_static[:, 0:1] = utils_mpc.quaternionToRPY(self.planner.q_static[3:7, 0])
+
+ # Update gait
+ self.gait.updateGait(self.k, self.k_mpc, self.q[0:7, 0:1], self.joystick.joystick_code)
# Compute target footstep based on current and reference velocities
- targetFootstep = self.footstepPlanner.computeTargetFootstep(self.q[0:7, 0:1], self.v[0:6, 0:1].copy(), o_v_ref)
+ 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:7, 0:1],
+ self.v[0:6, 0:1].copy(),
+ self.joystick.v_ref[0:6, 0])
+
+ # Transformation matrices between world and horizontal frames
+ oRh = self.footstepPlanner.getRz()
+ oTh = np.array([[self.q[0, 0]], [self.q[1, 0]], [0.0]])
# Update pos, vel and acc references for feet
# TODO: Make update take as parameters current gait, swing phase duration and remaining time
- self.footTrajectoryGenerator.update(self.k, targetFootstep)
-
- # Retrieve data from C++ planner
- # TODO
- """self.fsteps = self.planner.get_fsteps()
- self.gait = self.planner.get_gait()
- self.goals = self.planner.get_goals()
- self.vgoals = self.planner.get_vgoals()
- self.agoals = self.planner.get_agoals()"""
-
- # Run state planner (outputs the reference trajectory of the CoM / base)
- self.statePlanner.computeReferenceStates(self.q[0:7, 0:1], self.v[0:6, 0:1].copy(), o_v_ref, 0.0)
+ self.footTrajectoryGenerator.update(self.k, o_targetFootstep)
+
+ # Run state planner (outputs the reference trajectory of the base)
+ self.statePlanner.computeReferenceStates(self.q[0:7, 0:1], self.v[0:6, 0:1].copy(),
+ self.joystick.v_ref[0:6, 0:1], 0.0)
+
# Result can be retrieved with self.statePlanner.getReferenceStates()
xref = self.statePlanner.getReferenceStates()
fsteps = self.footstepPlanner.getFootsteps()
@@ -273,21 +266,14 @@ class Controller:
t_planner = time.time()
- """if(self.k > 11000):
-
- from IPython import embed
- embed()"""
-
- # self.planner.setGait(self.planner.gait)
-
- # Process MPC once every k_mpc iterations of TSID
+ # Solve MPC problem once every k_mpc iterations of the main loop
if (self.k % self.k_mpc) == 0:
try:
self.mpc_wrapper.solve(self.k, xref, fsteps, cgait)
except ValueError:
print("MPC Problem")
- # Retrieve reference contact forces
+ # Retrieve reference contact forces in horizontal frame
self.x_f_mpc = self.mpc_wrapper.get_latest_result()
t_mpc = time.time()
@@ -295,14 +281,14 @@ class Controller:
# Target state for the whole body control
self.x_f_wbc = (self.x_f_mpc[:, 0]).copy()
if not self.gait.getIsStatic():
- self.x_f_wbc[0] = self.q_estim[0, 0]
- self.x_f_wbc[1] = self.q_estim[1, 0]
+ self.x_f_wbc[0] = self.myController.dt * xref[6, 1]
+ self.x_f_wbc[1] = self.myController.dt * xref[7, 1]
self.x_f_wbc[2] = self.h_ref
self.x_f_wbc[3] = 0.0
self.x_f_wbc[4] = 0.0
- self.x_f_wbc[5] = self.yaw_estim
+ self.x_f_wbc[5] = self.myController.dt * xref[11, 1]
else: # Sort of position control to avoid slow drift
- self.x_f_wbc[0:3] = self.planner.q_static[0:3, 0]
+ self.x_f_wbc[0:3] = self.planner.q_static[0:3, 0] # TODO: Adapt to new code
self.x_f_wbc[3:6] = self.planner.RPY_static[:, 0]
self.x_f_wbc[6:12] = xref[6:, 1]
@@ -310,17 +296,20 @@ class Controller:
# If nothing wrong happened yet in the WBC controller
if (not self.myController.error) and (not self.joystick.stop):
- # Get velocity in base frame for pinocchio
- self.b_v[0:3, 0:1] = oMb.rotation.transpose() @ self.v[0:3, 0:1]
- self.b_v[3:6, 0:1] = oMb.rotation.transpose() @ self.v[3:6, 0:1]
- self.b_v[6:, 0] = self.v[6:, 0]
+ self.q_wbc = np.zeros((19, 1))
+ self.q_wbc[2, 0] = self.q[2, 0]
+ self.q_wbc[3:7, 0] = self.estimator.EulerToQuaternion([self.RPY_filt[0], self.RPY_filt[1], 0.0])
+ self.q_wbc[7:, 0] = self.q[7:, 0]
+
+ # Get velocity in base frame for Pinocchio
+ self.b_v = self.v.copy()
# Run InvKin + WBC QP
- self.myController.compute(self.q, self.b_v, self.x_f_wbc[:12],
+ self.myController.compute(self.q_wbc, self.b_v, self.x_f_wbc[:12],
self.x_f_wbc[12:], cgait[0, :],
- self.footTrajectoryGenerator.getFootPosition(),
- self.footTrajectoryGenerator.getFootVelocity(),
- self.footTrajectoryGenerator.getFootAcceleration())
+ oRh.transpose() @ (self.footTrajectoryGenerator.getFootPosition() - oTh),
+ oRh.transpose() @ self.footTrajectoryGenerator.getFootVelocity(),
+ oRh.transpose() @ self.footTrajectoryGenerator.getFootAcceleration())
# Quantities sent to the control board
self.result.P = 3.0 * np.ones(12)
@@ -329,14 +318,9 @@ class Controller:
self.result.v_des[:] = self.myController.vdes[6:, 0]
self.result.tau_ff[:] = 0.5 * self.myController.tau_ff
+ # Display robot in Gepetto corba viewer
"""if self.k % 5 == 0:
- self.solo.display(self.q)
- #self.view.display(self.q)
- #print("Pass")
- np.set_printoptions(linewidth=200, precision=2)
- print("###")
- #print(self.q.ravel())
- print(self.myController.tau_ff)"""
+ self.solo.display(self.q)"""
t_wbc = time.time()
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 05c15924b20f0844bc28ce817d1d97fe97c7379c..bcb2b723d7c2b7027c8b2cde7fc8c9f1d0d60563 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -124,8 +124,8 @@ class LoggerControl():
self.esti_kf_Z[self.i] = estimator.Z[:, 0]
# Logging from the main loop
- self.loop_o_q_int[self.i] = loop.q_estim[:, 0]
- self.loop_o_v[self.i] = loop.v_estim[:, 0]
+ self.loop_o_q_int[self.i] = loop.q[:, 0]
+ self.loop_o_v[self.i] = loop.v[:, 0]
# Logging from the planner
# self.planner_q_static[self.i] = planner.q_static[:]
@@ -268,7 +268,7 @@ class LoggerControl():
# plt.plot(t_range, self.log_q[i, :], "grey", linewidth=4)
# plt.plot(t_range[:-2], self.log_x_invkin[i, :-2], "g", linewidth=2)
# plt.plot(t_range[:-2], self.log_x_ref_invkin[i, :-2], "violet", linewidth=2, linestyle="--")
- plt.legend(["Robot state", "Robot reference state"], prop={'size': 8})
+ plt.legend(["Robot state", "Robot reference state", "Ground truth"], prop={'size': 8})
plt.ylabel(lgd[i])
plt.suptitle("Measured & Reference position and orientation")
@@ -285,14 +285,14 @@ class LoggerControl():
plt.plot(t_range, self.joy_v_ref[:, i], "r", linewidth=3)
if i < 3:
plt.plot(t_range, self.mocap_b_v[:, i], "k", linewidth=3)
- plt.plot(t_range, self.esti_FK_lin_vel[:, i], "violet", linewidth=3, linestyle="--")
+ # plt.plot(t_range, self.esti_FK_lin_vel[:, i], "violet", linewidth=3, linestyle="--")
else:
plt.plot(t_range, self.mocap_b_w[:, i-3], "k", linewidth=3)
# plt.plot(t_range, self.log_dq[i, :], "g", linewidth=2)
# plt.plot(t_range[:-2], self.log_dx_invkin[i, :-2], "g", linewidth=2)
# plt.plot(t_range[:-2], self.log_dx_ref_invkin[i, :-2], "violet", linewidth=2, linestyle="--")
- plt.legend(["Robot state", "Robot reference state"], prop={'size': 8})
+ plt.legend(["Robot state", "Robot reference state", "Ground truth"], prop={'size': 8})
plt.ylabel(lgd[i])
plt.suptitle("Measured & Reference linear and angular velocities")
diff --git a/scripts/solo12InvKin.py b/scripts/solo12InvKin.py
index 544b10ffa9d44a940e040e01b6d2dc71b84a3ebe..57fd4580365b5d12e440d35ba5af2e9340c5f073 100644
--- a/scripts/solo12InvKin.py
+++ b/scripts/solo12InvKin.py
@@ -1,10 +1,10 @@
from example_robot_data import load
-import time
import numpy as np
import pinocchio as pin
import libquadruped_reactive_walking as lrw
+
class Solo12InvKin:
def __init__(self, dt):
self.robot = load('solo12')
@@ -181,7 +181,6 @@ class Solo12InvKin:
self.pfeet_err.append(e1)
vfeet_ref.append(vref)
-
# BASE POSITION
idx = self.BASE_ID
pos = self.rdata.oMf[idx].translation
@@ -252,7 +251,7 @@ class Solo12InvKin:
print(invJ)
print("acc:")
print(acc)
-
+
ddq = invJ @ acc
self.q_cmd = pin.integrate(self.robot.model, q, invJ @ x_err)
self.dq_cmd = invJ @ dx_ref
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index 31a4c6de2b78ca02b98362fd4b1c71988b1d7d63..39b814c2a1cbb58e3c605d30737839f50a9ebc52 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -7,7 +7,7 @@ FootstepPlanner::FootstepPlanner()
, L(0.155)
, nextFootstep_(Matrix34::Zero())
, footsteps_()
- , Rz(Matrix3::Zero())
+ , Rz(MatrixN::Zero(3, 3))
, dt_cum()
, yaws()
, dx()
@@ -15,13 +15,12 @@ FootstepPlanner::FootstepPlanner()
, q_tmp(Vector3::Zero())
, q_dxdy(Vector3::Zero())
, RPY_(Vector3::Zero())
- , b_v(Vector3::Zero())
- , b_vref(Vector6::Zero())
{
// Empty
}
void FootstepPlanner::initialize(double dt_in,
+ double dt_wbc_in,
double T_mpc_in,
double h_ref_in,
MatrixN const& shouldersIn,
@@ -29,6 +28,7 @@ void FootstepPlanner::initialize(double dt_in,
int N_gait)
{
dt = dt_in;
+ dt_wbc = dt_wbc_in;
T_mpc = T_mpc_in;
h_ref = h_ref_in;
n_steps = (int)std::lround(T_mpc_in / dt_in);
@@ -36,6 +36,7 @@ void FootstepPlanner::initialize(double dt_in,
currentFootstep_ = shouldersIn.block(0, 0, 3, 4);
gait_ = &gaitIn;
targetFootstep_ = shouldersIn;
+ o_targetFootstep_ = shouldersIn;
dt_cum = VectorN::Zero(N_gait);
yaws = VectorN::Zero(N_gait);
dx = VectorN::Zero(N_gait);
@@ -47,7 +48,33 @@ void FootstepPlanner::initialize(double dt_in,
Rz(2, 2) = 1.0;
}
-void FootstepPlanner::computeFootsteps(VectorN const& q, Vector6 const& v, Vector6 const& vref)
+MatrixN FootstepPlanner::updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref)
+{
+ // Update location of feet in stance phase (for those which just entered stance phase)
+ if (refresh && gait_->isNewPhase())
+ {
+ updateNewContact();
+ }
+
+ // Feet in contact with the ground are moving in base frame (they don't move in world frame)
+ double rotation_yaw = dt_wbc * b_vref(5); // Rotation along Z for the last time step
+ double c = std::cos(rotation_yaw);
+ double s = std::sin(rotation_yaw);
+ Rz.topLeftCorner<2, 2>() << c, s, -s, c;
+ Vector2 dpos = dt_wbc * b_vref.head(2); // Displacement along X and Y for the last time step
+ for (int j = 0; j < 4; j++)
+ {
+ if (gait_->getCurrentGaitCoeff(0, j) == 1.0)
+ {
+ currentFootstep_.block(0, j, 2, 1) = Rz * (currentFootstep_.block(0, j, 2, 1) - dpos);
+ }
+ }
+
+ // Compute location of footsteps
+ return computeTargetFootstep(k, q, b_v, b_vref);
+}
+
+void FootstepPlanner::computeFootsteps(int k, Vector6 const& b_v, Vector6 const& b_vref)
{
for (uint i = 0; i < footsteps_.size(); i++)
{
@@ -66,37 +93,32 @@ void FootstepPlanner::computeFootsteps(VectorN const& q, Vector6 const& v, Vecto
// Cumulative time by adding the terms in the first column (remaining number of timesteps)
// Get future yaw yaws compared to current position
- dt_cum(0) = dt;
- yaws(0) = vref(5) * dt_cum(0) + RPY_(2);
+ dt_cum(0) = dt_wbc * k;
+ yaws(0) = b_vref(5) * dt_cum(0);
for (uint j = 1; j < footsteps_.size(); j++)
{
dt_cum(j) = gait.row(j).isZero() ? dt_cum(j - 1) : dt_cum(j - 1) + dt;
- yaws(j) = vref(5) * dt_cum(j) + RPY_(2);
+ yaws(j) = b_vref(5) * dt_cum(j);
}
// Displacement following the reference velocity compared to current position
- if (vref(5, 0) != 0)
+ if (b_vref(5, 0) != 0)
{
for (uint j = 0; j < footsteps_.size(); j++)
{
- dx(j) = (v(0) * std::sin(vref(5) * dt_cum(j)) + v(1) * (std::cos(vref(5) * dt_cum(j)) - 1.0)) / vref(5);
- dy(j) = (v(1) * std::sin(vref(5) * dt_cum(j)) - v(0) * (std::cos(vref(5) * dt_cum(j)) - 1.0)) / vref(5);
+ dx(j) = (b_v(0) * std::sin(b_vref(5) * dt_cum(j)) + b_v(1) * (std::cos(b_vref(5) * dt_cum(j)) - 1.0)) / b_vref(5);
+ dy(j) = (b_v(1) * std::sin(b_vref(5) * dt_cum(j)) - b_v(0) * (std::cos(b_vref(5) * dt_cum(j)) - 1.0)) / b_vref(5);
}
}
else
{
for (uint j = 0; j < footsteps_.size(); j++)
{
- dx(j) = v(0) * dt_cum(j);
- dy(j) = v(1) * dt_cum(j);
+ dx(j) = b_v(0) * dt_cum(j);
+ dy(j) = b_v(1) * dt_cum(j);
}
}
- // Get current and reference velocities in base frame (rotated yaw)
- b_v = Rz.transpose() * v.head(3);
- b_vref.head(3) = Rz.transpose() * vref.head(3);
- b_vref.tail(3) = Rz.transpose() * vref.tail(3);
-
// Update the footstep matrix depending on the different phases of the gait (swing & stance)
int i = 1;
while (!gait.row(i).isZero())
@@ -110,10 +132,6 @@ void FootstepPlanner::computeFootsteps(VectorN const& q, Vector6 const& v, Vecto
}
}
- // Current position without height
- Vector3 q_tmp = q.head(3);
- q_tmp(2) = 0.0;
-
// Feet that were in swing phase and are now in stance phase need to be updated
for (int j = 0; j < 4; j++)
{
@@ -123,31 +141,31 @@ void FootstepPlanner::computeFootsteps(VectorN const& q, Vector6 const& v, Vecto
q_dxdy << dx(i - 1, 0), dy(i - 1, 0), 0.0;
// Get future desired position of footsteps
- computeNextFootstep(i, j);
+ computeNextFootstep(i, j, b_v, b_vref);
// Get desired position of footstep compared to current position
double c = std::cos(yaws(i - 1));
double s = std::sin(yaws(i - 1));
Rz.topLeftCorner<2, 2>() << c, -s, s, c;
- footsteps_[i].col(j) = (Rz * nextFootstep_.col(j) + q_tmp + q_dxdy).transpose();
+ footsteps_[i].col(j) = (Rz * nextFootstep_.col(j) + q_dxdy).transpose();
}
}
i++;
}
}
-void FootstepPlanner::computeNextFootstep(int i, int j)
+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
// Add symmetry term
- nextFootstep_.col(j) = t_stance * 0.5 * b_v;
+ nextFootstep_.col(j) = t_stance * 0.5 * b_v.head(3);
// Add feedback term
- nextFootstep_.col(j) += k_feedback * (b_v - b_vref.head(3));
+ nextFootstep_.col(j) += k_feedback * (b_v.head(3) - b_vref.head(3));
// Add centrifugal term
Vector3 cross;
@@ -180,27 +198,26 @@ void FootstepPlanner::updateTargetFootsteps()
}
}
-MatrixN FootstepPlanner::computeTargetFootstep(VectorN const& q,
- Vector6 const& v,
- Vector6 const& b_vref)
+MatrixN FootstepPlanner::computeTargetFootstep(int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref)
{
- // Get the reference velocity in world frame (given in base frame)
+ // Compute the desired location of footsteps over the prediction horizon
+ computeFootsteps(k, b_v, b_vref);
+
+ // Update desired location of footsteps on the ground
+ updateTargetFootsteps();
+
+ // Get o_targetFootstep_ in world frame from targetFootstep_ in horizontal frame
quat_ = {q(6), q(3), q(4), q(5)}; // w, x, y, z
RPY_ << pinocchio::rpy::matrixToRpy(quat_.toRotationMatrix());
-
double c = std::cos(RPY_(2));
double s = std::sin(RPY_(2));
Rz.topLeftCorner<2, 2>() << c, -s, s, c;
+ for (int i = 0; i < 4; i++)
+ {
+ o_targetFootstep_.block(0, i, 2, 1) = Rz.topLeftCorner<2, 2>() * targetFootstep_.block(0, i, 2, 1) + q.head(2);
+ }
- Vector6 vref = b_vref;
- vref.head(3) = Rz * b_vref.head(3);
-
- // Compute the desired location of footsteps over the prediction horizon
- computeFootsteps(q, v, vref);
-
- // Update desired location of footsteps on the ground
- updateTargetFootsteps();
- return targetFootstep_;
+ return o_targetFootstep_;
}
void FootstepPlanner::updateNewContact()
@@ -216,6 +233,7 @@ void FootstepPlanner::updateNewContact()
MatrixN FootstepPlanner::getFootsteps() { return vectorToMatrix(footsteps_); }
MatrixN FootstepPlanner::getTargetFootsteps() { return targetFootstep_; }
+MatrixN FootstepPlanner::getRz() { return Rz; }
MatrixN FootstepPlanner::vectorToMatrix(std::vector<Matrix34> const& array)
{
@@ -228,4 +246,4 @@ MatrixN FootstepPlanner::vectorToMatrix(std::vector<Matrix34> const& array)
}
}
return M;
-}
\ No newline at end of file
+}
diff --git a/src/StatePlanner.cpp b/src/StatePlanner.cpp
index 15c4e269e4c496d43a645a4d7a6c2731a4fb5f29..c762b0fc9818cac232b1e0d46cec0766116826a8 100644
--- a/src/StatePlanner.cpp
+++ b/src/StatePlanner.cpp
@@ -24,8 +24,11 @@ void StatePlanner::computeReferenceStates(VectorN const& q, Vector6 const& v, Ve
RPY_ << pinocchio::rpy::matrixToRpy(quat.toRotationMatrix());
// Update the current state
- referenceStates_.block(0, 0, 3, 1) = q.head(3);
- referenceStates_.block(3, 0, 3, 1) = RPY_;
+ referenceStates_(0, 0) = 0.0; // In horizontal frame x = 0.0
+ referenceStates_(1, 0) = 0.0; // In horizontal frame y = 0.0
+ referenceStates_(2, 0) = q(2, 0); // We keep height
+ referenceStates_.block(3, 0, 2, 1) = RPY_.head(2); // We keep roll and pitch
+ referenceStates_(5, 0) = 0.0; // In horizontal frame yaw = 0.0
referenceStates_.block(6, 0, 3, 1) = v.head(3);
referenceStates_.block(9, 0, 3, 1) = v.tail(3);
@@ -51,7 +54,7 @@ void StatePlanner::computeReferenceStates(VectorN const& q, Vector6 const& v, Ve
referenceStates_(6, 1 + i) = vref(0) * std::cos(referenceStates_(5, 1 + i)) - vref(1) * std::sin(referenceStates_(5, 1 + i));
referenceStates_(7, 1 + i) = vref(0) * std::sin(referenceStates_(5, 1 + i)) + vref(1) * std::cos(referenceStates_(5, 1 + i));
- referenceStates_(5, 1 + i) += RPY_(2);
+ // referenceStates_(5, 1 + i) += RPY_(2);
referenceStates_(11, 1 + i) = vref(5);
}