diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 873e95559b028a4038f445151e2104e6a7d853be..718e2e8762b3f7d4bf07ab8548af4666cf8526bb 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -279,9 +279,10 @@ class Estimator {
Vector3 oTh_; // Translation between horizontal and world frame
double yaw_estim_; // Yaw angle in perfect world
- int N_queue_;
- VectorN v_filt_bis_;
- VectorN h_v_windowed_;
- std::deque<double> vx_queue_, vy_queue_, vz_queue_, wR_queue_, wP_queue_, wY_queue_;
+ // Filter the base velocity with an averaging window to remove oscillations at multiples of gait frequency
+ int N_queue_; // Number of samples in the averaging window
+ VectorN v_filt_bis_; // Base velocity (in base frame) filtered by averaging window
+ VectorN h_v_windowed_; // Base velocity (in horizontal frame) filtered by averaging window
+ std::deque<double> vx_queue_, vy_queue_, vz_queue_, wR_queue_, wP_queue_, wY_queue_; // Queues that hold samples
};
#endif // ESTIMATOR_H_INCLUDED
diff --git a/include/qrw/FakeRobot.hpp b/include/qrw/FakeRobot.hpp
index e51f6aed47181adef8e71a99811b932d74b3224b..96b9864d155ccf65a179887bc609d40c8e0e5869 100644
--- a/include/qrw/FakeRobot.hpp
+++ b/include/qrw/FakeRobot.hpp
@@ -32,14 +32,17 @@ class FakeJoints {
// Fake functions
void PrintVector(Vector12 const& data) {}
void SetZeroCommands() {}
- Vector12 GetPositions() { Vector12 des_pos; des_pos << 0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407; return des_pos; }
+ Vector12 GetPositions() {
+ Vector12 des_pos;
+ des_pos << 0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407;
+ return des_pos;
+ }
Vector12 GetVelocities() { return Vector12::Zero(); }
void SetPositionGains(Vector12 const& data) {}
void SetVelocityGains(Vector12 const& data) {}
void SetDesiredPositions(Vector12 const& data) {}
void SetDesiredVelocities(Vector12 const& data) {}
void SetTorques(Vector12 const& data) {}
-
};
class FakeImu {
@@ -62,7 +65,6 @@ class FakeImu {
Vector12 GetLinearAcceleration() { return 0.0 * Vector12::Random(); }
Vector12 GetGyroscope() { return 0.0 * Vector12::Random(); }
Vector12 GetAttitudeEuler() { return 0.0 * Vector12::Random(); }
-
};
class FakePowerboard {
@@ -85,7 +87,6 @@ class FakePowerboard {
double GetCurrent() { return 0.0; }
double GetVoltage() { return 0.0; }
double GetEnergy() { return 0.0; }
-
};
class FakeRobot {
@@ -95,7 +96,11 @@ class FakeRobot {
/// \brief Constructor
///
////////////////////////////////////////////////////////////////////////////////////////////////
- FakeRobot() {joints = new FakeJoints(); imu = new FakeImu(); powerboard = new FakePowerboard();}
+ FakeRobot() {
+ joints = new FakeJoints();
+ imu = new FakeImu();
+ powerboard = new FakePowerboard();
+ }
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -113,7 +118,6 @@ class FakeRobot {
FakeJoints* joints = nullptr;
FakeImu* imu = nullptr;
FakePowerboard* powerboard = nullptr;
-
};
#endif // FAKEROBOT_H_INCLUDED
diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 333b762b0778e55a78ff0db6b32ce444b36aa104..4121085b911df6dd2377d08e4fe276af9ae9c68d 100644
--- a/include/qrw/FootTrajectoryGenerator.hpp
+++ b/include/qrw/FootTrajectoryGenerator.hpp
@@ -56,14 +56,43 @@ class FootTrajectoryGenerator {
/// to the desired position on the ground (computed by the footstep planner)
///
/// \param[in] k Number of time steps since the start of the simulation
+ /// \param[in] targetFootstep Desired target locations at the end of the swing phases
///
////////////////////////////////////////////////////////////////////////////////////////////////
void update(int k, MatrixN const &targetFootstep);
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Get target positions of feet in desired frame from target positions in ideal world
+ ///
+ /// \param[in] R Rotation matrix to apply between output frame and the ideal world
+ /// \param[in] T Translation to apply between output frame and the ideal world
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
Eigen::MatrixXd getFootPositionBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &T);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Get target velocities of feet in desired frame from target positions in ideal world
+ ///
+ /// \param[in] R Rotation matrix to apply between output frame and the ideal world
+ /// \param[in] v_ref Reference linear velocity (if required for change of frame)
+ /// \param[in] w_ref Reference angular velocity (if required for change of frame)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
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);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Get target accelerations of feet in desired frame from target positions in ideal world
+ ///
+ /// \param[in] R Rotation matrix to apply between output frame and the ideal world
+ /// \param[in] v_ref Reference linear velocity (if required for change of frame)
+ /// \param[in] w_ref Reference angular velocity (if required for change of frame)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
Eigen::MatrixXd getFootAccelerationBaseFrame(const Eigen::Matrix<double, 3, 3> &R,
const Eigen::Matrix<double, 3, 1> &w_ref,
const Eigen::Matrix<double, 3, 1> &a_ref);
@@ -85,8 +114,8 @@ class FootTrajectoryGenerator {
double vertTime_; // Duration during which feet move only along Z when taking off and landing
Eigen::Matrix<int, 1, 4> feet; // Column indexes of feet currently in swing phase
- Vector4 t0s; // Elapsed time since the start of the swing phase movement
- Vector4 t_swing; // Swing phase duration for each foot
+ Vector4 t0s; // Elapsed time since the start of the swing phase movement
+ Vector4 t_swing; // Swing phase duration for each foot
Matrix34 targetFootstep_; // Target for the X component
diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index 939c48672a387454d01730e7d35e91bbadb6d5aa..9a17ee535fc75cf9722bbf8613bc68f7d6e3a76e 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -136,6 +136,8 @@ class FootstepPlanner {
///
/// \brief Transform a std::vector of N 3x4 matrices into a single Nx12 matrix
///
+ /// \param[in] array The std::vector of N 3x4 matrices to transform
+ ///
////////////////////////////////////////////////////////////////////////////////////////////////
MatrixN vectorToMatrix(std::vector<Matrix34> const& array);
@@ -155,7 +157,7 @@ class FootstepPlanner {
// Constant sized matrices
Matrix34 footsteps_under_shoulders_; // Positions of footsteps to be "under the shoulder"
- Matrix34 footsteps_offset_;
+ Matrix34 footsteps_offset_; // Hardcoded offset to add to footsteps positions
Matrix34 currentFootstep_; // Feet matrix
Matrix34 nextFootstep_; // Temporary matrix to perform computations
Matrix34 targetFootstep_; // In horizontal frame
@@ -175,7 +177,7 @@ class FootstepPlanner {
pinocchio::Data data_; // Pinocchio datas for forward kinematics
int foot_ids_[4] = {0, 0, 0, 0}; // Indexes of feet frames
Matrix34 pos_feet_; // Estimated feet positions based on measurements
- Vector19 q_FK_;
+ Vector19 q_FK_; // Estimated state of the base (height, roll, pitch, joints)
};
#endif // FOOTSTEPPLANNER_H_INCLUDED
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 7fe151c409794ba600f10a3d71f9947b79ae4961..84300131e8e7549a967fc56fcea6f943746420d8 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -410,47 +410,46 @@ void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait) {
// TODO: Joystick velocity given in base frame and not in horizontal frame (case of non flat ground)
// Update reference acceleration vector
- a_ref_.head(3) = (joystick_v_ref.head(3) - pinocchio::rpy::rpyToMatrix(0.0, 0.0, - v_ref_[5] * dt_wbc) * v_ref_.head(3)) / dt_wbc;
- a_ref_.tail(3) = (joystick_v_ref.tail(3) - pinocchio::rpy::rpyToMatrix(0.0, 0.0, - v_ref_[5] * dt_wbc) * v_ref_.tail(3)) / dt_wbc;
+ a_ref_.head(3) =
+ (joystick_v_ref.head(3) - pinocchio::rpy::rpyToMatrix(0.0, 0.0, -v_ref_[5] * dt_wbc) * v_ref_.head(3)) / dt_wbc;
+ a_ref_.tail(3) =
+ (joystick_v_ref.tail(3) - pinocchio::rpy::rpyToMatrix(0.0, 0.0, -v_ref_[5] * dt_wbc) * v_ref_.tail(3)) / dt_wbc;
// Update reference velocity vector
v_ref_.head(3) = joystick_v_ref.head(3);
v_ref_.tail(3) = joystick_v_ref.tail(3);
// Update position and velocity state vectors
- if (!gait.getIsStatic()) {
- // Integration to get evolution of perfect x, y and yaw
- Matrix2 Ryaw;
- Ryaw << cos(yaw_estim_), -sin(yaw_estim_), sin(yaw_estim_), cos(yaw_estim_);
- v_up_.head(2) = Ryaw * v_ref_.head(2);
- q_up_.head(2) = q_up_.head(2) + v_up_.head(2) * dt_wbc;
+ // Integration to get evolution of perfect x, y and yaw
+ Matrix2 Ryaw;
+ Ryaw << cos(yaw_estim_), -sin(yaw_estim_), sin(yaw_estim_), cos(yaw_estim_);
+ v_up_.head(2) = Ryaw * v_ref_.head(2);
+ q_up_.head(2) = q_up_.head(2) + v_up_.head(2) * dt_wbc;
- // Mix perfect x and y with height measurement
- q_up_[2] = q_filt_dyn_[2];
+ // Mix perfect x and y with height measurement
+ q_up_[2] = q_filt_dyn_[2];
- // Mix perfect yaw with pitch and roll measurements
- v_up_[5] = v_ref_[5];
- yaw_estim_ += v_ref_[5] * dt_wbc;
- q_up_.block(3, 0, 3, 1) << IMU_RPY_[0], IMU_RPY_[1], yaw_estim_;
+ // Mix perfect yaw with pitch and roll measurements
+ v_up_[5] = v_ref_[5];
+ yaw_estim_ += v_ref_[5] * dt_wbc;
+ q_up_.block(3, 0, 3, 1) << IMU_RPY_[0], IMU_RPY_[1], yaw_estim_;
- // Transformation matrices between world and base frames
- oRb_ = pinocchio::rpy::rpyToMatrix(IMU_RPY_(0, 0), IMU_RPY_(1, 0), yaw_estim_);
+ // Transformation matrices between world and base frames
+ oRb_ = pinocchio::rpy::rpyToMatrix(IMU_RPY_(0, 0), IMU_RPY_(1, 0), yaw_estim_);
- // Actuators measurements
- q_up_.tail(12) = q_filt_dyn_.tail(12);
- v_up_.tail(12) = v_filt_dyn_.tail(12);
+ // Actuators measurements
+ q_up_.tail(12) = q_filt_dyn_.tail(12);
+ v_up_.tail(12) = v_filt_dyn_.tail(12);
- // Velocities are the one estimated by the estimator
- hRb_ = pinocchio::rpy::rpyToMatrix(IMU_RPY_[0], IMU_RPY_[1], 0.0);
+ // Velocities are the one estimated by the estimator
+ hRb_ = pinocchio::rpy::rpyToMatrix(IMU_RPY_[0], IMU_RPY_[1], 0.0);
- h_v_.head(3) = hRb_ * v_filt_.block(0, 0, 3, 1);
- h_v_.tail(3) = hRb_ * v_filt_.block(3, 0, 3, 1);
- h_v_windowed_.head(3) = hRb_ * v_filt_bis_.block(0, 0, 3, 1);
- h_v_windowed_.tail(3) = hRb_ * v_filt_bis_.block(3, 0, 3, 1);
- } else {
- // TODO: Adapt static mode to new version of the code
- }
+ // Express estimated velocity and filtered estimated velocity in horizontal frame
+ h_v_.head(3) = hRb_ * v_filt_.block(0, 0, 3, 1);
+ h_v_.tail(3) = hRb_ * v_filt_.block(3, 0, 3, 1);
+ h_v_windowed_.head(3) = hRb_ * v_filt_bis_.block(0, 0, 3, 1);
+ h_v_windowed_.tail(3) = hRb_ * v_filt_bis_.block(3, 0, 3, 1);
// Transformation matrices between world and horizontal frames
oRh_ = Matrix3::Identity();
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index 49028f8efc290ed4795a7be05804d8a18d12a8bd..ea66aab76437a1629ac64ebc37c7a7d3b99569a5 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -154,7 +154,7 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
acceleration_(0, j) = 0.0;
acceleration_(1, j) = 0.0;
jerk_(0, j) = 0.0;
- jerk_(1, j) = 0.0;
+ jerk_(1, j) = 0.0;
} else {
position_(0, j) = Ax(5, j) + Ax(4, j) * ev + Ax(3, j) * std::pow(ev, 2) + Ax(2, j) * std::pow(ev, 3) +
Ax(1, j) * std::pow(ev, 4) + Ax(0, j) * std::pow(ev, 5);
@@ -176,7 +176,8 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
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);
- jerk_(2, j) = 2 * 3 * Az(3, j) + 3 * 4 * 2 * Az(2, j) * evz + 4 * 5 * 3 * Az(1, j) * std::pow(evz, 2) + 5 * 6 * 4 * Az(0, j) * std::pow(evz, 3);
+ jerk_(2, j) = 2 * 3 * Az(3, j) + 3 * 4 * 2 * Az(2, j) * evz + 4 * 5 * 3 * Az(1, j) * std::pow(evz, 2) +
+ 5 * 6 * 4 * Az(0, j) * std::pow(evz, 3);
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);
}
@@ -184,15 +185,14 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
void FootTrajectoryGenerator::update(int k, MatrixN const &targetFootstep) {
if ((k % k_mpc) == 0) {
// Status of feet
- feet = gait_->getCurrentGait().row(0).cast <int> ();
+ feet = gait_->getCurrentGait().row(0).cast<int>();
// If no foot in swing phase
if (feet.sum() == 4) return;
// 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)
- {
+ 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;
t0s[i] = std::max(0.0, value);
@@ -204,8 +204,7 @@ void FootTrajectoryGenerator::update(int k, MatrixN const &targetFootstep) {
// Increment of one time step for feet in swing phase
for (int i = 0; i < 4; i++) {
- if (feet(0, i) == 0)
- {
+ if (feet(0, i) == 0) {
double value = t0s[i] + dt_wbc;
t0s[i] = std::max(0.0, value);
}
@@ -213,8 +212,7 @@ void FootTrajectoryGenerator::update(int k, MatrixN const &targetFootstep) {
}
for (int i = 0; i < 4; i++) {
- if (feet(0, i) == 0)
- {
+ if (feet(0, i) == 0) {
updateFootPosition(i, targetFootstep.col(i));
}
}
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index e2649bf5e94bf4ae8ff1a03f27e45a015eeec613..1ebffe56b2adb7a6e4f4a91576cc80bd6056077e 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -131,8 +131,7 @@ void FootstepPlanner::computeFootsteps(int k, Vector6 const& b_v, Vector6 const&
}
// Update the footstep matrix depending on the different phases of the gait (swing & stance)
- for (int i = 1; i < gait.rows(); i++)
- {
+ for (int i = 1; i < gait.rows(); i++) {
// Feet that were in stance phase and are still in stance phase do not move
for (int j = 0; j < 4; j++) {
if (gait(i - 1, j) * gait(i, j) > 0) {
@@ -165,8 +164,8 @@ void FootstepPlanner::computeNextFootstep(int i, int j, Vector6 const& b_v, Vect
double t_stance = gait_->getPhaseDuration(i, j, 1.0); // 1.0 for stance phase
- if (!gait_->getIsStatic())
- {
+ // Disable heuristic terms if gait is going to switch to static so that feet land at vertical of shoulders
+ if (!gait_->getIsStatic()) {
// Add symmetry term
nextFootstep_.col(j) = t_stance * 0.5 * b_v.head(3);