From 6cef08d8b8d0d8788cadb1a1373a22dbaeea6455 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Wed, 18 Aug 2021 11:57:46 +0200
Subject: [PATCH] Add oRb matrix in estimator and expose it with bindings
---
include/qrw/Estimator.hpp | 2 ++
python/gepadd.cpp | 1 +
scripts/Controller.py | 4 +++-
scripts/LoggerControl.py | 3 ++-
src/Estimator.cpp | 4 ++++
5 files changed, 12 insertions(+), 2 deletions(-)
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 475abfb1..4ea154c0 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -213,6 +213,7 @@ class Estimator {
VectorN getVRef() { return v_ref_; }
VectorN getHV() { return h_v_; }
VectorN getHVWindowed() { return h_v_windowed_; }
+ Matrix3 getoRb() { return oRb_; }
Matrix3 getoRh() { return oRh_; }
Vector3 getoTh() { return oTh_; }
double getYawEstim() { return yaw_estim_; }
@@ -232,6 +233,7 @@ class Estimator {
Vector3 IMU_lin_acc_; // Linear acceleration of the IMU (gravity compensated)
Vector3 IMU_ang_vel_; // Angular velocity of the IMU
Vector3 IMU_RPY_; // Roll Pitch Yaw orientation of the IMU
+ Matrix3 oRb_; // Rotation between base and world frame
pinocchio::SE3::Quaternion IMU_ang_pos_; // Quaternion orientation of the IMU
Vector12 actuators_pos_; // Measured positions of actuators
Vector12 actuators_vel_; // Measured velocities of actuators
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index fa1128ab..8e129d5f 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -354,6 +354,7 @@ struct EstimatorPythonVisitor : public bp::def_visitor<EstimatorPythonVisitor<Es
.def("getVRef", &Estimator::getVRef, "")
.def("getHV", &Estimator::getHV, "")
.def("getHVWindowed", &Estimator::getHVWindowed, "")
+ .def("getoRb", &Estimator::getoRb, "")
.def("getoRh", &Estimator::getoRh, "")
.def("getoTh", &Estimator::getoTh, "")
.def("getYawEstim", &Estimator::getYawEstim, "")
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 2adca9b7..a8090571 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -219,6 +219,7 @@ class Controller:
# Update state vectors of the robot (q and v) + transformation matrices between world and horizontal frames
self.estimator.updateState(self.joystick.v_ref, self.gait)
+ oRb = self.estimator.getoRb()
oRh = self.estimator.getoRh()
oTh = self.estimator.getoTh().reshape((3, 1))
self.v_ref[0:6, 0] = self.estimator.getVRef()
@@ -258,7 +259,8 @@ class Controller:
t_planner = time.time()
- print("iteration : " , self.k) # print iteration
+ if self.k % 250 == 0:
+ print("iteration : " , self.k) # print iteration
# TODO: Add 25Hz filter for the inputs of the MPC
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 7453c2f4..7a521563 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -344,6 +344,7 @@ class LoggerControl():
plt.plot(t_range, self.joy_v_ref[:, i], "r", linewidth=3)
if i < 3:
plt.plot(t_range, self.mocap_h_v[:, i], "k", linewidth=3)
+ plt.plot(t_range, self.loop_h_v_filt_mpc[:, i], linewidth=3, color="forestgreen")
else:
plt.plot(t_range, self.mocap_b_w[:, i-3], "k", linewidth=3)
@@ -354,7 +355,7 @@ class LoggerControl():
# plt.plot(t_range, self.log_dq[i, :], "g", linewidth=2)
# plt.plot(t_range[:-2], self.log_dx_invkin[i, :-2], "g", linewidth=2)
# plt.plot(t_range[:-2], self.log_dx_ref_invkin[i, :-2], "violet", linewidth=2, linestyle="--")
- plt.legend(["Robot state", "Robot reference state", "Ground truth", "Alternative"], prop={'size': 8})
+ plt.legend(["Robot state", "Robot reference state", "Ground truth", "Robot state (LP 15Hz)"], prop={'size': 8})
plt.ylabel(lgd[i])
plt.suptitle("Measured & Reference linear and angular velocities")
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 138eadb1..1a2623b5 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -51,6 +51,7 @@ Estimator::Estimator()
IMU_lin_acc_(Vector3::Zero()),
IMU_ang_vel_(Vector3::Zero()),
IMU_RPY_(Vector3::Zero()),
+ oRb_(Matrix3::Identity()),
IMU_ang_pos_(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0)),
actuators_pos_(Vector12::Zero()),
actuators_vel_(Vector12::Zero()),
@@ -432,6 +433,9 @@ void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait) {
yaw_estim_ += v_ref_[5] * dt_wbc;
q_up_.block(3, 0, 3, 1) << IMU_RPY_[0], IMU_RPY_[1], yaw_estim_;
+ // Transformation matrices between world and base frames
+ oRb_ = pinocchio::rpy::rpyToMatrix(IMU_RPY_(0, 0), IMU_RPY_(1, 0), yaw_estim_);
+
// Actuators measurements
q_up_.tail(12) = q_filt_dyn_.tail(12);
--
GitLab