From ee334b055a11adcdf3e78e427c2bf1de3144506c Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Tue, 13 Apr 2021 17:02:24 +0200
Subject: [PATCH] New StatePlanner class independent from Planner to generate
the CoM trajectory
---
CMakeLists.txt | 2 +
include/qrw/FootstepPlanner.hpp | 21 +-------
include/qrw/Planner.hpp | 5 +-
include/qrw/StatePlanner.hpp | 75 ++++++++++++++++++++++++++
python/gepadd.cpp | 41 ++++++++++++--
scripts/Controller.py | 15 ++++--
scripts/Planner.py | 5 +-
src/FootstepPlanner.cpp | 84 +----------------------------
src/Planner.cpp | 7 ++-
src/StatePlanner.cpp | 96 +++++++++++++++++++++++++++++++++
10 files changed, 232 insertions(+), 119 deletions(-)
create mode 100644 include/qrw/StatePlanner.hpp
create mode 100644 src/StatePlanner.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8c70d0cf..76f43b84 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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
diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index e0c6babe..bafb666f 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -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
diff --git a/include/qrw/Planner.hpp b/include/qrw/Planner.hpp
index 2d634b7c..8ed8f312 100644
--- a/include/qrw/Planner.hpp
+++ b/include/qrw/Planner.hpp
@@ -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();
diff --git a/include/qrw/StatePlanner.hpp b/include/qrw/StatePlanner.hpp
new file mode 100644
index 00000000..0ddd0f0b
--- /dev/null
+++ b/include/qrw/StatePlanner.hpp
@@ -0,0 +1,75 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \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
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 25c3baad..475d69c2 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -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
diff --git a/scripts/Controller.py b/scripts/Controller.py
index cab2d170..3efb0aee 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -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:
diff --git a/scripts/Planner.py b/scripts/Planner.py
index d93e2dd5..10e3240e 100644
--- a/scripts/Planner.py
+++ b/scripts/Planner.py
@@ -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()
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index ecfeafec..71ae61d5 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -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_; }
diff --git a/src/Planner.cpp b/src/Planner.cpp
index 32e36be1..3d75bb15 100644
--- a/src/Planner.cpp
+++ b/src/Planner.cpp
@@ -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(); }
diff --git a/src/StatePlanner.cpp b/src/StatePlanner.cpp
new file mode 100644
index 00000000..613deb3a
--- /dev/null
+++ b/src/StatePlanner.cpp
@@ -0,0 +1,96 @@
+#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_;
+ }
+ }*/
+
+}
--
GitLab