diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 7b52aadcf2c12c759154d47546d5fc81d1ec73ce..3f5338fbdc969d53dfb65a2d7832697c1cb74f5b 100644
--- a/include/qrw/FootTrajectoryGenerator.hpp
+++ b/include/qrw/FootTrajectoryGenerator.hpp
@@ -38,7 +38,7 @@ public:
MatrixN const& initialFootPosition,
double const& dt_tsid_in,
int const& k_mpc_in,
- Gait & gait); // std::shared_ptr<Gait> gait);
+ Gait & gait);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/include/qrw/Gait.hpp b/include/qrw/Gait.hpp
index d223fc3b8f82a400528894a040498bb951aa36b1..bbd82a6e66b58e750e4977751434769cdc536255 100644
--- a/include/qrw/Gait.hpp
+++ b/include/qrw/Gait.hpp
@@ -13,10 +13,7 @@
#define GAIT_H_INCLUDED
#include "qrw/Types.h"
-
-#define N0_gait 20
-// Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is always at
-// least one empty line at the end of the gait matrix
+#include "qrw/config.h"
// Order of feet/legs: FL, FR, HL, HR
@@ -99,19 +96,6 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
double getPhaseDuration(int i, int j, double value);
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Move one step further in the gait cycle
- ///
- /// \details Decrease by 1 the number of remaining step for the current phase of the gait
- /// Transfer current gait phase into past gait matrix
- /// Insert future desired gait phase at the end of the gait matrix
- ///
- /// \param[in] k number of WBC iterations since the start of the simulation
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- void roll(int k, Matrix34 const& footstep, Matrix34& currentFootstep);
-
// TODO
void updateGait(int const k, int const k_mpc, VectorN const& q, int const joystickCode);
@@ -125,7 +109,15 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
bool changeGait(int const code, VectorN const& q);
- // TODO
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Move one step further in the gait cycle
+ ///
+ /// \details Decrease by 1 the number of remaining step for the current phase of the gait
+ /// Transfer current gait phase into past gait matrix
+ /// Insert future desired gait phase at the end of the gait matrix
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
void rollGait();
////////////////////////////////////////////////////////////////////////////////////////////////
@@ -154,6 +146,7 @@ private:
double dt_; // Time step of the contact sequence (time step of the MPC)
double T_gait_; // Gait period
double T_mpc_; // MPC period (prediction horizon)
+ int n_steps_; // Number of time steps in the prediction horizon
double remainingTime_;
diff --git a/include/qrw/config.h b/include/qrw/config.h
new file mode 100644
index 0000000000000000000000000000000000000000..97577104c602c2ac7c0b98c265d73b2046769d55
--- /dev/null
+++ b/include/qrw/config.h
@@ -0,0 +1,9 @@
+#ifndef CONFIG_H_INCLUDED
+#define CONFIG_H_INCLUDED
+
+// Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is always at
+// least one empty line at the end of the gait matrix
+#define N0_gait 20
+
+
+#endif // CONFIG_H_INCLUDED
diff --git a/src/Gait.cpp b/src/Gait.cpp
index 51ca5ed7e8327a50d31f6413fa4670a7ca85cc41..1062952ddb335a2bc83d8d579d41ddbe19083352 100644
--- a/src/Gait.cpp
+++ b/src/Gait.cpp
@@ -1,9 +1,9 @@
#include "qrw/Gait.hpp"
Gait::Gait()
- : pastGait_(MatrixN::Zero(N0_gait, 5))
- , currentGait_(MatrixN::Zero(N0_gait, 5))
- , desiredGait_(MatrixN::Zero(N0_gait, 5))
+ : pastGait_(MatrixN::Zero(N0_gait, 4))
+ , currentGait_(MatrixN::Zero(N0_gait, 4))
+ , desiredGait_(MatrixN::Zero(N0_gait, 4))
, dt_(0.0)
, T_gait_(0.0)
, T_mpc_(0.0)
@@ -21,6 +21,10 @@ void Gait::initialize(double dt_in, double T_gait_in, double T_mpc_in)
dt_ = dt_in;
T_gait_ = T_gait_in;
T_mpc_ = T_mpc_in;
+ n_steps_ = (int)std::lround(T_mpc_in / dt_in);
+
+ if((n_steps_ > N0_gait) || ((int)std::lround(T_gait_in / dt_in) > N0_gait))
+ throw std::invalid_argument("Sizes of matrices are too small for considered durations. Increase N0_gait in config file.");
create_trot();
create_gait_f();
@@ -32,16 +36,20 @@ int Gait::create_walk()
// Number of timesteps in 1/4th period of gait
int N = (int)std::lround(0.25 * T_gait_ / dt_);
- desiredGait_ = Eigen::Matrix<double, N0_gait, 5>::Zero();
- desiredGait_.block(0, 0, 4, 1) << N, N, N, N;
+ desiredGait_ = Eigen::Matrix<double, N0_gait, 4>::Zero();
// Set stance and swing phases
// Coefficient (i, j) is equal to 0.0 if the j-th feet is in swing phase during the i-th phase
// Coefficient (i, j) is equal to 1.0 if the j-th feet is in stance phase during the i-th phase
- desiredGait_.block(0, 1, 1, 4) << 0.0, 1.0, 1.0, 1.0;
- desiredGait_.block(1, 1, 1, 4) << 1.0, 0.0, 1.0, 1.0;
- desiredGait_.block(2, 1, 1, 4) << 1.0, 1.0, 0.0, 1.0;
- desiredGait_.block(3, 1, 1, 4) << 1.0, 1.0, 1.0, 0.0;
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 0, 1, 1, 1;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1, 0, 1, 1;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1, 1, 0, 1;
+ desiredGait_.block(2*N, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1, 1, 1, 0;
+ desiredGait_.block(3*N, 0, N, 4) = sequence.colwise().replicate(N);
return 0;
}
@@ -51,16 +59,16 @@ int Gait::create_trot()
// Number of timesteps in a half period of gait
int N = (int)std::lround(0.5 * T_gait_ / dt_);
- desiredGait_ = Eigen::Matrix<double, N0_gait, 5>::Zero();
- desiredGait_.block(0, 0, 2, 1) << N, N;
+ desiredGait_ = Eigen::Matrix<double, N0_gait, 4>::Zero();
// Set stance and swing phases
// Coefficient (i, j) is equal to 0.0 if the j-th feet is in swing phase during the i-th phase
// Coefficient (i, j) is equal to 1.0 if the j-th feet is in stance phase during the i-th phase
- desiredGait_(0, 1) = 1.0;
- desiredGait_(0, 4) = 1.0;
- desiredGait_(1, 2) = 1.0;
- desiredGait_(1, 3) = 1.0;
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 1, 0, 0, 1;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 0, 1, 1, 0;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
return 0;
}
@@ -71,15 +79,15 @@ int Gait::create_pacing()
int N = (int)std::lround(0.5 * T_gait_ / dt_);
desiredGait_ = Eigen::Matrix<double, N0_gait, 5>::Zero();
- desiredGait_.block(0, 0, 2, 1) << N, N;
// Set stance and swing phases
// Coefficient (i, j) is equal to 0.0 if the j-th feet is in swing phase during the i-th phase
// Coefficient (i, j) is equal to 1.0 if the j-th feet is in stance phase during the i-th phase
- desiredGait_(0, 1) = 1.0;
- desiredGait_(0, 3) = 1.0;
- desiredGait_(1, 2) = 1.0;
- desiredGait_(1, 4) = 1.0;
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 1, 0, 1, 0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 0, 1, 0, 1;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
return 0;
}
@@ -90,15 +98,15 @@ int Gait::create_bounding()
int N = (int)std::lround(0.5 * T_gait_ / dt_);
desiredGait_ = Eigen::Matrix<double, N0_gait, 5>::Zero();
- desiredGait_.block(0, 0, 2, 1) << N, N;
// Set stance and swing phases
// Coefficient (i, j) is equal to 0.0 if the j-th feet is in swing phase during the i-th phase
// Coefficient (i, j) is equal to 1.0 if the j-th feet is in stance phase during the i-th phase
- desiredGait_(0, 1) = 1.0;
- desiredGait_(0, 2) = 1.0;
- desiredGait_(1, 3) = 1.0;
- desiredGait_(1, 4) = 1.0;
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 1, 1, 0, 0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 0, 0, 1, 1;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
return 0;
}
@@ -109,73 +117,46 @@ int Gait::create_static()
int N = (int)std::lround(T_gait_ / dt_);
desiredGait_ = Eigen::Matrix<double, N0_gait, 5>::Zero();
- desiredGait_(0, 0) = N;
// Set stance and swing phases
// Coefficient (i, j) is equal to 0.0 if the j-th feet is in swing phase during the i-th phase
// Coefficient (i, j) is equal to 1.0 if the j-th feet is in stance phase during the i-th phase
- desiredGait_(0, 1) = 1.0;
- desiredGait_(0, 2) = 1.0;
- desiredGait_(0, 3) = 1.0;
- desiredGait_(0, 4) = 1.0;
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 1, 1, 1, 1;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
return 0;
}
int Gait::create_gait_f()
{
- double sum = 0.0;
- double offset = 0.0;
int i = 0;
- int j = 0;
- // Fill future gait matrix
- while (sum < (T_mpc_ / dt_))
+ // Fill currrent gait matrix
+ for (int j = 0; j < n_steps_; j++)
{
currentGait_.row(j) = desiredGait_.row(i);
- sum += desiredGait_(i, 0);
- offset += desiredGait_(i, 0);
i++;
- j++;
- if (desiredGait_(i, 0) == 0)
+ if (desiredGait_.row(i).isZero())
{
i = 0;
- offset = 0.0;
} // Loop back if T_mpc_ longer than gait duration
}
- // Remove excess time steps
- currentGait_(j - 1, 0) -= sum - (T_mpc_ / dt_);
- offset -= sum - (T_mpc_ / dt_);
-
- // Age future desired gait to take into account what has been put in the future gait matrix
- j = 1;
- while (desiredGait_(j, 0) > 0.0)
+ // Get index of first empty line
+ int index = 1;
+ while (!desiredGait_.row(index).isZero())
{
- j++;
+ index++;
}
- for (double k = 0; k < offset; k++)
+ // Age desired gait to take into account what has been put in the current gait matrix
+ for (int k = 0; k < i; k++)
{
- if ((desiredGait_.block(0, 1, 1, 4)).isApprox(desiredGait_.block(j - 1, 1, 1, 4)))
+ for (int m = 0; m < index-1; m++) // TODO: Find optimized circular shift function
{
- desiredGait_(j - 1, 0) += 1.0;
- }
- else
- {
- desiredGait_.row(j) = desiredGait_.row(0);
- desiredGait_(j, 0) = 1.0;
- j++;
- }
- if (desiredGait_(0, 0) == 1.0)
- {
- desiredGait_.block(0, 0, N0_gait - 1, 5) = desiredGait_.block(1, 0, N0_gait - 1, 5);
- j--;
- }
- else
- {
- desiredGait_(0, 0) -= 1.0;
- }
+ desiredGait_.row(m).swap(desiredGait_.row(m+1));
+ }
}
return 0;
@@ -183,23 +164,23 @@ int Gait::create_gait_f()
double Gait::getPhaseDuration(int i, int j, double value)
{
- double t_phase = currentGait_(i, 0);
+ double t_phase = 1;
int a = i;
// Looking for the end of the swing/stance phase in currentGait_
- while ((currentGait_(i + 1, 0) > 0.0) && (currentGait_(i + 1, 1 + j) == value))
+ while ((!currentGait_.row(i + 1).isZero()) && (currentGait_(i + 1, j) == value))
{
i++;
- t_phase += currentGait_(i, 0);
+ t_phase++;
}
// If we reach the end of currentGait_ we continue looking for the end of the swing/stance phase in desiredGait_
- if (currentGait_(i + 1, 0) == 0.0)
+ if (currentGait_.row(i + 1).isZero())
{
int k = 0;
- while ((desiredGait_(k, 0) > 0.0) && (desiredGait_(k, 1 + j) == value))
+ while ((!desiredGait_.row(k).isZero()) && (desiredGait_(k, j) == value))
{
- t_phase += desiredGait_(k, 0);
k++;
+ t_phase++;
}
}
// We suppose that we found the end of the swing/stance phase either in currentGait_ or desiredGait_
@@ -207,18 +188,18 @@ double Gait::getPhaseDuration(int i, int j, double value)
remainingTime_ = t_phase;
// Looking for the beginning of the swing/stance phase in currentGait_
- while ((a > 0) && (currentGait_(a - 1, 1 + j) == value))
+ while ((a > 0) && (currentGait_(a - 1, j) == value))
{
a--;
- t_phase += currentGait_(a, 0);
+ t_phase++;
}
// If we reach the end of currentGait_ we continue looking for the beginning of the swing/stance phase in pastGait_
if (a == 0)
{
- while ((pastGait_(a, 0) > 0.0) && (pastGait_(a, 1 + j) == value))
+ while ((!pastGait_.row(a).isZero()) && (pastGait_(a, j) == value))
{
- t_phase += pastGait_(a, 0);
a++;
+ t_phase++;
}
}
// We suppose that we found the beginning of the swing/stance phase either in currentGait_ or pastGait_
@@ -226,87 +207,6 @@ double Gait::getPhaseDuration(int i, int j, double value)
return t_phase * dt_; // Take into account time step value
}
-void Gait::roll(int k, Matrix34 const& footstep, Matrix34& currentFootstep)
-{
- // Transfer current gait into past gait
- // If current gait is the same than the first line of past gait we just increment the counter
- if ((currentGait_.block(0, 1, 1, 4)).isApprox(pastGait_.block(0, 1, 1, 4)))
- {
- pastGait_(0, 0) += 1.0;
- }
- else
- { // If current gait is not the same than the first line of past gait we have to insert it
- Eigen::Matrix<double, 5, 5> tmp = pastGait_.block(0, 0, N0_gait - 1, 5);
- pastGait_.block(1, 0, N0_gait - 1, 5) = tmp;
- pastGait_.row(0) = currentGait_.row(0);
- pastGait_(0, 0) = 1.0;
- }
-
- // Age future gait
- if (currentGait_(0, 0) == 1.0)
- {
- currentGait_.block(0, 0, N0_gait - 1, 5) = currentGait_.block(1, 0, N0_gait - 1, 5);
- // Entering new contact phase, store positions of feet that are now in contact
- if (k != 0)
- {
- for (int i = 0; i < 4; i++)
- {
- if (currentGait_(0, 1 + i) == 1.0)
- {
- currentFootstep.col(i) = footstep.col(i);
- }
- }
- }
- }
- else
- {
- currentGait_(0, 0) -= 1.0;
- }
-
- // Get index of first empty line
- int i = 1;
- while (currentGait_(i, 0) > 0.0)
- {
- i++;
- }
- // Increment last gait line or insert a new line
- if ((currentGait_.block(i - 1, 1, 1, 4)).isApprox(desiredGait_.block(0, 1, 1, 4)))
- {
- currentGait_(i - 1, 0) += 1.0;
- }
- else
- {
- currentGait_.row(i) = desiredGait_.row(0);
- currentGait_(i, 0) = 1.0;
- }
-
- // Age future desired gait
- // Get index of first empty line
- int j = 1;
- while (desiredGait_(j, 0) > 0.0)
- {
- j++;
- }
- // Increment last gait line or insert a new line
- if ((desiredGait_.block(0, 1, 1, 4)).isApprox(desiredGait_.block(j - 1, 1, 1, 4)))
- {
- desiredGait_(j - 1, 0) += 1.0;
- }
- else
- {
- desiredGait_.row(j) = desiredGait_.row(0);
- desiredGait_(j, 0) = 1.0;
- }
- if (desiredGait_(0, 0) == 1.0)
- {
- desiredGait_.block(0, 0, N0_gait - 1, 5) = desiredGait_.block(1, 0, N0_gait - 1, 5);
- }
- else
- {
- desiredGait_(0, 0) -= 1.0;
- }
-}
-
void Gait::updateGait(int const k,
int const k_mpc,
VectorN const& q,
@@ -344,73 +244,42 @@ bool Gait::changeGait(int const code, VectorN const& q)
void Gait::rollGait()
{
// Transfer current gait into past gait
- // If current gait is the same than the first line of past gait we just increment the counter
- if ((currentGait_.block(0, 1, 1, 4)).isApprox(pastGait_.block(0, 1, 1, 4)))
+ for (int m = n_steps_; m > 0; m--) // TODO: Find optimized circular shift function
{
- pastGait_(0, 0) += 1.0;
- }
- else
- { // If current gait is not the same than the first line of past gait we have to insert it
- Eigen::Matrix<double, 5, 5> tmp = pastGait_.block(0, 0, N0_gait - 1, 5);
- pastGait_.block(1, 0, N0_gait - 1, 5) = tmp;
- pastGait_.row(0) = currentGait_.row(0);
- pastGait_(0, 0) = 1.0;
+ pastGait_.row(m).swap(pastGait_.row(m-1));
}
+ pastGait_.row(0) = currentGait_.row(0);
- // Age future gait
- if (currentGait_(0, 0) == 1.0)
+
+ // Entering new contact phase, store positions of feet that are now in contact
+ if (!currentGait_.row(0).isApprox(currentGait_.row(1)))
{
- currentGait_.block(0, 0, N0_gait - 1, 5) = currentGait_.block(1, 0, N0_gait - 1, 5);
newPhase_ = true;
}
else
{
- currentGait_(0, 0) -= 1.0;
newPhase_ = false;
}
- // Get index of first empty line
- int i = 1;
- while (currentGait_(i, 0) > 0.0)
+ // Age current gait
+ int index = 1;
+ while (!currentGait_.row(index).isZero())
{
- i++;
- }
- // Increment last gait line or insert a new line
- if ((currentGait_.block(i - 1, 1, 1, 4)).isApprox(desiredGait_.block(0, 1, 1, 4)))
- {
- currentGait_(i - 1, 0) += 1.0;
- }
- else
- {
- currentGait_.row(i) = desiredGait_.row(0);
- currentGait_(i, 0) = 1.0;
+ currentGait_.row(index-1).swap(currentGait_.row(index));
+ index++;
}
- // Age future desired gait
- // Get index of first empty line
- int j = 1;
- while (desiredGait_(j, 0) > 0.0)
- {
- j++;
- }
- // Increment last gait line or insert a new line
- if ((desiredGait_.block(0, 1, 1, 4)).isApprox(desiredGait_.block(j - 1, 1, 1, 4)))
- {
- desiredGait_(j - 1, 0) += 1.0;
- }
- else
- {
- desiredGait_.row(j) = desiredGait_.row(0);
- desiredGait_(j, 0) = 1.0;
- }
- if (desiredGait_(0, 0) == 1.0)
- {
- desiredGait_.block(0, 0, N0_gait - 1, 5) = desiredGait_.block(1, 0, N0_gait - 1, 5);
- }
- else
+ // Insert a new line from desired gait into current gait
+ currentGait_.row(index-1) = desiredGait_.row(0);
+
+ // Age desired gait
+ index = 1;
+ while (!desiredGait_.row(index).isZero())
{
- desiredGait_(0, 0) -= 1.0;
+ desiredGait_.row(index-1).swap(desiredGait_.row(index));
+ index++;
}
+
}
bool Gait::setGait(MatrixN const& gaitMatrix)