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

New StatePlanner class independent from Planner to generate the CoM trajectory

parent 41029676
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 232 additions and 119 deletions
CMakeLists.txt
+ 2
− 0
View file @ ee334b05
......@@ -50,6 +50,7 @@ set(${PROJECT_NAME}_HEADERS
include/qrw/Gait.hpp
include/qrw/FootTrajectoryGenerator.hpp
include/qrw/FootstepPlanner.hpp
include/qrw/StatePlanner.hpp
include/qrw/Types.h
include/qrw/InvKin.hpp
include/qrw/QPWBC.hpp
......@@ -63,6 +64,7 @@ set(${PROJECT_NAME}_SOURCES
src/Gait.cpp
src/FootTrajectoryGenerator.cpp
src/FootstepPlanner.cpp
src/StatePlanner.cpp
src/Planner.cpp
src/InvKin.cpp
src/QPWBC.cpp
......
include/qrw/FootstepPlanner.hpp
+ 2
− 19
View file @ ee334b05
......@@ -59,8 +59,7 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
MatrixN computeTargetFootstep(VectorN const& q,
Vector6 const& v,
Vector6 const& b_vref,
double const z_average);
Vector6 const& b_vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
......@@ -73,7 +72,7 @@ public:
void rollGait(int const k,
int const k_mpc);
MatrixN getXReference();
// MatrixN getXReference();
MatrixN getFootsteps();
MatrixN getTargetFootsteps();
......@@ -103,22 +102,6 @@ private:
////////////////////////////////////////////////////////////////////////////////////////////////
void compute_next_footstep(int i, int j);
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute the reference trajectory of the CoM for each time step of the
/// predition horizon. The ouput is a matrix of size 12 by (N+1) with N the number
/// of time steps in the gait cycle (T_gait/dt) and 12 the position, orientation,
/// linear velocity and angular velocity vertically stacked. The first column contains
/// the current state while the remaining N columns contains the desired future states.
///
/// \param[in] q current position vector of the flying base in world frame (linear and angular stacked)
/// \param[in] v current velocity vector of the flying base in world frame (linear and angular stacked)
/// \param[in] vref desired velocity vector of the flying base in world frame (linear and angular stacked)
/// \param[in] z_average average height of feet currently in stance phase
///
////////////////////////////////////////////////////////////////////////////////////////////////
int getRefStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average);
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Update desired location of footsteps using information coming from the footsteps planner
......
include/qrw/Planner.hpp
+ 1
− 4
View file @ ee334b05
......@@ -68,14 +68,12 @@ public:
/// \param[in] q current position vector of the flying base in world frame (linear and angular stacked)
/// \param[in] v current velocity vector of the flying base in world frame (linear and angular stacked)
/// \param[in] b_vref desired velocity vector of the flying base in base frame (linear and angular stacked)
/// \param[in] z_average average height of feet currently in stance phase
///
////////////////////////////////////////////////////////////////////////////////////////////////
void run_planner(int const k,
VectorN const& q,
Vector6 const& v,
Vector6 const& b_vref,
double const z_average);
Vector6 const& b_vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
......@@ -102,7 +100,6 @@ public:
void setGait(MatrixN const& gaitMatrix);
// Accessors (to retrieve C data from Python)
MatrixN get_xref();
MatrixN get_fsteps();
MatrixN get_gait();
Matrix3N get_goals();
......
include/qrw/StatePlanner.hpp 0 → 100644
+ 75
− 0
View file @ ee334b05
///////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief This is the header for StatePlanner class
///
/// \details Planner that outputs the reference trajectory of the base based on the reference
/// velocity given by the user and the current position/velocity of the base
///
//////////////////////////////////////////////////////////////////////////////////////////////////
#ifndef STATEPLANNER_H_INCLUDED
#define STATEPLANNER_H_INCLUDED
#include "pinocchio/math/rpy.hpp"
#include "qrw/Types.h"
class StatePlanner
{
public:
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Empty constructor
///
////////////////////////////////////////////////////////////////////////////////////////////////
StatePlanner();
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Destructor.
///
////////////////////////////////////////////////////////////////////////////////////////////////
~StatePlanner() {}
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Initializer
///
////////////////////////////////////////////////////////////////////////////////////////////////
void initialize(double dt_in, double T_mpc_in, double h_ref_in);
////////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Compute the reference trajectory of the CoM for each time step of the
/// predition horizon. The ouput is a matrix of size 12 by (N+1) with N the number
/// of time steps in the gait cycle (T_gait/dt) and 12 the position, orientation,
/// linear velocity and angular velocity vertically stacked. The first column contains
/// the current state while the remaining N columns contains the desired future states.
///
/// \param[in] q current position vector of the flying base in world frame (linear and angular stacked)
/// \param[in] v current velocity vector of the flying base in world frame (linear and angular stacked)
/// \param[in] vref desired velocity vector of the flying base in world frame (linear and angular stacked)
/// \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);
MatrixN getXReference() { return xref_; }
private:
double dt_; // Time step of the contact sequence (time step of the MPC)
double T_mpc_; // MPC period (prediction horizon)
double h_ref_; // Reference height for the trunk
int n_steps_; // Number of time steps in the prediction horizon
Vector3 RPY_; // To store roll, pitch and yaw angles
// 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_;
VectorN dt_vector_; // Vector containing all time steps in the prediction horizon
};
#endif // STATEPLANNER_H_INCLUDED
python/gepadd.cpp
+ 38
− 3
View file @ ee334b05
......@@ -2,6 +2,7 @@
#include "qrw/InvKin.hpp"
#include "qrw/MPC.hpp"
#include "qrw/Planner.hpp"
#include "qrw/StatePlanner.hpp"
#include "qrw/QPWBC.hpp"
#include <boost/python.hpp>
......@@ -37,7 +38,9 @@ struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC>>
void exposeMPC() { MPCPythonVisitor<MPC>::expose(); }
/////////////////////////////////
/// Binding Planner class
/////////////////////////////////
template <typename Planner>
struct PlannerPythonVisitor : public bp::def_visitor<PlannerPythonVisitor<Planner>>
{
......@@ -50,7 +53,6 @@ struct PlannerPythonVisitor : public bp::def_visitor<PlannerPythonVisitor<Planne
"fsteps_in", "shoulders positions"),
"Constructor with parameters."))
.def("get_xref", &Planner::get_xref, "Get xref matrix.\n")
.def("get_fsteps", &Planner::get_fsteps, "Get fsteps matrix.\n")
.def("get_gait", &Planner::get_gait, "Get gait matrix.\n")
.def("get_goals", &Planner::get_goals, "Get position goals matrix.\n")
......@@ -58,7 +60,7 @@ struct PlannerPythonVisitor : public bp::def_visitor<PlannerPythonVisitor<Planne
.def("get_agoals", &Planner::get_agoals, "Get acceleration goals matrix.\n")
// Run Planner from Python
.def("run_planner", &Planner::run_planner, bp::args("k", "q", "v", "b_vref", "z_average"),
.def("run_planner", &Planner::run_planner, bp::args("k", "q", "v", "b_vref"),
"Run Planner from Python.\n")
// Update gait matrix from Python
......@@ -79,7 +81,39 @@ struct PlannerPythonVisitor : public bp::def_visitor<PlannerPythonVisitor<Planne
};
void exposePlanner() { PlannerPythonVisitor<Planner>::expose(); }
/////////////////////////////////
/// Binding StatePlanner class
/////////////////////////////////
template <typename StatePlanner>
struct StatePlannerPythonVisitor : public bp::def_visitor<StatePlannerPythonVisitor<StatePlanner>>
{
template <class PyClassStatePlanner>
void visit(PyClassStatePlanner& cl) const
{
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
.def("getXReference", &StatePlanner::getXReference, "Get xref matrix.\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"),
"Run StatePlanner from Python.\n");
}
static void expose()
{
bp::class_<StatePlanner>("StatePlanner", bp::no_init).def(StatePlannerPythonVisitor<StatePlanner>());
ENABLE_SPECIFIC_MATRIX_TYPE(MatrixN);
}
};
void exposeStatePlanner() { StatePlannerPythonVisitor<StatePlanner>::expose(); }
/////////////////////////////////
/// Binding InvKin class
/////////////////////////////////
template <typename InvKin>
struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin>>
{
......@@ -144,6 +178,7 @@ BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
exposeMPC();
exposePlanner();
exposeStatePlanner();
exposeInvKin();
exposeQPWBC();
}
\ No newline at end of file
scripts/Controller.py
+ 12
− 3
View file @ ee334b05
......@@ -10,6 +10,7 @@ import pybullet as pyb
from Planner import PyPlanner
import pinocchio as pin
from solopython.utils.viewerClient import viewerClient, NonBlockingViewerFromRobot
import libquadruped_reactive_walking as lqrw
class Result:
"""Object to store the result of the control loop
......@@ -116,6 +117,9 @@ class Controller:
self.planner = PyPlanner(dt_mpc, dt_wbc, T_gait, T_mpc,
k_mpc, on_solo8, h_ref, self.fsteps_init)
self.statePlanner = lqrw.StatePlanner()
self.statePlanner.initialize(dt_mpc, T_mpc, h_ref)
# Wrapper that makes the link with the solver that you want to use for the MPC
# First argument to True to have PA's MPC, to False to have Thomas's MPC
self.enable_multiprocessing = True
......@@ -211,11 +215,16 @@ class Controller:
self.planner.updateGait(self.k, self.k_mpc, self.q[0:7, 0:1], self.joystick)
# Run planner
self.planner.run_planner(self.k, self.k_mpc, self.q[0:7, 0:1],
self.v[0:6, 0:1].copy(), self.joystick.v_ref, self.q_estim[2, 0], 0.0)
self.planner.run_planner(self.k, self.q[0:7, 0:1], self.v[0:6, 0:1].copy(), self.joystick.v_ref)
# 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(), self.joystick.v_ref, 0.0)
# Result can be retrieved with self.statePlanner.getXReference()
self.planner.xref = self.statePlanner.getXReference()
t_planner = time.time()
self.planner.setGait(self.planner.gait)
# self.planner.setGait(self.planner.gait)
# Process MPC once every k_mpc iterations of TSID
if (self.k % self.k_mpc) == 0:
......
scripts/Planner.py
+ 2
− 3
View file @ ee334b05
......@@ -32,12 +32,11 @@ class PyPlanner:
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, k_mpc, q, v, b_vref, h_estim, z_average):
def run_planner(self, k, q, v, b_vref):
# Run C++ planner
self.planner.run_planner(k, q, v, b_vref, np.double(z_average))
self.planner.run_planner(k, q, v, b_vref)
# Retrieve data from C++ planner
self.xref = self.planner.get_xref()
self.fsteps = self.planner.get_fsteps()
self.gait = self.planner.get_gait()
self.goals = self.planner.get_goals()
......
src/FootstepPlanner.cpp
+ 1
− 83
View file @ ee334b05
......@@ -159,81 +159,6 @@ void FootstepPlanner::compute_next_footstep(int i, int j)
nextFootstep_.row(2) = Vector4::Zero().transpose();
}
int FootstepPlanner::getRefStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average)
{
VectorN dt_vector = VectorN::LinSpaced(n_steps, dt, T_mpc);
// 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 x and y depending on x and y velocities (cumulative sum)
if (vref(5) != 0)
{
for (int i = 0; i < n_steps; i++)
{
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);
}
}
else
{
for (int i = 0; i < n_steps; i++)
{
xref(0, 1 + i) = vref(0) * dt_vector(i);
xref(1, 1 + i) = vref(1) * dt_vector(i);
}
}
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;
}
// 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
// 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);
for (int i = 0; i < n_steps; i++)
{
xref(0, 1 + i) += xref(0, 0);
xref(1, 1 + i) += xref(1, 0);
}
if (gait_->getIsStatic())
{
Vector19 q_static = gait_->getQStatic();
Eigen::Quaterniond quat(q_static(6, 0), q_static(3, 0), q_static(4, 0), q_static(5, 0)); // w, x, y, z
RPY << pinocchio::rpy::matrixToRpy(quat.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;
}
}
return 0;
}
void FootstepPlanner::update_target_footsteps()
{
for (int i = 0; i < 4; i++)
......@@ -249,8 +174,7 @@ void FootstepPlanner::update_target_footsteps()
MatrixN FootstepPlanner::computeTargetFootstep(VectorN const& q,
Vector6 const& v,
Vector6 const& b_vref,
double const z_average)
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
......@@ -267,11 +191,6 @@ MatrixN FootstepPlanner::computeTargetFootstep(VectorN const& q,
// Compute the desired location of footsteps over the prediction horizon
compute_footsteps(q, v, vref);
// Get the reference trajectory for the MPC
getRefStates(q, v, vref, z_average);
// Update desired location of footsteps on the ground
update_target_footsteps();
return targetFootstep_;
......@@ -284,7 +203,6 @@ void FootstepPlanner::rollGait(int const k,
gait_->roll(k, footsteps_[1], currentFootstep_);
}
MatrixN FootstepPlanner::getXReference() { return xref; }
MatrixN FootstepPlanner::getFootsteps() { return vectorToMatrix(footsteps_); }
MatrixN FootstepPlanner::getTargetFootsteps() { return targetFootstep_; }
......
src/Planner.cpp
+ 3
− 4
View file @ ee334b05
......@@ -27,11 +27,11 @@ Planner::Planner(double dt_in,
void Planner::run_planner(int const k,
VectorN const& q,
Vector6 const& v,
Vector6 const& b_vref,
double const z_average)
Vector6 const& b_vref)
{
targetFootstep_ = footstepPlanner_.computeTargetFootstep(q, v, b_vref, z_average);
targetFootstep_ = footstepPlanner_.computeTargetFootstep(q, v, b_vref);
fooTrajectoryGenerator_.update(k, targetFootstep_);
}
void Planner::updateGait(int const k,
......@@ -48,7 +48,6 @@ void Planner::setGait(MatrixN const& gaitMatrix)
gait_->setGait(gaitMatrix);
}
MatrixN Planner::get_xref() { return footstepPlanner_.getXReference(); }
MatrixN Planner::get_fsteps() { return footstepPlanner_.getFootsteps(); }
MatrixN Planner::get_gait() { return gait_->getCurrentGait(); }
Matrix3N Planner::get_goals() { return fooTrajectoryGenerator_.getFootPosition(); }
......
src/StatePlanner.cpp 0 → 100644
+ 96
− 0
View file @ ee334b05
#include "qrw/StatePlanner.hpp"
StatePlanner::StatePlanner()
: dt_(0.0)
, T_mpc_(0.0)
, h_ref_(0.0)
, n_steps_(0)
, RPY_(Vector3::Zero())
{
// Empty
}
void StatePlanner::initialize(double dt_in, double T_mpc_in, double h_ref_in)
{
dt_ = dt_in;
T_mpc_ = T_mpc_in;
h_ref_ = h_ref_in;
n_steps_ = (int)std::lround(T_mpc_in / dt_in);
xref_ = MatrixN::Zero(12, 1 + n_steps_);
dt_vector_ = VectorN::LinSpaced(n_steps_, dt_, T_mpc_);
}
void StatePlanner::computeRefStates(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 x and y depending on x and y velocities (cumulative sum)
if (vref(5) != 0)
{
for (int i = 0; i < n_steps_; i++)
{
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);
}
}
else
{
for (int i = 0; i < n_steps_; i++)
{
xref_(0, 1 + i) = vref(0) * dt_vector_(i);
xref_(1, 1 + i) = vref(1) * dt_vector_(i);
}
}
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;
}
// 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
// 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);
for (int i = 0; i < n_steps_; i++)
{
xref_(0, 1 + i) += xref_(0, 0);
xref_(1, 1 + i) += xref_(1, 0);
}
/*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_;
}
}*/
}
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