diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 3f5338fbdc969d53dfb65a2d7832697c1cb74f5b..9ebd0ef415b5cee6b1929959ad17ff6ae4c733ff 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);
+ Gait& gait);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index 1f925121761778274c6ee3084c74b49817fa083d..f26d8b00f6cf7968a7b92ae3c78a6b906233c15d 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -43,7 +43,7 @@ public:
double T_mpc_in,
double h_ref_in,
MatrixN const& shouldersIn,
- Gait & gaitIn);
+ Gait& gaitIn);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -52,7 +52,6 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
~FootstepPlanner() {}
-
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute the desired location of footsteps and update relevant matrices
@@ -88,7 +87,7 @@ private:
/// \param[in] vref desired velocity vector of the flying base in world frame(linear and angular stacked)
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void compute_footsteps(VectorN const& q, Vector6 const& v, Vector6 const& vref);
+ void computeFootsteps(VectorN const& q, Vector6 const& v, Vector6 const& vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -100,18 +99,18 @@ private:
/// \retval Matrix with the next footstep positions
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void compute_next_footstep(int i, int j);
+ void computeNextFootstep(int i, int j);
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Update desired location of footsteps using information coming from the footsteps planner
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void update_target_footsteps();
+ void updateTargetFootsteps();
MatrixN vectorToMatrix(std::array<Matrix34, N0_gait> const& array);
- Gait* gait_; // Gait object to hold the gait informations
+ Gait* gait_; // Gait object to hold the gait informations
double dt; // Time step of the contact sequence (time step of the MPC)
double T_gait; // Gait period
@@ -141,10 +140,10 @@ private:
Vector3 q_tmp;
Vector3 q_dxdy;
- Vector3 RPY;
+ Vector3 RPY_;
+ Eigen::Quaterniond quat_;
Vector3 b_v;
Vector6 b_vref;
-
};
#endif // FOOTSTEPPLANNER_H_INCLUDED
diff --git a/include/qrw/Gait.hpp b/include/qrw/Gait.hpp
index bbd82a6e66b58e750e4977751434769cdc536255..10fe3eefb0be8cb8ba284fb7f434274cb0b35837 100644
--- a/include/qrw/Gait.hpp
+++ b/include/qrw/Gait.hpp
@@ -41,49 +41,6 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
void initialize(double dt_in, double T_gait_in, double T_mpc_in);
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a slow walking gait, raising and moving only one foot at a time
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- int create_walk();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a trot gait with diagonaly opposed legs moving at the same time
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- int create_trot();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a pacing gait with legs on the same side (left or right) moving at the same time
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- int create_pacing();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a bounding gait with legs on the same side (front or hind) moving at the same time
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- int create_bounding();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Create a static gait with all legs in stance phase
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- int create_static();
-
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Initialize content of the gait matrix based on the desired gait, the gait period and
- /// the length of the prediciton horizon
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- int create_gait_f();
-
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute the remaining and total duration of a swing phase or a stance phase based
@@ -96,9 +53,6 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
double getPhaseDuration(int i, int j, double value);
- // TODO
- void updateGait(int const k, int const k_mpc, VectorN const& q, int const joystickCode);
-
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Handle the joystick code to trigger events (change of gait for instance)
@@ -118,6 +72,9 @@ public:
/// Insert future desired gait phase at the end of the gait matrix
///
////////////////////////////////////////////////////////////////////////////////////////////////
+ void updateGait(int const k, int const k_mpc, VectorN const& q, int const joystickCode);
+
+ // TODO
void rollGait();
////////////////////////////////////////////////////////////////////////////////////////////////
@@ -139,6 +96,49 @@ public:
bool isNewPhase() { return newPhase_; }
private:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a slow walking gait, raising and moving only one foot at a time
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_walk();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a trot gait with diagonaly opposed legs moving at the same time
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_trot();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a pacing gait with legs on the same side (left or right) moving at the same time
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_pacing();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a bounding gait with legs on the same side (front or hind) moving at the same time
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_bounding();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Create a static gait with all legs in stance phase
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_static();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize content of the gait matrix based on the desired gait, the gait period and
+ /// the length of the prediciton horizon
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void create_gait_f();
+
MatrixN pastGait_; // Past gait
MatrixN currentGait_; // Current and future gait
MatrixN desiredGait_; // Future desired gait
diff --git a/include/qrw/StatePlanner.hpp b/include/qrw/StatePlanner.hpp
index 21d3cb3fae7d03a6127845ba5eca775a469ad946..0bca1cf85c98105c3c0f12f8df8101d88ecf8349 100644
--- a/include/qrw/StatePlanner.hpp
+++ b/include/qrw/StatePlanner.hpp
@@ -52,9 +52,9 @@ public:
/// \param[in] z_average average height of feet currently in stance phase
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void computeRefStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average);
+ void computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average);
- MatrixN getXReference() { return xref_; }
+ MatrixN getReferenceStates() { return referenceStates_; }
int getNSteps() { return n_steps_; }
private:
@@ -66,7 +66,7 @@ private:
// Reference trajectory matrix of size 12 by (1 + N) with the current state of
// the robot in column 0 and the N steps of the prediction horizon in the others
- MatrixN xref_;
+ MatrixN referenceStates_;
VectorN dt_vector_; // Vector containing all time steps in the prediction horizon
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 610ed706c0da9fbc3483e61e2c8a2472989779ca..a6685eac72825b4eb9d146b7a54d5996c343fcdf 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -52,14 +52,14 @@ struct StatePlannerPythonVisitor : public bp::def_visitor<StatePlannerPythonVisi
{
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def("getXReference", &StatePlanner::getXReference, "Get xref matrix.\n")
+ .def("getReferenceStates", &StatePlanner::getReferenceStates, "Get xref matrix.\n")
.def("getNSteps", &StatePlanner::getNSteps, "Get number of steps in prediction horizon.\n")
.def("initialize", &StatePlanner::initialize, bp::args("dt_in", "T_mpc_in", "h_ref_in"),
"Initialize StatePlanner from Python.\n")
// Run StatePlanner from Python
- .def("computeRefStates", &StatePlanner::computeRefStates, bp::args("q", "v", "b_vref", "z_average"),
+ .def("computeReferenceStates", &StatePlanner::computeReferenceStates, bp::args("q", "v", "b_vref", "z_average"),
"Run StatePlanner from Python.\n");
}
diff --git a/scripts/Controller.py b/scripts/Controller.py
index c7b2d57fe83a7c039078457ccc4dd5fde22e014c..ca84f3ef50bab9c417de8eb711b4f00c363d6c5a 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -259,9 +259,9 @@ class Controller:
self.agoals = self.planner.get_agoals()"""
# Run state planner (outputs the reference trajectory of the CoM / base)
- self.statePlanner.computeRefStates(self.q[0:7, 0:1], self.v[0:6, 0:1].copy(), o_v_ref, 0.0)
- # Result can be retrieved with self.statePlanner.getXReference()
- xref = self.statePlanner.getXReference()
+ self.statePlanner.computeReferenceStates(self.q[0:7, 0:1], self.v[0:6, 0:1].copy(), o_v_ref, 0.0)
+ # Result can be retrieved with self.statePlanner.getReferenceStates()
+ xref = self.statePlanner.getReferenceStates()
fsteps = self.footstepPlanner.getFootsteps()
cgait = self.gait.getCurrentGait()
diff --git a/scripts/Planner.py b/scripts/Planner.py
new file mode 100644
index 0000000000000000000000000000000000000000..333ba0a63a2b03490bef6cd89f94db70caa2030a
--- /dev/null
+++ b/scripts/Planner.py
@@ -0,0 +1,74 @@
+import time
+import numpy as np
+import libquadruped_reactive_walking as la
+
+class PyPlanner:
+ def __init__(self, dt, dt_tsid, T_gait, T_mpc, k_mpc, on_solo8, h_ref, fsteps_init):
+ # Reference height for the trunk
+ self.h_ref = h_ref
+
+ # Number of time steps in the prediction horizon
+ self.n_steps = np.int(T_gait/dt)
+
+ # Reference trajectory matrix of size 12 by (1 + N) with the current state of
+ # the robot in column 0 and the N steps of the prediction horizon in the others
+ self.xref = np.zeros((12, 1 + self.n_steps))
+
+ # Gait matrix
+ self.gait = np.zeros((20, 5))
+ self.is_static = False # Flag for static gait
+ self.q_static = np.zeros(19)
+ self.RPY_static = np.zeros((3, 1))
+
+ self.goals = fsteps_init.copy() # Store 3D target position for feet
+ self.vgoals = np.zeros((3, 4)) # Store 3D target velocity for feet
+ self.agoals = np.zeros((3, 4)) # Store 3D target acceleration for feet
+ self.target_position = np.zeros((3, 4)) # Store 3D target acceleration for feet
+
+ # C++ class
+ shoulders = np.zeros((3, 4))
+ shoulders[0, :] = [0.1946, 0.1946, -0.1946, -0.1946]
+ shoulders[1, :] = [0.14695, -0.14695, 0.14695, -0.14695]
+ self.planner = la.Planner(dt, dt_tsid, T_gait, T_mpc, k_mpc, h_ref, fsteps_init, shoulders)
+
+ def run_planner(self, k, q, v, b_vref):
+ # Run C++ planner
+ self.planner.run_planner(k, q, v, b_vref)
+
+ # Retrieve data from C++ planner
+ self.fsteps = self.planner.get_fsteps()
+ self.gait = self.planner.get_gait()
+ self.goals = self.planner.get_goals()
+ self.vgoals = self.planner.get_vgoals()
+ self.agoals = self.planner.get_agoals()
+ return 0
+
+ def updateGait(self, k, k_mpc, q, joystick=None):
+ # Check joystick buttons to trigger a change of gait type
+ joystick_code = 0
+ if joystick is not None:
+ if joystick.northButton:
+ joystick_code = 1
+ self.is_static = False
+ joystick.northButton = False
+ elif joystick.eastButton:
+ joystick_code = 2
+ self.is_static = False
+ joystick.eastButton = False
+ elif joystick.southButton:
+ joystick_code = 3
+ self.is_static = False
+ joystick.southButton = False
+ elif joystick.westButton:
+ joystick_code = 4
+ self.is_static = True
+ self.q_static[0:7, 0:1] = q.copy()
+ joystick.westButton = False
+
+ # Update gait internally with functions of the gait class
+ self.planner.updateGait(k, k_mpc, q, joystick_code)
+ return 0
+
+ def setGait(self, gaitMatrix):
+ # Directly set the gait matrix from Python
+ self.planner.setGait(gaitMatrix)
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 5f45f1f588782f1f49fabd71f1821f7588a28514..45b0a8a45fc6ffa4950448ef1500bdfb1780fe7f 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -14,7 +14,6 @@ from LoggerSensors import LoggerSensors
from LoggerControl import LoggerControl
-
SIMULATION = True
LOGGING = False
PLOTTING = True
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index 4b194f315c41b411c494a50fe24d6029251b3dfb..f94e0905fb093cbbe67fa226e10dc0accbcb20e1 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -26,7 +26,7 @@ void FootTrajectoryGenerator::initialize(double const maxHeightIn,
MatrixN const& initialFootPosition,
double const& dt_tsid_in,
int const& k_mpc_in,
- Gait & gaitIn)
+ Gait& gaitIn)
{
dt_tsid = dt_tsid_in;
k_mpc = k_mpc_in;
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index d543a021b04acf772f476b14cdcb17b4fa3bb400..bcbffb4d08d3279314afdcdf108a952709da028d 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -14,7 +14,7 @@ FootstepPlanner::FootstepPlanner()
, dy(VectorN::Zero(N0_gait))
, q_tmp(Vector3::Zero())
, q_dxdy(Vector3::Zero())
- , RPY(Vector3::Zero())
+ , RPY_(Vector3::Zero())
, b_v(Vector3::Zero())
, b_vref(Vector6::Zero())
{
@@ -25,7 +25,7 @@ void FootstepPlanner::initialize(double dt_in,
double T_mpc_in,
double h_ref_in,
MatrixN const& shouldersIn,
- Gait & gaitIn)
+ Gait& gaitIn)
{
dt = dt_in;
T_mpc = T_mpc_in;
@@ -39,7 +39,7 @@ void FootstepPlanner::initialize(double dt_in,
Rz(2, 2) = 1.0;
}
-void FootstepPlanner::compute_footsteps(VectorN const& q, Vector6 const& v, Vector6 const& vref)
+void FootstepPlanner::computeFootsteps(VectorN const& q, Vector6 const& v, Vector6 const& vref)
{
footsteps_.fill(Matrix34::Zero());
MatrixN gait = gait_->getCurrentGait();
@@ -56,11 +56,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) = dt;
- yaws(0) = vref(5) * dt_cum(0) + RPY(2);
+ yaws(0) = vref(5) * dt_cum(0) + RPY_(2);
for (int j = 1; j < N0_gait; j++)
{
dt_cum(j) = gait.row(j).isZero() ? dt_cum(j - 1) : dt_cum(j - 1) + dt;
- yaws(j) = vref(5) * dt_cum(j) + RPY(2);
+ yaws(j) = vref(5) * dt_cum(j) + RPY_(2);
}
// Displacement following the reference velocity compared to current position
@@ -112,7 +112,7 @@ void FootstepPlanner::compute_footsteps(VectorN const& q, Vector6 const& v, Vect
q_dxdy << dx(i - 1, 0), dy(i - 1, 0), 0.0;
// Get future desired position of footsteps
- compute_next_footstep(i, j);
+ computeNextFootstep(i, j);
// Get desired position of footstep compared to current position
double c = std::cos(yaws(i - 1));
@@ -126,7 +126,7 @@ void FootstepPlanner::compute_footsteps(VectorN const& q, Vector6 const& v, Vect
}
}
-void FootstepPlanner::compute_next_footstep(int i, int j)
+void FootstepPlanner::computeNextFootstep(int i, int j)
{
nextFootstep_ = Matrix34::Zero();
@@ -156,7 +156,7 @@ void FootstepPlanner::compute_next_footstep(int i, int j)
nextFootstep_.row(2) = Vector4::Zero().transpose();
}
-void FootstepPlanner::update_target_footsteps()
+void FootstepPlanner::updateTargetFootsteps()
{
for (int i = 0; i < 4; i++)
{
@@ -174,31 +174,29 @@ MatrixN FootstepPlanner::computeTargetFootstep(VectorN const& q,
Vector6 const& b_vref)
{
// Get the reference velocity in world frame (given in base frame)
- Eigen::Quaterniond quat(q(6), q(3), q(4), q(5)); // w, x, y, z
- RPY << pinocchio::rpy::matrixToRpy(quat.toRotationMatrix());
+ quat_ = {q(6), q(3), q(4), q(5)}; // w, x, y, z
+ RPY_ << pinocchio::rpy::matrixToRpy(quat_.toRotationMatrix());
- double c = std::cos(RPY(2));
- double s = std::sin(RPY(2));
+ double c = std::cos(RPY_(2));
+ double s = std::sin(RPY_(2));
Rz.topLeftCorner<2, 2>() << c, -s, s, c;
Vector6 vref = b_vref;
vref.head(3) = Rz * b_vref.head(3);
-
// Compute the desired location of footsteps over the prediction horizon
- compute_footsteps(q, v, vref);
+ computeFootsteps(q, v, vref);
// Update desired location of footsteps on the ground
- update_target_footsteps();
+ updateTargetFootsteps();
return targetFootstep_;
}
-void FootstepPlanner::updateNewContact() // Gait const& gait) // MaxtrixN const& currentGait)
+void FootstepPlanner::updateNewContact()
{
- // Entering new contact phase, store positions of feet that are now in contact
for (int i = 0; i < 4; i++)
{
- if (gait_->getCurrentGaitCoeff(0, i) == 1.0) //if (currentGait(0, 1 + i) == 1.0)
+ if (gait_->getCurrentGaitCoeff(0, i) == 1.0)
{
currentFootstep_.col(i) = (footsteps_[1]).col(i);
}
diff --git a/src/Gait.cpp b/src/Gait.cpp
index 1062952ddb335a2bc83d8d579d41ddbe19083352..ebbf43cb893f1e693c9387f899e9b4467083e740 100644
--- a/src/Gait.cpp
+++ b/src/Gait.cpp
@@ -31,104 +31,76 @@ void Gait::initialize(double dt_in, double T_gait_in, double T_mpc_in)
}
-int Gait::create_walk()
+void 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, 4>::Zero();
+ desiredGait_ = MatrixN::Zero(N0_gait, 4);
- // 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
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;
+ sequence << 0.0, 1.0, 1.0, 1.0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1.0, 0.0, 1.0, 1.0;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1.0, 1.0, 0.0, 1.0;
+ desiredGait_.block(2*N, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1.0, 1.0, 1.0, 0.0;
+ desiredGait_.block(3*N, 0, N, 4) = sequence.colwise().replicate(N);
}
-int Gait::create_trot()
+void 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, 4>::Zero();
+ desiredGait_ = MatrixN::Zero(N0_gait, 4);
- // 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
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;
+ sequence << 1.0, 0.0, 0.0, 1.0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 0.0, 1.0, 1.0, 0.0;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
}
-int Gait::create_pacing()
+void Gait::create_pacing()
{
// 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_ = MatrixN::Zero(N0_gait, 4);
- // 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
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;
+ sequence << 1.0, 0.0, 1.0, 0.0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 0.0, 1.0, 0.0, 1.0;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
}
-int Gait::create_bounding()
+void Gait::create_bounding()
{
// 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_ = MatrixN::Zero(N0_gait, 4);
- // 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
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;
+ sequence << 1.0, 1.0, 0.0, 0.0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 0.0, 0.0, 1.0, 1.0;
desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
-
- return 0;
}
-int Gait::create_static()
+void Gait::create_static()
{
- // Number of timesteps in a half period of gait
- int N = (int)std::lround(T_gait_ / dt_);
+ desiredGait_ = MatrixN::Zero(N0_gait, 4);
- desiredGait_ = Eigen::Matrix<double, N0_gait, 5>::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
Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1, 1, 1, 1;
+ sequence << 1.0, 1.0, 1.0, 1.0;
desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
-
- return 0;
}
-int Gait::create_gait_f()
+void Gait::create_gait_f()
{
int i = 0;
@@ -158,8 +130,6 @@ int Gait::create_gait_f()
desiredGait_.row(m).swap(desiredGait_.row(m+1));
}
}
-
- return 0;
}
double Gait::getPhaseDuration(int i, int j, double value)
diff --git a/src/StatePlanner.cpp b/src/StatePlanner.cpp
index 72b67032124a2a7b3960e6cb30283f59e2f692d0..978f732659c4a6544b0b9ec22f1afd6febad051a 100644
--- a/src/StatePlanner.cpp
+++ b/src/StatePlanner.cpp
@@ -14,81 +14,43 @@ void StatePlanner::initialize(double dt_in, double T_mpc_in, double h_ref_in)
dt_ = dt_in;
h_ref_ = h_ref_in;
n_steps_ = (int)std::lround(T_mpc_in / dt_in);
- xref_ = MatrixN::Zero(12, 1 + n_steps_);
+ referenceStates_ = MatrixN::Zero(12, 1 + n_steps_);
dt_vector_ = VectorN::LinSpaced(n_steps_, dt_, T_mpc_in);
}
-void StatePlanner::computeRefStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average)
+void StatePlanner::computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average)
{
Eigen::Quaterniond quat(q(6), q(3), q(4), q(5)); // w, x, y, z
RPY_ << pinocchio::rpy::matrixToRpy(quat.toRotationMatrix());
- // Update yaw and yaw velocity
- xref_.block(5, 1, 1, n_steps_) = vref(5) * dt_vector_.transpose();
- for (int i = 0; i < n_steps_; i++)
- {
- xref_(11, 1 + i) = vref(5);
- }
-
- // Update x and y velocities taking into account the rotation of the base over the prediction horizon
- for (int i = 0; i < n_steps_; i++)
- {
- xref_(6, 1 + i) = vref(0) * std::cos(xref_(5, 1 + i)) - vref(1) * std::sin(xref_(5, 1 + i));
- xref_(7, 1 + i) = vref(0) * std::sin(xref_(5, 1 + i)) + vref(1) * std::cos(xref_(5, 1 + i));
- }
+ // Update the current state
+ referenceStates_.block(0, 0, 3, 1) = q.head(3);
+ referenceStates_.block(3, 0, 3, 1) = RPY_;
+ referenceStates_.block(6, 0, 3, 1) = v.head(3);
+ referenceStates_.block(9, 0, 3, 1) = v.tail(3);
- // Update x and y depending on x and y velocities (cumulative sum)
- if (vref(5) != 0)
+ for (int i = 0; i < n_steps_; i++)
{
- for (int i = 0; i < n_steps_; i++)
+ if (vref(5) != 0)
{
- xref_(0, 1 + i) = (vref(0) * std::sin(vref(5) * dt_vector_(i)) + vref(1) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) / vref(5);
- xref_(1, 1 + i) = (vref(1) * std::sin(vref(5) * dt_vector_(i)) - vref(0) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) / vref(5);
+ referenceStates_(0, 1 + i) = (vref(0) * std::sin(vref(5) * dt_vector_(i)) + vref(1) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) / vref(5);
+ referenceStates_(1, 1 + i) = (vref(1) * std::sin(vref(5) * dt_vector_(i)) - vref(0) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) / vref(5);
}
- }
- else
- {
- for (int i = 0; i < n_steps_; i++)
+ else
{
- xref_(0, 1 + i) = vref(0) * dt_vector_(i);
- xref_(1, 1 + i) = vref(1) * dt_vector_(i);
+ referenceStates_(0, 1 + i) = vref(0) * dt_vector_(i);
+ referenceStates_(1, 1 + i) = vref(1) * dt_vector_(i);
}
- }
+ referenceStates_(0, 1 + i) += referenceStates_(0, 0);
+ referenceStates_(1, 1 + i) += referenceStates_(1, 0);
- for (int i = 0; i < n_steps_; i++)
- {
- xref_(5, 1 + i) += RPY_(2);
- xref_(2, 1 + i) = h_ref_ + z_average;
- xref_(8, 1 + i) = 0.0;
- }
+ referenceStates_(2, 1 + i) = h_ref_ + z_average;
- // No need to update Z velocity as the reference is always 0
- // No need to update roll and roll velocity as the reference is always 0 for those
- // No need to update pitch and pitch velocity as the reference is always 0 for those
+ referenceStates_(5, 1 + i) = vref(5) * dt_vector_(i) + RPY_(2);
- // Update the current state
- xref_.block(0, 0, 3, 1) = q.head(3);
- xref_.block(3, 0, 3, 1) = RPY_;
- xref_.block(6, 0, 3, 1) = v.head(3);
- xref_.block(9, 0, 3, 1) = v.tail(3);
+ referenceStates_(6, 1 + i) = vref(0) * std::cos(referenceStates_(5, 1 + i)) - vref(1) * std::sin(referenceStates_(5, 1 + i));
+ referenceStates_(7, 1 + i) = vref(0) * std::sin(referenceStates_(5, 1 + i)) + vref(1) * std::cos(referenceStates_(5, 1 + i));
- for (int i = 0; i < n_steps_; i++)
- {
- xref_(0, 1 + i) += xref_(0, 0);
- xref_(1, 1 + i) += xref_(1, 0);
+ referenceStates_(11, 1 + i) = vref(5);
}
-
- /*if (gait_->getIsStatic())
- {
- Vector19 q_static = gait_->getQStatic();
- Eigen::Quaterniond quatStatic(q_static(6, 0), q_static(3, 0), q_static(4, 0), q_static(5, 0)); // w, x, y, z
- RPY_ << pinocchio::rpy::matrixToRpy(quatStatic.toRotationMatrix());
-
- for (int i = 0; i < n_steps_; i++)
- {
- xref_.block(0, 1 + i, 3, 1) = q_static.block(0, 0, 3, 1);
- xref_.block(3, 1 + i, 3, 1) = RPY_;
- }
- }*/
-
}