From bca4c60d090941f24fef51476a26a9b7eed7218f Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Fri, 16 Apr 2021 14:08:57 +0200
Subject: [PATCH] Cleaning and commenting
---
include/qrw/FootTrajectoryGenerator.hpp | 4 +--
include/qrw/FootstepPlanner.hpp | 36 +++++++------------
include/qrw/StatePlanner.hpp | 1 -
python/gepadd.cpp | 48 +------------------------
scripts/Controller.py | 15 +++-----
src/FootTrajectoryGenerator.cpp | 2 +-
src/FootstepPlanner.cpp | 13 +------
src/StatePlanner.cpp | 4 +--
8 files changed, 23 insertions(+), 100 deletions(-)
diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 63ae2b74..7b52aadc 100644
--- a/include/qrw/FootTrajectoryGenerator.hpp
+++ b/include/qrw/FootTrajectoryGenerator.hpp
@@ -74,7 +74,7 @@ public:
MatrixN getFootAcceleration() { return acceleration_; } ///< Get the next foot acceleration
private:
- Gait* gait_; // std::shared_ptr<Gait> gait_; ///< Target lock before the touchdown
+ Gait* gait_; ///< Target lock before the touchdown
double dt_tsid; ///<
int k_mpc; ///<
double maxHeight_; ///< Apex height of the swinging trajectory
@@ -93,4 +93,4 @@ private:
Matrix34 velocity_; // velocity computed in updateFootPosition
Matrix34 acceleration_; // acceleration computed in updateFootPosition
};
-#endif // PLANNER_H_INCLUDED
+#endif // TRAJGEN_H_INCLUDED
diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index 628a8985..1f925121 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -32,18 +32,18 @@ public:
///
/// \brief Default constructor
///
- /// \param[in] dt_in
- /// \param[in] T_mpc_in
- /// \param[in] h_ref_in
- /// \param[in] shoulderIn
+ /// \param[in] dt_in Time step of the contact sequence (time step of the MPC)
+ /// \param[in] T_mpc_in MPC period (prediction horizon)
+ /// \param[in] h_ref_in Reference height for the trunk
+ /// \param[in] shoulderIn Position of shoulders in local frame
+ /// \param[in] gaitIn Gait object to hold the gait informations
///
////////////////////////////////////////////////////////////////////////////////////////////////
void initialize(double dt_in,
- int k_mpc_in,
double T_mpc_in,
double h_ref_in,
MatrixN const& shouldersIn,
- Gait & gaitIn); // std::shared_ptr<Gait> gaitIn);
+ Gait & gaitIn);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -55,6 +55,11 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
///
+ /// \brief Compute the desired location of footsteps and update relevant matrices
+ ///
+ /// \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)
///
////////////////////////////////////////////////////////////////////////////////////////////////
MatrixN computeTargetFootstep(VectorN const& q,
@@ -63,22 +68,11 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////
///
+ /// \brief Refresh feet position when entering a new contact phase
///
////////////////////////////////////////////////////////////////////////////////////////////////
void updateNewContact();
- ////////////////////////////////////////////////////////////////////////////////////////////////
- ///
- /// \brief Update desired location of footsteps using information coming from the footsteps planner
- ///
- /// \param[in] k number of time steps since the start of the simulation
- /// \param[in] k_mpc number of wbc time steps for one time step of the MPC
- ///
- ////////////////////////////////////////////////////////////////////////////////////////////////
- /*void rollGait(int const k,
- int const k_mpc);*/
-
- // MatrixN getXReference();
MatrixN getFootsteps();
MatrixN getTargetFootsteps();
@@ -117,11 +111,10 @@ private:
MatrixN vectorToMatrix(std::array<Matrix34, N0_gait> const& array);
- Gait* gait_; // std::shared_ptr<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
- int k_mpc; // Number of TSID iterations for one iteration of the MPC
double T_mpc; // MPC period (prediction horizon)
double h_ref; // Reference height for the trunk
@@ -152,9 +145,6 @@ private:
Vector3 b_v;
Vector6 b_vref;
- // 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;
};
#endif // FOOTSTEPPLANNER_H_INCLUDED
diff --git a/include/qrw/StatePlanner.hpp b/include/qrw/StatePlanner.hpp
index 0ddd0f0b..fa321e20 100644
--- a/include/qrw/StatePlanner.hpp
+++ b/include/qrw/StatePlanner.hpp
@@ -58,7 +58,6 @@ public:
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
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 2e4580ec..d21ac974 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -41,51 +41,6 @@ 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>>
-{
- template <class PyClassPlanner>
- void visit(PyClassPlanner& cl) const
- {
- cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def(bp::init<double, double, double, double, int, double, const MatrixN&, const MatrixN&>(
- bp::args("dt_in", "dt_tsid_in", "T_gait_in", "T_mpc_in", "k_mpc_in", "h_ref_in",
- "fsteps_in", "shoulders positions"),
- "Constructor with parameters."))
-
- .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")
- .def("get_vgoals", &Planner::get_vgoals, "Get velocity goals matrix.\n")
- .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"),
- "Run Planner from Python.\n")
-
- // Update gait matrix from Python
- .def("updateGait", &Planner::updateGait, bp::args("k", "k_mpc", "q", "joystickCode"),
- "Update gait matrix from Python.\n")
-
- // Set gait matrix from Python
- .def("setGait", &Planner::setGait, bp::args("gaitMatrix"),
- "Set gait matrix from Python.\n");
- }
-
- static void expose()
- {
- bp::class_<Planner>("Planner", bp::no_init).def(PlannerPythonVisitor<Planner>());
-
- ENABLE_SPECIFIC_MATRIX_TYPE(MatrixN);
- }
-};
-void exposePlanner() { PlannerPythonVisitor<Planner>::expose(); }
-*/
-
/////////////////////////////////
/// Binding StatePlanner class
/////////////////////////////////
@@ -165,7 +120,7 @@ struct FootstepPlannerPythonVisitor : public bp::def_visitor<FootstepPlannerPyth
.def("getFootsteps", &FootstepPlanner::getFootsteps, "Get footsteps_ matrix.\n")
- .def("initialize", &FootstepPlanner::initialize, bp::args("dt_in", "k_mpc_in", "T_mpc_in", "h_ref_in", "shouldersIn", "gaitIn"),
+ .def("initialize", &FootstepPlanner::initialize, bp::args("dt_in", "T_mpc_in", "h_ref_in", "shouldersIn", "gaitIn"),
"Initialize FootstepPlanner from Python.\n")
// Compute target location of footsteps from Python
@@ -285,7 +240,6 @@ BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
eigenpy::enableEigenPy();
exposeMPC();
- // exposePlanner();
exposeStatePlanner();
exposeGait();
exposeFootstepPlanner();
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 48812315..d13419c6 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -7,7 +7,6 @@ import time
from QP_WBC import wbc_controller
import MPC_Wrapper
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
@@ -127,7 +126,7 @@ class Controller:
shoulders[0, :] = [0.1946, 0.1946, -0.1946, -0.1946]
shoulders[1, :] = [0.14695, -0.14695, 0.14695, -0.14695]
self.footstepPlanner = lqrw.FootstepPlanner()
- self.footstepPlanner.initialize(dt_mpc, k_mpc, T_mpc, self.h_ref, shoulders.copy(), self.gait)
+ self.footstepPlanner.initialize(dt_mpc, T_mpc, self.h_ref, shoulders.copy(), self.gait)
self.footTrajectoryGenerator = lqrw.FootTrajectoryGenerator()
self.footTrajectoryGenerator.initialize(0.05, 0.07, self.fsteps_init.copy(), shoulders.copy(), dt_wbc, k_mpc, self.gait)
@@ -227,15 +226,9 @@ class Controller:
# Update gait
self.gait.updateGait(self.k, self.k_mpc, self.q[0:7, 0:1], self.joystick.joystick_code)
- # Update gait
- # 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.q[0:7, 0:1], self.v[0:6, 0:1].copy(), self.joystick.v_ref)
-
- if(self.k % self.k_mpc == 0 and self.k != 0 and self.gait.isNewPhase()): # If new contact phase
- self.footstepPlanner.updateNewContact() # .getCurrentGait())
-
+ # Update footsteps if new contact phase
+ 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)
Eigen::Quaterniond quat(q(6, 0), q(3, 0), q(4, 0), q(5, 0)); // w, x, y, z
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index 09a977a9..ddfafcb3 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) // std::shared_ptr<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 81fa1f0a..4d8ea435 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -17,20 +17,17 @@ FootstepPlanner::FootstepPlanner()
, RPY(Vector3::Zero())
, b_v(Vector3::Zero())
, b_vref(Vector6::Zero())
- , xref()
{
// Empty
}
void FootstepPlanner::initialize(double dt_in,
- int k_mpc_in,
double T_mpc_in,
double h_ref_in,
MatrixN const& shouldersIn,
- Gait & gaitIn) // std::shared_ptr<Gait> gaitIn)
+ Gait & gaitIn)
{
dt = dt_in;
- k_mpc = k_mpc_in;
T_mpc = T_mpc_in;
h_ref = h_ref_in;
n_steps = (int)std::lround(T_mpc_in / dt_in);
@@ -40,7 +37,6 @@ void FootstepPlanner::initialize(double dt_in,
targetFootstep_ = shouldersIn;
footsteps_.fill(Matrix34::Zero());
Rz(2, 2) = 1.0;
- xref = MatrixN::Zero(12, 1 + n_steps);
}
void FootstepPlanner::compute_footsteps(VectorN const& q, Vector6 const& v, Vector6 const& vref)
@@ -209,13 +205,6 @@ void FootstepPlanner::updateNewContact() // Gait const& gait) // MaxtrixN const&
}
}
-/*void FootstepPlanner::rollGait(int const k,
- int const k_mpc)
-{
- if (k % k_mpc == 0)
- gait_->roll(k, footsteps_[1], currentFootstep_);
-}*/
-
MatrixN FootstepPlanner::getFootsteps() { return vectorToMatrix(footsteps_); }
MatrixN FootstepPlanner::getTargetFootsteps() { return targetFootstep_; }
diff --git a/src/StatePlanner.cpp b/src/StatePlanner.cpp
index 613deb3a..72b67032 100644
--- a/src/StatePlanner.cpp
+++ b/src/StatePlanner.cpp
@@ -2,7 +2,6 @@
StatePlanner::StatePlanner()
: dt_(0.0)
- , T_mpc_(0.0)
, h_ref_(0.0)
, n_steps_(0)
, RPY_(Vector3::Zero())
@@ -13,11 +12,10 @@ StatePlanner::StatePlanner()
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_);
+ 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)
--
GitLab