diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp
index ce83187d205f93a3a5fb516d075a91d13e327d23..78df15fe50279452a30c2739d19e4da8ca622146 100644
--- a/include/qrw/MPC.hpp
+++ b/include/qrw/MPC.hpp
@@ -29,7 +29,7 @@ class MPC {
Eigen::Matrix<double, 3, 4> footholds = Eigen::Matrix<double, 3, 4>::Zero();
Eigen::Matrix<double, 1, 12> footholds_tmp = Eigen::Matrix<double, 12, 1>::Zero();
Eigen::Matrix<double, 3, 4> lever_arms = Eigen::Matrix<double, 3, 4>::Zero();
- Eigen::Matrix<int, 20, 5> gait = Eigen::Matrix<int, 20, 5>::Zero();
+ Eigen::Matrix<int, 20, 4> gait = Eigen::Matrix<int, 20, 4>::Zero();
Eigen::Matrix<double, 12, 1> g = Eigen::Matrix<double, 12, 1>::Zero();
Eigen::Matrix<double, 12, 12> A = Eigen::Matrix<double, 12, 12>::Identity();
diff --git a/scripts/Controller.py b/scripts/Controller.py
index d13419c61bda14a3a031784d3a8bc4b0efaa89b4..848f7b63204b9541db6fb7d34f1196dd350c4020 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -193,7 +193,7 @@ class Controller:
self.joystick.update_v_ref(self.k, self.velID)
# Process state estimator
- self.estimator.run_filter(self.k, self.gait.getCurrentGait()[0, 1:],
+ self.estimator.run_filter(self.k, self.gait.getCurrentGait()[0, :],
device, self.footTrajectoryGenerator.getFootPosition(),
self.gait.getCurrentGait()[0, 0])
t_filter = time.time()
@@ -227,7 +227,7 @@ class Controller:
self.gait.updateGait(self.k, self.k_mpc, self.q[0:7, 0:1], self.joystick.joystick_code)
# Update footsteps if new contact phase
- if(self.k % self.k_mpc == 0 and self.k != 0 and self.gait.isNewPhase()):
+ if(self.k % self.k_mpc == 0 and self.k != 0 and self.gait.isNewPhase()):
self.footstepPlanner.updateNewContact()
"""// Get the reference velocity in world frame (given in base frame)
@@ -264,7 +264,7 @@ class Controller:
# Result can be retrieved with self.statePlanner.getXReference()
xref = self.statePlanner.getXReference()
fsteps = self.footstepPlanner.getFootsteps()
- gait = self.gait.getCurrentGait()
+ cgait = self.gait.getCurrentGait()
t_planner = time.time()
@@ -278,7 +278,7 @@ class Controller:
# Process MPC once every k_mpc iterations of TSID
if (self.k % self.k_mpc) == 0:
try:
- self.mpc_wrapper.solve(self.k, xref, fsteps, gait)
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait)
except ValueError:
print("MPC Problem")
@@ -320,7 +320,7 @@ class Controller:
# Run InvKin + WBC QP
self.myController.compute(self.q, self.b_v, self.x_f_wbc[:12],
- self.x_f_wbc[12:], gait[0, 1:],
+ self.x_f_wbc[12:], cgait[0, :],
self.footTrajectoryGenerator.getFootPosition(),
self.footTrajectoryGenerator.getFootVelocity(),
self.footTrajectoryGenerator.getFootAcceleration())
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index 41ba849739e0055fdce4539ebaec3991ea2537ff..959d866310386f46e08714829a0f8d72e58a6fbc 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -50,9 +50,9 @@ class MPC_Wrapper:
if multiprocessing: # Setup variables in the shared memory
self.newData = Value('b', False)
self.newResult = Value('b', False)
- self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 13*20))
+ self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*20))
self.dataOut = Array('d', [0] * 24 * (np.int(self.n_steps)))
- self.fsteps_future = np.zeros((20, 13))
+ self.fsteps_future = np.zeros((20, 12))
self.running = Value('b', True)
else:
# Create the new version of the MPC solver object
@@ -89,13 +89,13 @@ class MPC_Wrapper:
pt = 0
while (gait[pt, 0] != 0):
pt += 1
- if k > 2 and not np.array_equal(gait[0, 1:], gait[pt-1, 1:]):
+ if k > 2 and not np.array_equal(gait[0, :], gait[pt-1, :]):
mass = 2.5 # Todo: grab from URDF?
nb_ctc = np.sum(gait[pt-1, 1:])
F = 9.81 * mass / nb_ctc
self.last_available_result[12:, self.n_steps-1] = np.zeros(12)
for i in range(4):
- if (gait[pt-1, 1+i] == 1):
+ if (gait[pt-1, i] == 1):
self.last_available_result[12+3*i+2, self.n_steps-1] = F
return 0
@@ -191,7 +191,7 @@ class MPC_Wrapper:
# Reshaping 1-dimensional data
k = int(kf[0])
xref = np.reshape(xref_1dim, (12, self.n_steps+1))
- fsteps = np.reshape(fsteps_1dim, (20, 13))
+ fsteps = np.reshape(fsteps_1dim, (20, 12))
# Create the MPC object of the parallel process during the first iteration
if k == 0:
@@ -249,7 +249,7 @@ class MPC_Wrapper:
"""
# Sizes of the different variables that are stored in the C-type array
- sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 13*20]
+ sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*20]
csizes = np.cumsum(sizes)
# Return decompressed variables in a list
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index ddfafcb302a9135d61d26100573eafe257705404..4b194f315c41b411c494a50fe24d6029251b3dfb 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -113,7 +113,7 @@ void FootTrajectoryGenerator::update(int k, MatrixN const& targetFootstep)
feet.clear();
for (int i = 0; i < 4; i++)
{
- if (gait_->getCurrentGait()(0, 1 + i) == 0)
+ if (gait_->getCurrentGait()(0, i) == 0)
feet.push_back(i);
}
// If no foot in swing phase
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index 4d8ea4358723c31ccb23fe99f384d055a55439c5..68bee2c44ac79868063030b1bc5af09e53ebc7f4 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -47,7 +47,7 @@ void FootstepPlanner::compute_footsteps(VectorN const& q, Vector6 const& v, Vect
// Set current position of feet for feet in stance phase
for (int j = 0; j < 4; j++)
{
- if (gait(0, 1 + j) == 1.0)
+ if (gait(0, j) == 1.0)
{
footsteps_[0].col(j) = currentFootstep_.col(j);
}
@@ -55,11 +55,11 @@ void FootstepPlanner::compute_footsteps(VectorN const& q, Vector6 const& v, Vect
// 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) = gait(0, 0) * dt;
+ dt_cum(0) = dt;
yaws(0) = vref(5) * dt_cum(0) + RPY(2);
for (int j = 1; j < N0_gait; j++)
{
- dt_cum(j) = dt_cum(j - 1) + gait(j) * dt;
+ dt_cum(j) = gait.row(j).isZero() ? dt_cum(j - 1) : dt_cum(j - 1) + gait(j) * dt;
yaws(j) = vref(5) * dt_cum(j) + RPY(2);
}
@@ -88,12 +88,12 @@ void FootstepPlanner::compute_footsteps(VectorN const& q, Vector6 const& v, Vect
// Update the footstep matrix depending on the different phases of the gait (swing & stance)
int i = 1;
- while (gait(i, 0) != 0)
+ while (!gait.row(i).isZero())
{
// 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, 1 + j) * gait(i, 1 + j) > 0)
+ if (gait(i - 1, j) * gait(i, j) > 0)
{
footsteps_[i].col(j) = footsteps_[i - 1].col(j);
}
@@ -106,7 +106,7 @@ void FootstepPlanner::compute_footsteps(VectorN const& q, Vector6 const& v, Vect
// Feet that were in swing phase and are now in stance phase need to be updated
for (int j = 0; j < 4; j++)
{
- if ((1 - gait(i - 1, 1 + j)) * gait(i, 1 + j) > 0)
+ if ((1 - gait(i - 1, j)) * gait(i, j) > 0)
{
// Offset to the future position
q_dxdy << dx(i - 1, 0), dy(i - 1, 0), 0.0;
@@ -198,7 +198,7 @@ void FootstepPlanner::updateNewContact() // Gait const& gait) // MaxtrixN const&
// Entering new contact phase, store positions of feet that are now in contact
for (int i = 0; i < 4; i++)
{
- if (gait_->getCurrentGaitCoeff(0, 1 + i) == 1.0) //if (currentGait(0, 1 + i) == 1.0)
+ if (gait_->getCurrentGaitCoeff(0, i) == 1.0) //if (currentGait(0, 1 + i) == 1.0)
{
currentFootstep_.col(i) = (footsteps_[1]).col(i);
}
@@ -210,13 +210,12 @@ MatrixN FootstepPlanner::getTargetFootsteps() { return targetFootstep_; }
MatrixN FootstepPlanner::vectorToMatrix(std::array<Matrix34, N0_gait> const& array)
{
- MatrixN M = MatrixN::Zero(N0_gait, 13);
- M.col(0) = gait_->getCurrentGait().col(0);
+ MatrixN M = MatrixN::Zero(N0_gait, 12);
for (int i = 0; i < N0_gait; i++)
{
for (int j = 0; j < 4; j++)
{
- M.row(i).segment<3>(1 + 3 * j) = array[i].col(j);
+ M.row(i).segment<3>(3 * j) = array[i].col(j);
}
}
return M;
diff --git a/src/MPC.cpp b/src/MPC.cpp
index 2aeba4125bb92812645f0b4cc50df5d52ab2acad..a9e7a83f58ee6b785940d34f223644722aa0e946 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -415,8 +415,8 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
int j = 0;
int k_cum = 0;
// Iterate over all phases of the gait
- while (gait(j, 0) != 0) {
- for (int k = k_cum; k < (k_cum + gait(j, 0)); k++) {
+ while (!gait.row(j).isZero()) {
+ for (int k = k_cum; k < (k_cum + 1); k++) {
// Get inverse of the inertia matrix for time step k
double c = cos(xref(5, k));
double s = sin(xref(5, k));
@@ -427,7 +427,7 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
// Get skew-symetric matrix for each foothold
// Eigen::Map<Eigen::Matrix<double, 3, 4>> fsteps_tmp((fsteps.block(j, 1, 1, 12)).data(), 3, 4);
- footholds_tmp = fsteps.block(j, 1, 1, 12);
+ footholds_tmp = fsteps.row(j); // block(j, 1, 1, 12);
// footholds = footholds_tmp.reshaped(3, 4);
Eigen::Map<Eigen::MatrixXd> footholds_bis(footholds_tmp.data(), 3, 4);
@@ -443,7 +443,7 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
}
}
- k_cum += gait(j, 0);
+ k_cum++;
j++;
}
@@ -656,20 +656,16 @@ N is the number of time step in the prediction horizon.
*/
int MPC::construct_S() {
int i = 0;
- int k = 0;
- Eigen::Matrix<int, 20, 5> inv_gait = Eigen::Matrix<int, 20, 5>::Ones() - gait;
- while (gait(i, 0) != 0) {
+ Eigen::Matrix<int, 20, 4> inv_gait = Eigen::Matrix<int, 20, 4>::Ones() - gait;
+ while (!gait.row(i).isZero()) {
// S_gait.block(k*12, 0, gait[i, 0]*12, 1) = (1 - (gait.block(i, 1, 1, 4)).transpose()).replicate<gait[i, 0], 1>()
// not finished;
- for (int a = 0; a < gait(i, 0); a++) {
- for (int b = 0; b < 4; b++) {
- for (int c = 0; c < 3; c++) {
- S_gait(k * 12 + 12 * a + 3 * b + c, 0) = inv_gait(i, 1 + b);
- }
+ for (int b = 0; b < 4; b++) {
+ for (int c = 0; c < 3; c++) {
+ S_gait(i * 12 + 3 * b + c, 0) = inv_gait(i, b);
}
}
- k += gait(i, 0);
i++;
}
@@ -680,21 +676,19 @@ int MPC::construct_S() {
Reconstruct the gait matrix based on the fsteps matrix since only the last one is received by the MPC
*/
int MPC::construct_gait(Eigen::MatrixXd fsteps_in) {
- // First column is identical
- gait.col(0) = fsteps_in.col(0).cast<int>();
int k = 0;
- while (gait(k, 0) != 0) {
+ while (!gait.row(k).isZero()) {
for (int i = 0; i < 4; i++) {
- if (fsteps_in(k, 1 + i * 3) == 0.0) {
- gait(k, 1 + i) = 0;
+ if (fsteps_in(k, i * 3) == 0.0) {
+ gait(k, i) = 0;
} else {
- gait(k, 1 + i) = 1;
+ gait(k, i) = 1;
}
}
k++;
}
- gait.row(k) << 0, 0, 0, 0, 0;
+ gait.row(k) << 0, 0, 0, 0;
return 0;
}