Skip to content
Snippets Groups Projects
Commit 7bb84904 authored by Pierre-Alexandre Leziart's avatar Pierre-Alexandre Leziart
Browse files

Adapt Gait class to use only 4 columns gait matrix

parent bca4c60d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/qrw/FootTrajectoryGenerator.hpp
+ 1
− 1
View file @ 7bb84904
......@@ -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);
////////////////////////////////////////////////////////////////////////////////////////////////
///
......
include/qrw/Gait.hpp
+ 11
− 18
View file @ 7bb84904
......@@ -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_;
......
include/qrw/config.h 0 → 100644
+ 9
− 0
View file @ 7bb84904
#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
src/Gait.cpp
+ 78
− 209
View file @ 7bb84904
#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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment