diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 21bb3db5be6fff60acfeff05b3b2eeab8d1064b5..536ef25a0e72fab96d1b0a0809f9fa6b30e9759b 100644
--- a/include/qrw/FootTrajectoryGenerator.hpp
+++ b/include/qrw/FootTrajectoryGenerator.hpp
@@ -63,6 +63,10 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
void update(int k, MatrixN const& targetFootstep);
+ Eigen::MatrixXd getFootPositionBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &T);
+ Eigen::MatrixXd getFootVelocityBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &v_ref, const Eigen::Matrix<double, 3, 1> &w_ref);
+ Eigen::MatrixXd getFootAccelerationBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &w_ref);
+
MatrixN getTargetPosition() { return targetFootstep_; } ///< Get the foot goal position
MatrixN getFootPosition() { return position_; } ///< Get the next foot position
MatrixN getFootVelocity() { return velocity_; } ///< Get the next foot velocity
@@ -70,10 +74,11 @@ public:
private:
Gait* gait_; ///< Target lock before the touchdown
- double dt_tsid; ///<
+ double dt_wbc; ///<
int k_mpc; ///<
double maxHeight_; ///< Apex height of the swinging trajectory
double lockTime_; ///< Target lock before the touchdown
+ double vertTime_; ///< Duration during which feet move only along Z when taking off and landing
std::vector<int> feet;
Vector4 t0s;
@@ -87,5 +92,9 @@ private:
Matrix34 position_; // position computed in updateFootPosition
Matrix34 velocity_; // velocity computed in updateFootPosition
Matrix34 acceleration_; // acceleration computed in updateFootPosition
+
+ Matrix34 position_base_; // position computed in updateFootPosition in base frame
+ Matrix34 velocity_base_; // velocity computed in updateFootPosition in base frame
+ Matrix34 acceleration_base_; // acceleration computed in updateFootPosition in base frame
};
#endif // TRAJGEN_H_INCLUDED
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index b7ec78a09e9ee7eb0641c5c1c57bfffa7a83c338..f35667cd9bec05a4706be3db595d98540f926935 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -65,6 +65,7 @@ public:
double max_height;
double lock_time;
+ double vert_time;
std::vector<double> osqp_w_states;
std::vector<double> osqp_w_forces;
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index ae0478e9a8a7a1bd9a542fa8055c2acc67a861e0..018c537858a1bf0c1a82269d18ef43f89d0677ba 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -151,6 +151,13 @@ struct FootTrajectoryGeneratorPythonVisitor : public bp::def_visitor<FootTraject
{
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+ .def("getFootPositionBaseFrame", &FootTrajectoryGenerator::getFootPositionBaseFrame,
+ bp::args("R", "T"), "Get position_ matrix in base frame.\n")
+ .def("getFootVelocityBaseFrame", &FootTrajectoryGenerator::getFootVelocityBaseFrame,
+ bp::args("R", "v_ref", "w_ref"), "Get velocity_ matrix in base frame.\n")
+ .def("getFootAccelerationBaseFrame", &FootTrajectoryGenerator::getFootAccelerationBaseFrame,
+ bp::args("R", "w_ref"), "Get acceleration_ matrix in base frame.\n")
+
.def("getFootPosition", &FootTrajectoryGenerator::getFootPosition, "Get position_ matrix.\n")
.def("getFootVelocity", &FootTrajectoryGenerator::getFootVelocity, "Get velocity_ matrix.\n")
.def("getFootAcceleration", &FootTrajectoryGenerator::getFootAcceleration, "Get acceleration_ matrix.\n")
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 15d2c0a3dc72f1ded19de04fa94fec12fbbe7ae1..0bc45948497b679cf0a5d08efb41e6256d654bee 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -293,18 +293,10 @@ class Controller:
self.b_v[:6, 0] = self.v_ref[:6, 0] # Base at reference velocity (TODO: add hRb once v_ref is considered in base frame)
self.b_v[6:, 0] = self.myController.vdes[6:, 0] # with reference angular velocities of previous loop
- # Feet command acceleration in base frame
- self.feet_a_cmd = oRh.transpose() @ self.footTrajectoryGenerator.getFootAcceleration() \
- - self.cross12(self.v_ref[3:6, 0:1], self.cross12(self.v_ref[3:6, 0:1], self.feet_p_cmd)) \
- - 2 * self.cross12(self.v_ref[3:6, 0:1], self.feet_v_cmd)
-
- # Feet command velocity in base frame
- self.feet_v_cmd = oRh.transpose() @ self.footTrajectoryGenerator.getFootVelocity()
- self.feet_v_cmd = self.feet_v_cmd - self.v_ref[0:3, 0:1] - self.cross12(self.v_ref[3:6, 0:1], self.feet_p_cmd)
-
- # Feet command position in base frame
- self.feet_p_cmd = oRh.transpose() @ (self.footTrajectoryGenerator.getFootPosition()
- - np.array([[0.0], [0.0], [self.h_ref]]) - oTh)
+ # Feet command position, velocity and acceleration in base frame
+ self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(oRh.transpose(), self.v_ref[3:6, 0:1])
+ self.feet_v_cmd = self.footTrajectoryGenerator.getFootVelocityBaseFrame(oRh.transpose(), self.v_ref[0:3, 0:1], self.v_ref[3:6, 0:1])
+ self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(oRh.transpose(), np.array([[0.0], [0.0], [self.h_ref]]) + oTh)
# Run InvKin + WBC QP
self.myController.compute(self.q_wbc, self.b_v,
@@ -413,15 +405,16 @@ class Controller:
self.q[2, 0] = self.estimator.q_filt[2, 0]
# Mix perfect yaw with pitch and roll measurements
- self.yaw_estim += self.v_ref[5, 0:1] * self.myController.dt
- self.q[3:7, 0] = self.estimator.EulerToQuaternion([self.estimator.RPY[0], self.estimator.RPY[1], self.yaw_estim])
+ self.yaw_estim += self.v_ref[5, 0] * self.myController.dt
+ self.q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.estimator.RPY[0], self.estimator.RPY[1], self.yaw_estim)).coeffs()
# Actuators measurements
self.q[7:, 0] = self.estimator.q_filt[7:, 0]
# Velocities are the one estimated by the estimator
self.v = self.estimator.v_filt.copy()
- hRb = utils_mpc.EulerToRotation(self.estimator.RPY[0], self.estimator.RPY[1], 0.0)
+ hRb = pin.rpy.rpyToMatrix(self.estimator.RPY[0], self.estimator.RPY[1], 0.0)
+
self.h_v[0:3, 0:1] = hRb @ self.v[0:3, 0:1]
self.h_v[3:6, 0:1] = hRb @ self.v[3:6, 0:1]
@@ -440,17 +433,3 @@ class Controller:
oTh = np.array([[self.q[0, 0]], [self.q[1, 0]], [0.0]])
return oRh, oTh
-
- def cross12(self, left, right):
- """Numpy is inefficient for this
-
- Args:
- left (3x1 array): left term of the cross product
- right (3x4 array): right term of the cross product
- """
- res = np.zeros((3, 4))
- for i in range(4):
- res[:, i] = np.array([left[1, 0] * right[2, i] - left[2, 0] * right[1, i],
- left[2, 0] * right[0, i] - left[0, 0] * right[2, i],
- left[0, 0] * right[1, i] - left[1, 0] * right[0, i]])
- return res
diff --git a/scripts/Estimator.py b/scripts/Estimator.py
index 0f3831021c076e8d0482e9c3d5c2a97c97360441..c3241c4bd58e02ced9c30776758f9d95de20a0e3 100644
--- a/scripts/Estimator.py
+++ b/scripts/Estimator.py
@@ -358,15 +358,15 @@ class Estimator:
self.IMU_ang_vel[:] = device.baseAngularVelocity
# Angular position of the trunk (local frame)
- self.RPY = self.quaternionToRPY(device.baseOrientation)
+ self.RPY = pin.rpy.matrixToRpy(pin.Quaternion(device.baseOrientation).toRotationMatrix())
if (self.k_log <= 1):
- self.offset_yaw_IMU = self.RPY[2, 0]
+ self.offset_yaw_IMU = self.RPY[2]
self.RPY[2] -= self.offset_yaw_IMU # Remove initial offset of IMU
- self.IMU_ang_pos[:] = self.EulerToQuaternion([self.RPY[0],
- self.RPY[1],
- self.RPY[2]])
+ self.IMU_ang_pos[:] = pin.Quaternion(pin.rpy.rpyToMatrix(self.RPY[0],
+ self.RPY[1],
+ self.RPY[2])).coeffs()
# Above could be commented since IMU_ang_pos yaw is not used anywhere and instead
# replace by: self.IMU_ang_pos[:] = device.baseOrientation
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index 6a01193a211e821669110184236cd17e47da6d57..33fb3528e0ad01e7671e1d787bb88260b6a6f02a 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -4,7 +4,7 @@
FootTrajectoryGenerator::FootTrajectoryGenerator()
: gait_(NULL)
- , dt_tsid(0.0)
+ , dt_wbc(0.0)
, k_mpc(0)
, maxHeight_(0.0)
, lockTime_(0.0)
@@ -17,17 +17,22 @@ FootTrajectoryGenerator::FootTrajectoryGenerator()
, position_(Matrix34::Zero())
, velocity_(Matrix34::Zero())
, acceleration_(Matrix34::Zero())
+ , position_base_(Matrix34::Zero())
+ , velocity_base_(Matrix34::Zero())
+ , acceleration_base_(Matrix34::Zero())
{
}
void FootTrajectoryGenerator::initialize(Params& params, Gait& gaitIn)
{
- dt_tsid = params.dt_wbc;
+ dt_wbc = params.dt_wbc;
k_mpc = (int)std::round(params.dt_mpc / params.dt_wbc);
maxHeight_ = params.max_height;
lockTime_ = params.lock_time;
+ vertTime_ = params.vert_time;
targetFootstep_ << Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_init.data(), params.footsteps_init.size());
position_ = targetFootstep_;
+ position_base_ = targetFootstep_;
gait_ = &gaitIn;
}
@@ -41,9 +46,9 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const& tar
double x0 = position_(0, j);
double y0 = position_(1, j);
- double t = t0s[j];
- double d = t_swing[j];
- double dt = dt_tsid;
+ double t = t0s[j] - vertTime_;
+ double d = t_swing[j] - 2 * vertTime_;
+ double dt = dt_wbc;
if (t < d - lockTime_)
{
@@ -67,14 +72,16 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const& tar
}
// Coefficients for z (deterministic)
+ double Tz = t_swing[j];
Vector4 Az;
- Az(0, j) = -maxHeight_ / (std::pow((d / 2), 3) * std::pow((d - d / 2), 3));
- Az(1, j) = (3 * d * maxHeight_) / (std::pow((d / 2), 3) * std::pow((d - d / 2), 3));
- Az(2, j) = -(3 * std::pow(d, 2) * maxHeight_) / (std::pow((d / 2), 3) * std::pow((d - d / 2), 3));
- Az(3, j) = (std::pow(d, 3) * maxHeight_) / (std::pow((d / 2), 3) * std::pow((d - d / 2), 3));
+ Az(0, j) = -maxHeight_ / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
+ Az(1, j) = (3 * Tz * maxHeight_) / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
+ Az(2, j) = -(3 * std::pow(Tz, 2) * maxHeight_) / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
+ Az(3, j) = (std::pow(Tz, 3) * maxHeight_) / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
// Get the next point
double ev = t + dt;
+ double evz = t0s[j] + dt;
if (t < 0.0 || t > d) // Just vertical motion
{
@@ -94,9 +101,9 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const& tar
acceleration_(0, j) = 2 * Ax(3, j) + 3 * 2 * Ax(2, j) * ev + 4 * 3 * Ax(1, j) * std::pow(ev, 2) + 5 * 4 * Ax(0, j) * std::pow(ev, 3);
acceleration_(1, j) = 2 * Ay(3, j) + 3 * 2 * Ay(2, j) * ev + 4 * 3 * Ay(1, j) * std::pow(ev, 2) + 5 * 4 * Ay(0, j) * std::pow(ev, 3);
}
- velocity_(2, j) = 3 * Az(3, j) * std::pow(ev, 2) + 4 * Az(2, j) * std::pow(ev, 3) + 5 * Az(1, j) * std::pow(ev, 4) + 6 * Az(0, j) * std::pow(ev, 5);
- acceleration_(2, j) = 2 * 3 * Az(3, j) * ev + 3 * 4 * Az(2, j) * std::pow(ev, 2) + 4 * 5 * Az(1, j) * std::pow(ev, 3) + 5 * 6 * Az(0, j) * std::pow(ev, 4);
- position_(2, j) = Az(3, j) * std::pow(ev, 3) + Az(2, j) * std::pow(ev, 4) + Az(1, j) * std::pow(ev, 5) + Az(0, j) * std::pow(ev, 6);
+ velocity_(2, j) = 3 * Az(3, j) * std::pow(evz, 2) + 4 * Az(2, j) * std::pow(evz, 3) + 5 * Az(1, j) * std::pow(evz, 4) + 6 * Az(0, j) * std::pow(evz, 5);
+ acceleration_(2, j) = 2 * 3 * Az(3, j) * evz + 3 * 4 * Az(2, j) * std::pow(evz, 2) + 4 * 5 * Az(1, j) * std::pow(evz, 3) + 5 * 6 * Az(0, j) * std::pow(evz, 4);
+ position_(2, j) = Az(3, j) * std::pow(evz, 3) + Az(2, j) * std::pow(evz, 4) + Az(1, j) * std::pow(evz, 5) + Az(0, j) * std::pow(evz, 6);
}
void FootTrajectoryGenerator::update(int k, MatrixN const& targetFootstep)
@@ -119,7 +126,7 @@ void FootTrajectoryGenerator::update(int k, MatrixN const& targetFootstep)
{
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_tsid - dt_tsid;
+ double value = t_swing[i] - (gait_->getRemainingTime() * k_mpc - ((k + 1) % k_mpc)) * dt_wbc - dt_wbc;
t0s[i] = std::max(0.0, value);
}
}
@@ -132,7 +139,7 @@ void FootTrajectoryGenerator::update(int k, MatrixN const& targetFootstep)
// 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_tsid;
+ double value = t0s[feet[i]] + dt_wbc;
t0s[feet[i]] = std::max(0.0, value);
}
}
@@ -143,3 +150,21 @@ void FootTrajectoryGenerator::update(int k, MatrixN const& targetFootstep)
}
return;
}
+
+Eigen::MatrixXd FootTrajectoryGenerator::getFootPositionBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &T) {
+
+ position_base_ = R * (position_ - T.replicate<1, 4>()); // Value saved because it is used to get velocity and acceleration
+ return position_base_;
+}
+
+Eigen::MatrixXd FootTrajectoryGenerator::getFootVelocityBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &v_ref,
+ const Eigen::Matrix<double, 3, 1> &w_ref) {
+
+ velocity_base_ = R * velocity_ - v_ref.replicate<1, 4>() + position_base_.colwise().cross(w_ref); // Value saved because it is used to get acceleration
+ return velocity_base_;
+}
+
+Eigen::MatrixXd FootTrajectoryGenerator::getFootAccelerationBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &w_ref) {
+
+ return R * acceleration_ - (position_base_.colwise().cross(w_ref)).colwise().cross(w_ref) + 2 * velocity_base_.colwise().cross(w_ref);
+}
diff --git a/src/Params.cpp b/src/Params.cpp
index 5b50a8eaca3376eee0991a06e85d319721c6cc15..e142d59c64f7b40e7ba9b96f50b90a6363ec95fa 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -29,6 +29,7 @@ Params::Params()
, max_height(0.0)
, lock_time(0.0)
+ , vert_time(0.0)
, osqp_w_states(12, 0.0) // Fill with zeros, will be filled with values later
, osqp_w_forces(3, 0.0) // Fill with zeros, will be filled with values later
diff --git a/src/config_solo12.yaml b/src/config_solo12.yaml
index 054251a1ab231e90f76f79b4526ce4873a9827f2..3b540d97992a613d9631eab5e244257de84b2aed 100644
--- a/src/config_solo12.yaml
+++ b/src/config_solo12.yaml
@@ -29,7 +29,8 @@ robot:
# Parameters of FootTrajectoryGenerator
max_height: 0.05 # Apex height of the swinging trajectory [m]
- lock_time: 0.07 # Target lock before the touchdown [s]
+ lock_time: 0.04 # Target lock before the touchdown [s]
+ vert_time: 0.02 # Duration during which feet move only along Z when taking off and landing
# Parameters of MPC with OSQP
osqp_w_states: [2.0, 2.0, 20.0, 0.25, 0.25, 10.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3] # Weights for state tracking error
@@ -43,5 +44,5 @@ robot:
# Parameters of WBC QP problem
Q1: 0.1 # Weights for the "delta articular accelerations" optimization variables
Q2: 5.0 # Weights for the "delta contact forces" optimization variables
- Fz_max: 25.0 # Maximum vertical contact force [N]
+ Fz_max: 35.0 # Maximum vertical contact force [N]
Fz_min: 0.0 # Minimal vertical contact force [N]
\ No newline at end of file