diff --git a/include/qrw/Filter.hpp b/include/qrw/Filter.hpp
index da37c38671709cc90b1bc5076dee93f2af5967e4..c87fb7983fb1a1f4be9854c276d8426db3524b4b 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 e093e3450fd6be39c5498cecb880147c692681fb..c4db4f8cc5f3343843ffa199c4bc153b7185b84d 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 8d0759aa9b77c5ad48a028d41d8fe9d0a9ba4449..c7373a87c846d8294689718a66f9d24657eae6c6 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 98418191dcd169eb8d8e34b4dc303cdb724c46a8..b31944f21b801546027374ca3a08028b94340643 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 065a58aab714777b774bf522f59d6fba8c10cd84..779b2d830e08f2dfe907f1d5fabf88828e329df6 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 f68f97487328eeb5b1d171d06ad2bbb41b484faa..0fcc40d6767ce8510b7ba525479fb51d8611145b 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 17b5db83ee378bfd3b5457e3755a2c045b237afc..7bab040f7ba82c3383a65fd73df3a630712af977 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 a77ef5aafca4e13efa38146d41118a8d3324aea5..6dba205d3b017f048cb963e0f2e9760f175d1b2f 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 bc7607f28164f4822d21f72e9c57043f434f1d3a..48b397b4bd187c5730e9646c8b612c727cd1dfa0 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 add6241061d672965c1b67aa62797900027030e9..405f89af1d4d5929db529557b73e53fe9a27509c 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