From 60ff897efe152159dc7f414433911adcf4ff7bd9 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Fri, 6 Aug 2021 17:57:11 +0200
Subject: [PATCH] Switch orientation part of q vector in main loop to roll
pitch yaw instead of quaternion
---
include/qrw/Filter.hpp | 3 ++-
include/qrw/FootstepPlanner.hpp | 4 ++--
python/gepadd.cpp | 2 +-
scripts/Controller.py | 16 ++++++++--------
scripts/LoggerControl.py | 2 +-
scripts/MPC_Wrapper.py | 3 +--
src/Estimator.cpp | 5 ++---
src/Filter.cpp | 17 ++++++-----------
src/FootstepPlanner.cpp | 20 ++++++++++++--------
src/StatePlanner.cpp | 7 +++++--
10 files changed, 40 insertions(+), 39 deletions(-)
diff --git a/include/qrw/Filter.hpp b/include/qrw/Filter.hpp
index da37c386..c87fb798 100644
--- a/include/qrw/Filter.hpp
+++ b/include/qrw/Filter.hpp
@@ -41,9 +41,10 @@ public:
/// \brief Run one iteration of the filter and return the filtered measurement
///
/// \param[in] x Quantity to filter
+ /// \param[in] check_modulo Check for the +-pi modulo of orientation if true
///
////////////////////////////////////////////////////////////////////////////////////////////////
- VectorN filter(VectorN const& x);
+ VectorN filter(Vector6 const& x, bool check_modulo);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index e093e345..c4db4f8c 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -83,7 +83,7 @@ private:
/// \param[in] b_vref desired velocity vector of the flying base in horizontal frame (linear and angular stacked)
///
////////////////////////////////////////////////////////////////////////////////////////////////
- MatrixN computeTargetFootstep(int k, Vector7 const& q, Vector6 const& b_v, Vector6 const& b_vref);
+ MatrixN computeTargetFootstep(int k, Vector6 const& q, Vector6 const& b_v, Vector6 const& b_vref);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -92,7 +92,7 @@ private:
/// \param[in] q current configuration vector
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void updateNewContact(Vector19 const& q);
+ void updateNewContact(Vector18 const& q);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 8d0759aa..c7373a87 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -59,7 +59,7 @@ struct FilterPythonVisitor : public bp::def_visitor<FilterPythonVisitor<Filter>>
"Initialize Filter from Python.\n")
// Run Filter from Python
- .def("filter", &Filter::filter, bp::args("x"), "Run Filter from Python.\n")
+ .def("filter", &Filter::filter, bp::args("x", "check_modulo"), "Run Filter from Python.\n")
.def("getFilt", &Filter::getFilt, "Get filtered quantity.\n");
}
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 98418191..b31944f2 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -116,9 +116,9 @@ class Controller:
self.enable_hybrid_control = True
self.h_ref = params.h_ref
- self.q = np.zeros((19, 1))
- self.q[0:7, 0] = np.array([0.0, 0.0, self.h_ref, 0.0, 0.0, 0.0, 1.0])
- self.q[7:, 0] = q_init
+ self.q = np.zeros((18, 1)) #Â Orientation part is in roll pitch yaw
+ self.q[0:6, 0] = np.array([0.0, 0.0, self.h_ref, 0.0, 0.0, 0.0])
+ self.q[6:, 0] = q_init
self.v = np.zeros((18, 1))
self.b_v = np.zeros((18, 1))
self.o_v_filt = np.zeros((18, 1))
@@ -248,10 +248,10 @@ class Controller:
self.gait.updateGait(self.k, self.k_mpc, self.joystick.joystick_code)
#Â Quantities go through a 1st order low pass filter with fc = 15 Hz
- self.q_filt_mpc[:, 0] = self.filter_mpc_q.filter(self.q[:7, 0:1])
- self.h_v_filt_mpc[:, 0] = self.filter_mpc_v.filter(self.h_v[:6, 0:1])
- self.h_v_bis_filt_mpc[:, 0] = self.filter_mpc_v_bis.filter(self.h_v_bis[:6, 0:1])
- self.vref_filt_mpc[:, 0] = self.filter_mpc_vref.filter(self.v_ref[:6, 0:1])
+ self.q_filt_mpc[:, 0] = self.filter_mpc_q.filter(self.q[:6, 0:1], True)
+ self.h_v_filt_mpc[:, 0] = self.filter_mpc_v.filter(self.h_v[:6, 0:1], False)
+ self.h_v_bis_filt_mpc[:, 0] = self.filter_mpc_v_bis.filter(self.h_v_bis[:6, 0:1], False)
+ self.vref_filt_mpc[:, 0] = self.filter_mpc_vref.filter(self.v_ref[:6, 0:1], False)
# Compute target footstep based on current and reference velocities
o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
@@ -261,7 +261,7 @@ class Controller:
self.v_ref[0:6, 0])
# Run state planner (outputs the reference trajectory of the base)
- self.statePlanner.computeReferenceStates(self.q[0:7, 0:1], self.h_v[0:6, 0:1].copy(),
+ self.statePlanner.computeReferenceStates(self.q[0:6, 0:1], self.h_v[0:6, 0:1].copy(),
self.v_ref[0:6, 0:1], 0.0)
# Result can be retrieved with self.statePlanner.getReferenceStates()
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 065a58aa..779b2d83 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -44,7 +44,7 @@ class LoggerControl():
self.esti_kf_Z = np.zeros([logSize, 16]) # measurement for the Kalman filter
# Loop
- self.loop_o_q_int = np.zeros([logSize, 19]) # position in world frame (esti_q_filt + dt * loop_o_v)
+ self.loop_o_q_int = np.zeros([logSize, 18]) # position in world frame (esti_q_filt + dt * loop_o_v)
self.loop_o_v = np.zeros([logSize, 18]) # estimated velocity in world frame
self.loop_h_v = np.zeros([logSize, 18]) # estimated velocity in horizontal frame
self.loop_h_v_bis = np.zeros([logSize, 6]) # estimated velocity in horizontal frame
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index f68f9748..0fcc40d6 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -76,8 +76,7 @@ class MPC_Wrapper:
# Setup initial result for the first iteration of the main control loop
x_init = np.zeros(12)
- x_init[0:3] = q_init[0:3, 0]
- x_init[3:6] = pin.rpy.matrixToRpy(pin.Quaternion(q_init[3:7, 0]).toRotationMatrix())
+ x_init[0:6] = q_init[0:6, 0].copy()
if self.mpc_type == 3: # Need more space to store optimized footsteps
self.last_available_result = np.zeros((32, (np.int(self.n_steps))))
else:
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 17b5db83..7bab040f 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -75,7 +75,7 @@ Estimator::Estimator()
, q_filt_dyn_(VectorN::Zero(19, 1))
, v_filt_dyn_(VectorN::Zero(18, 1))
, v_secu_dyn_(VectorN::Zero(12, 1))
- , q_up_(VectorN::Zero(19))
+ , q_up_(VectorN::Zero(18))
, v_ref_(VectorN::Zero(6))
, h_v_(VectorN::Zero(6))
, oRh_(Matrix3::Identity())
@@ -123,7 +123,6 @@ void Estimator::initialize(Params& params)
q_filt_dyn_(6, 0) = 1.0; // Last term of the quaternion
q_up_(2, 0) = params.h_ref; // Reference height
- q_up_(6, 0) = 1.0; // Last term of the quaternion
q_up_.tail(12) = Vector12(params.q_init.data()); // Actuator initial positions
// Path to the robot URDF (TODO: Automatic path)
@@ -448,7 +447,7 @@ void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait)
// Mix perfect yaw with pitch and roll measurements
yaw_estim_ += v_ref_[5] * dt_wbc;
- q_up_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(IMU_RPY_[0], IMU_RPY_[1], yaw_estim_)).coeffs();
+ q_up_.block(3, 0, 3, 1) << IMU_RPY_[0], IMU_RPY_[1], yaw_estim_;
// Actuators measurements
q_up_.tail(12) = q_filt_dyn_.tail(12);
diff --git a/src/Filter.cpp b/src/Filter.cpp
index a77ef5aa..6dba205d 100644
--- a/src/Filter.cpp
+++ b/src/Filter.cpp
@@ -23,15 +23,14 @@ void Filter::initialize(Params& params)
y_queue_.resize(a_.rows()-1, Vector6::Zero());
}
-VectorN Filter::filter(VectorN const& x)
+VectorN Filter::filter(Vector6 const& x, bool check_modulo)
{
- // If x is position + quaternion then we convert quaternion to RPY
- if (x.rows() == 7)
- {
- x_.head(3) = x.head(3);
- Eigen::Quaterniond quat(x(6, 0), x(3, 0), x(4, 0), x(5, 0)); // w, x, y, z
- x_.tail(3) = pinocchio::rpy::matrixToRpy(quat.toRotationMatrix());
+ // Retrieve measurement
+ x_ = x;
+ // Handle modulo for orientation
+ if (check_modulo)
+ {
// Handle 2 pi modulo for roll, pitch and yaw
// Should happen sometimes for yaw but now for roll and pitch except
// if the robot rolls over
@@ -43,10 +42,6 @@ VectorN Filter::filter(VectorN const& x)
}
}
}
- else // Otherwise we can directly use x
- {
- x_ = x;
- }
// Initialisation of the value in the queues to the first measurement
if (!init_)
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index bc7607f2..48b397b4 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -66,6 +66,11 @@ void FootstepPlanner::initialize(Params& params, Gait& gaitIn)
MatrixN FootstepPlanner::updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref)
{
+ if (q.rows() != 18)
+ {
+ throw std::runtime_error("q should be a vector of size 18 (pos+RPY+mot)");
+ }
+
// Update location of feet in stance phase (for those which just entered stance phase)
if (refresh && gait_->isNewPhase())
{
@@ -87,7 +92,7 @@ MatrixN FootstepPlanner::updateFootsteps(bool refresh, int k, VectorN const& q,
}
// Compute location of footsteps
- return computeTargetFootstep(k, q.head(7), b_v, b_vref);
+ return computeTargetFootstep(k, q.head(6), b_v, b_vref);
}
void FootstepPlanner::computeFootsteps(int k, Vector6 const& b_v, Vector6 const& b_vref)
@@ -214,7 +219,7 @@ void FootstepPlanner::updateTargetFootsteps()
}
}
-MatrixN FootstepPlanner::computeTargetFootstep(int k, Vector7 const& q, Vector6 const& b_v, Vector6 const& b_vref)
+MatrixN FootstepPlanner::computeTargetFootstep(int k, Vector6 const& q, Vector6 const& b_v, Vector6 const& b_vref)
{
// Compute the desired location of footsteps over the prediction horizon
computeFootsteps(k, b_v, b_vref);
@@ -223,8 +228,7 @@ MatrixN FootstepPlanner::computeTargetFootstep(int k, Vector7 const& q, Vector6
updateTargetFootsteps();
// Get o_targetFootstep_ in world frame from targetFootstep_ in horizontal frame
- quat_ = {q(6), q(3), q(4), q(5)}; // w, x, y, z
- RPY_ << pinocchio::rpy::matrixToRpy(quat_.toRotationMatrix());
+ RPY_ = q.tail(3);
double c = std::cos(RPY_(2));
double s = std::sin(RPY_(2));
Rz.topLeftCorner<2, 2>() << c, -s, s, c;
@@ -236,13 +240,13 @@ MatrixN FootstepPlanner::computeTargetFootstep(int k, Vector7 const& q, Vector6
return o_targetFootstep_;
}
-void FootstepPlanner::updateNewContact(Vector19 const& q)
+void FootstepPlanner::updateNewContact(Vector18 const& q)
{
// Remove translation and yaw rotation to get position in local frame
- q_FK_ = q;
q_FK_.head(2) = Vector2::Zero();
- Vector3 RPY = pinocchio::rpy::matrixToRpy(pinocchio::SE3::Quaternion(q_FK_(6), q_FK_(3), q_FK_(4), q_FK_(5)).toRotationMatrix());
- q_FK_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(RPY(0, 0), RPY(1, 0), 0.0)).coeffs();
+ q_FK_(2, 0) = q(2, 0);
+ q_FK_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q(3, 0), q(4, 0), 0.0)).coeffs();
+ q_FK_.tail(12) = q.tail(12);
// Update model and data of the robot
pinocchio::forwardKinematics(model_, data_, q_FK_);
diff --git a/src/StatePlanner.cpp b/src/StatePlanner.cpp
index add62410..405f89af 100644
--- a/src/StatePlanner.cpp
+++ b/src/StatePlanner.cpp
@@ -20,8 +20,11 @@ void StatePlanner::initialize(Params& params)
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());
+ if (q.rows() != 6)
+ {
+ throw std::runtime_error("q should be a vector of size 6");
+ }
+ RPY_ = q.tail(3);
// Update the current state
referenceStates_(0, 0) = 0.0; // In horizontal frame x = 0.0
--
GitLab