From f688142a9128e42a8577730a249095ae7e0688ae Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Thu, 19 Aug 2021 14:43:50 +0200
Subject: [PATCH] Working on Logging (adding, removing, cleaning)
---
include/qrw/Estimator.hpp | 7 ++--
include/qrw/QPWBC.hpp | 2 ++
python/gepadd.cpp | 3 ++
scripts/Controller.py | 63 +++-----------------------------
scripts/LoggerControl.py | 65 ++++++++++++++++------------------
scripts/main_solo12_control.py | 2 +-
src/Estimator.cpp | 35 ++++++++----------
7 files changed, 60 insertions(+), 117 deletions(-)
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 4ea154c0..bd7daa67 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -164,7 +164,7 @@ class Estimator {
////////////////////////////////////////////////////////////////////////////////////////////////
void run_filter(MatrixN const& gait, MatrixN const& goals, VectorN const& baseLinearAcceleration,
VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q_mes,
- VectorN const& v_mes, VectorN const& dummyPos, VectorN const& b_baseVel);
+ VectorN const& v_mes, VectorN const& dummyPos, Vector3 const& b_baseVel);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -210,6 +210,7 @@ class Estimator {
Vector3 getFilterPosFiltX() { return filter_xyz_pos_.getFiltX(); }
VectorN getQUpdated() { return q_up_; }
+ VectorN getVUpdated() { return v_up_; }
VectorN getVRef() { return v_ref_; }
VectorN getHV() { return h_v_; }
VectorN getHVWindowed() { return h_v_windowed_; }
@@ -223,7 +224,6 @@ class Estimator {
ComplementaryFilter filter_xyz_vel_; // Complementary filter for base velocity
double dt_wbc; // Time step of the estimator
- double alpha_v_; // Low pass coefficient for the outputted filtered velocity
double alpha_secu_; // Low pass coefficient for the outputted filtered velocity for security check
double offset_yaw_IMU_; // Yaw orientation of the IMU at startup
bool perfect_estimator; // Enable perfect estimator (directly from the PyBullet simulation)
@@ -267,7 +267,8 @@ class Estimator {
Vector12 q_security_; // Position limits for the actuators above which safety controller is triggered
// For updateState function
- VectorN q_up_; // Configuration vector
+ VectorN q_up_; // Configuration vector in ideal world frame
+ VectorN v_up_; // Velocity vector in ideal world frame
VectorN v_ref_; // Reference velocity vector
VectorN h_v_; // Velocity vector in horizontal frame
Matrix3 oRh_; // Rotation between horizontal and world frame
diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index b3296c36..e1189929 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -292,7 +292,9 @@ class WbcWrapper {
VectorN get_qdes() { return qdes_; }
VectorN get_vdes() { return vdes_; }
VectorN get_tau_ff() { return tau_ff_; }
+ VectorN get_ddq_cmd() { return ddq_cmd_; }
VectorN get_f_with_delta() { return f_with_delta_; }
+ VectorN get_ddq_with_delta() { return ddq_with_delta_; }
MatrixN get_feet_pos() { return invkin_->get_posf().transpose(); }
MatrixN get_feet_err() { return log_feet_pos_target - invkin_->get_posf().transpose(); }
MatrixN get_feet_vel() { return invkin_->get_vf().transpose(); }
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 9c5f633b..85f81109 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -297,7 +297,9 @@ struct WbcWrapperPythonVisitor : public bp::def_visitor<WbcWrapperPythonVisitor<
.def_readonly("qdes", &WbcWrapper::get_qdes)
.def_readonly("vdes", &WbcWrapper::get_vdes)
.def_readonly("tau_ff", &WbcWrapper::get_tau_ff)
+ .def_readonly("ddq_cmd", &WbcWrapper::get_ddq_cmd)
.def_readonly("f_with_delta", &WbcWrapper::get_f_with_delta)
+ .def_readonly("ddq_with_delta", &WbcWrapper::get_ddq_with_delta)
.def_readonly("feet_pos", &WbcWrapper::get_feet_pos)
.def_readonly("feet_err", &WbcWrapper::get_feet_err)
.def_readonly("feet_vel", &WbcWrapper::get_feet_vel)
@@ -353,6 +355,7 @@ struct EstimatorPythonVisitor : public bp::def_visitor<EstimatorPythonVisitor<Es
.def("getFilterPosAlpha", &Estimator::getFilterPosAlpha, "")
.def("getFilterPosFiltX", &Estimator::getFilterPosFiltX, "")
.def("getQUpdated", &Estimator::getQUpdated, "")
+ .def("getVUpdated", &Estimator::getVUpdated, "")
.def("getVRef", &Estimator::getVRef, "")
.def("getHV", &Estimator::getHV, "")
.def("getHVWindowed", &Estimator::getHVWindowed, "")
diff --git a/scripts/Controller.py b/scripts/Controller.py
index d8044358..5b62c584 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -189,8 +189,6 @@ class Controller:
# Interface with the PD+ on the control board
self.result = Result()
- test_footTrajectoryGenerator(params)
-
# Run the control loop once with a dummy device for initialization
dDevice = dummyDevice()
dDevice.joints.positions = q_init
@@ -228,6 +226,7 @@ class Controller:
self.h_v[0:6, 0] = self.estimator.getHV()
self.h_v_windowed[0:6, 0] = self.estimator.getHVWindowed()
self.q[:, 0] = self.estimator.getQUpdated()
+ self.v[:, 0] = self.estimator.getVUpdated()
self.yaw_estim = self.estimator.getYawEstim()
# TODO: Understand why using Python or C++ h_v leads to a slightly different result since the
# difference between them at each time step is 1e-16 at max (butterfly effect?)
@@ -330,7 +329,8 @@ class Controller:
self.result.D = self.Kd_main * np.ones(12)
self.result.q_des[:] = self.wbcWrapper.qdes[:]
self.result.v_des[:] = self.wbcWrapper.vdes[:]
- self.result.tau_ff[:] = self.Kff_main * self.wbcWrapper.tau_ff
+ self.result.FF = self.Kff_main * np.ones(12)
+ self.result.tau_ff[:] = self.wbcWrapper.tau_ff
# Display robot in Gepetto corba viewer
if self.enable_corba_viewer and (self.k % 5 == 0):
@@ -385,6 +385,7 @@ class Controller:
self.result.D = 0.1 * np.ones(12)
self.result.q_des[:] = np.zeros(12)
self.result.v_des[:] = np.zeros(12)
+ self.result.FF = np.zeros(12)
self.result.tau_ff[:] = np.zeros(12)
def log_misc(self, tic, t_filter, t_planner, t_mpc, t_wbc):
@@ -394,59 +395,3 @@ class Controller:
self.t_mpc = t_mpc - t_planner
self.t_wbc = t_wbc - t_mpc
self.t_loop = time.time() - tic
-
-def test_footTrajectoryGenerator(params):
-
- gait = lqrw.Gait()
- gait.initialize(params)
-
- ftg = lqrw.FootTrajectoryGenerator()
- ftg.initialize(params, gait)
-
- o_targetFootstep = np.zeros((3, 4))
- o_targetFootstep = 2.0 * ftg.getFootPosition()
- k = 0
- N = 1000
- log_p_ref = np.zeros((N, 3, 4))
- log_p_target = np.zeros((N, 3, 4))
- log_p = np.zeros((N, 3, 4))
- log_v = np.zeros((N, 3, 4))
- log_a = np.zeros((N, 3, 4))
- while k < N:
-
- # Update reference
- o_targetFootstep[0, :] -= 0.0001
-
- # Update gait
- gait.updateGait(k, 20, 0)
-
- # Update foot trajectory generator
- ftg.update(k, o_targetFootstep)
- log_p_ref[k] = o_targetFootstep
- log_p_target[k] = ftg.getTargetPosition()
- log_p[k] = ftg.getFootPosition()
- log_v[k] = ftg.getFootVelocity()
- log_a[k] = ftg.getFootAcceleration()
-
- k += 1
-
- from matplotlib import pyplot as plt
-
- index12 = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
- lgd_X = ["FL", "FR", "HL", "HR"]
- lgd_Y = ["Pos X", "Pos Y", "Pos Z"]
- plt.figure()
- for i in range(12):
- if i == 0:
- ax0 = plt.subplot(3, 4, index12[i])
- else:
- plt.subplot(3, 4, index12[i], sharex=ax0)
- plt.plot(log_p_ref[:, i % 3, np.int(i/3)], color='r', linewidth=3, marker='')
- plt.plot(log_p_target[:, i % 3, np.int(i/3)], color='forestgreen', linewidth=3, marker='')
- plt.plot(log_p[:, i % 3, np.int(i/3)], color='b', linewidth=3, marker='')
- plt.legend([lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)]+" Ref"], prop={'size': 8})
- plt.suptitle("")
-
- plt.show(block=True)
-
- print("END TEST")
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index ff467175..59bdc9b6 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -41,11 +41,10 @@ class LoggerControl():
self.esti_LP_filt_x = np.zeros([logSize, 3]) # filtered output of the position complementary filter
# Loop
- self.loop_o_q_int = np.zeros([logSize, 18]) # position in world frame (esti_q_filt + dt * loop_o_v)
+ self.loop_o_q = np.zeros([logSize, 18]) # position in ideal 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_windowed = np.zeros([logSize, 6]) # estimated velocity in horizontal frame
- self.loop_pos_virtual_world = np.zeros([logSize, 3]) # x, y, yaw perfect position in world
self.loop_t_filter = np.zeros([logSize]) # time taken by the estimator
self.loop_t_planner = np.zeros([logSize]) #Â time taken by the planning
self.loop_t_mpc = np.zeros([logSize]) # time taken by the mcp
@@ -59,8 +58,6 @@ class LoggerControl():
# Gait
self.planner_gait = np.zeros([logSize, N0_gait, 4]) # Gait sequence
self.planner_is_static = np.zeros([logSize]) # if the planner is in static mode or not
- self.planner_q_static = np.zeros([logSize, 19]) # position in static mode (4 stance phase)
- self.planner_RPY_static = np.zeros([logSize, 3]) # RPY orientation in static mode (4 stance phase)
# State planner
if statePlanner is not None:
@@ -90,8 +87,11 @@ class LoggerControl():
self.wbc_D = np.zeros([logSize, 12]) # derivative gains of the PD+
self.wbc_q_des = np.zeros([logSize, 12]) # desired position of actuators
self.wbc_v_des = np.zeros([logSize, 12]) # desired velocity of actuators
+ self.wbc_FF = np.zeros([logSize, 12]) # gains for the feedforward torques
self.wbc_tau_ff = np.zeros([logSize, 12]) # feedforward torques computed by the WBC
+ self.wbc_ddq_IK = np.zeros([logSize, 18]) # joint accelerations computed by the IK
self.wbc_f_ctc = np.zeros([logSize, 12]) # contact forces computed by the WBC
+ self.wbc_ddq_QP = np.zeros([logSize, 18]) # joint accelerations computed by the QP
self.wbc_feet_pos = np.zeros([logSize, 3, 4]) # current feet positions according to WBC
self.wbc_feet_pos_target = np.zeros([logSize, 3, 4]) # current feet positions targets for WBC
self.wbc_feet_err = np.zeros([logSize, 3, 4]) # error between feet positions and their reference
@@ -135,11 +135,10 @@ class LoggerControl():
self.esti_LP_filt_x[self.i] = estimator.getFilterPosFiltX()
# Logging from the main loop
- self.loop_o_q_int[self.i] = loop.q[:, 0]
+ self.loop_o_q[self.i] = loop.q[:, 0]
self.loop_o_v[self.i] = loop.v[:, 0]
self.loop_h_v[self.i] = loop.h_v[:, 0]
self.loop_h_v_windowed[self.i] = loop.h_v_windowed[:, 0]
- self.loop_pos_virtual_world[self.i] = np.array([loop.q[0, 0], loop.q[1, 0], loop.yaw_estim])
self.loop_t_filter[self.i] = loop.t_filter
self.loop_t_planner[self.i] = loop.t_planner
self.loop_t_mpc[self.i] = loop.t_mpc
@@ -151,8 +150,6 @@ class LoggerControl():
self.loop_vref_filt_mpc[self.i] = loop.vref_filt_mpc[:, 0]
# Logging from the planner
- # self.planner_q_static[self.i] = planner.q_static[:]
- # self.planner_RPY_static[self.i] = planner.RPY_static[:, 0]
self.planner_xref[self.i] = statePlanner.getReferenceStates()
self.planner_fsteps[self.i] = footstepPlanner.getFootsteps()
self.planner_gait[self.i] = gait.getCurrentGait()
@@ -171,8 +168,11 @@ class LoggerControl():
self.wbc_D[self.i] = loop.result.D
self.wbc_q_des[self.i] = loop.result.q_des
self.wbc_v_des[self.i] = loop.result.v_des
+ self.wbc_FF[self.i] = loop.result.FF
self.wbc_tau_ff[self.i] = loop.result.tau_ff
+ self.wbc_ddq_IK[self.i] = wbc.ddq_cmd
self.wbc_f_ctc[self.i] = wbc.f_with_delta
+ self.wbc_ddq_QP[self.i] = wbc.ddq_with_delta
self.wbc_feet_pos[self.i] = wbc.feet_pos
self.wbc_feet_pos_target[self.i] = wbc.feet_pos_target
self.wbc_feet_err[self.i] = wbc.feet_err
@@ -245,7 +245,7 @@ class LoggerControl():
pin.computeAllTerms(solo12.model, solo12.data, q, np.zeros((12, 1)))
feet_pos = np.zeros([self.esti_q_filt.shape[0], 3, 4])
for i in range(self.esti_q_filt.shape[0]):
- q[:, 0] = self.loop_o_q_int[i, 6:]
+ q[:, 0] = self.loop_o_q[i, 6:]
pin.forwardKinematics(solo12.model, solo12.data, q)
pin.updateFramePlacements(solo12.model, solo12.data)
for j, idx in enumerate(foot_ids):
@@ -308,12 +308,9 @@ class LoggerControl():
ax0 = plt.subplot(3, 2, index6[i])
else:
plt.subplot(3, 2, index6[i], sharex=ax0)
- if i in [0, 1]:
- plt.plot(t_range, self.loop_pos_virtual_world[:, i], "b", linewidth=3)
- plt.plot(t_range, self.loop_pos_virtual_world[:, i], "r", linewidth=3)
- elif i == 5:
- plt.plot(t_range, self.loop_pos_virtual_world[:, 2], "b", linewidth=3)
- plt.plot(t_range, self.loop_pos_virtual_world[:, 2], "r", linewidth=3)
+ if i in [0, 1, 5]:
+ plt.plot(t_range, self.loop_o_q[:, i], "b", linewidth=3)
+ plt.plot(t_range, self.loop_o_q[:, i], "r", linewidth=3)
else:
plt.plot(t_range, self.planner_xref[:, i, 0], "b", linewidth=2)
plt.plot(t_range, self.planner_xref[:, i, 1], "r", linewidth=3)
@@ -377,18 +374,18 @@ class LoggerControl():
steps = np.zeros((12, 1))
o_step = np.zeros((3, 1))
plt.figure()
- plt.plot(self.loop_o_q_int[:, 0], self.loop_o_q_int[:, 1], linewidth=2, color="k")
+ plt.plot(self.loop_o_q[:, 0], self.loop_o_q[:, 1], linewidth=2, color="k")
for i in range(self.planner_fsteps.shape[0]):
fsteps = self.planner_fsteps[i]
- RPY = utils_mpc.quaternionToRPY(self.loop_o_q_int[i, 3:7])
+ RPY = utils_mpc.quaternionToRPY(self.loop_o_q[i, 3:7])
quat[:, 0] = utils_mpc.EulerToQuaternion([0.0, 0.0, RPY[2]])
oRh = pin.Quaternion(quat).toRotationMatrix()
for j in range(4):
#if np.any(fsteps[k, (j*3):((j+1)*3)]) and not np.array_equal(steps[(j*3):((j+1)*3), 0],
# fsteps[k, (j*3):((j+1)*3)]):
# steps[(j*3):((j+1)*3), 0] = fsteps[k, (j*3):((j+1)*3)]
- # o_step[:, 0:1] = oRh @ steps[(j*3):((j+1)*3), 0:1] + self.loop_o_q_int[i:(i+1), 0:3].transpose()
- o_step[:, 0:1] = oRh @ fsteps[0:1, (j*3):((j+1)*3)].transpose() + self.loop_o_q_int[i:(i+1), 0:3].transpose()
+ # o_step[:, 0:1] = oRh @ steps[(j*3):((j+1)*3), 0:1] + self.loop_o_q[i:(i+1), 0:3].transpose()
+ o_step[:, 0:1] = oRh @ fsteps[0:1, (j*3):((j+1)*3)].transpose() + self.loop_o_q[i:(i+1), 0:3].transpose()
plt.plot(o_step[0, 0], o_step[1, 0], linestyle=None, linewidth=1, marker="o", color=f_c[j])
"""
@@ -402,9 +399,9 @@ class LoggerControl():
plt.subplot(3, 4, index12[i], sharex=ax0)
tau_fb = self.wbc_P[:, i] * (self.wbc_q_des[:, i] - self.esti_q_filt[:, 7+i]) + \
self.wbc_D[:, i] * (self.wbc_v_des[:, i] - self.esti_v_filt[:, 6+i])
- h1, = plt.plot(t_range, self.wbc_tau_ff[:, i], "r", linewidth=3)
+ h1, = plt.plot(t_range, self.wbc_FF[:, i] * self.wbc_tau_ff[:, i], "r", linewidth=3)
h2, = plt.plot(t_range, tau_fb, "b", linewidth=3)
- h3, = plt.plot(t_range, self.wbc_tau_ff[:, i] + tau_fb, "g", linewidth=3)
+ h3, = plt.plot(t_range, self.wbc_FF[:, i] * self.wbc_tau_ff[:, i] + tau_fb, "g", linewidth=3)
h4, = plt.plot(t_range[:-1], loggerSensors.torquesFromCurrentMeasurment[1:, i],
"violet", linewidth=3, linestyle="--")
plt.xlabel("Time [s]")
@@ -626,7 +623,7 @@ class LoggerControl():
else:
plt.subplot(3, 2, index6[i], sharex=ax0)
- plt.plot(t_range, self.loop_o_q_int[:, i], linewidth=3)
+ plt.plot(t_range, self.loop_o_q[:, i], linewidth=3)
plt.plot(t_range, self.loop_q_filt_mpc[:, i], linewidth=3)
plt.legend(["Estimated", "Estimated 15Hz filt"], prop={'size': 8})
@@ -687,10 +684,9 @@ class LoggerControl():
esti_LP_alpha=self.esti_LP_alpha,
esti_LP_filt_x=self.esti_LP_filt_x,
- loop_o_q_int=self.loop_o_q_int,
+ loop_o_q=self.loop_o_q,
loop_o_v=self.loop_o_v,
loop_h_v=self.loop_h_v,
- loop_pos_virtual_world=self.loop_pos_virtual_world,
loop_t_filter=self.loop_t_filter,
loop_t_planner=self.loop_t_planner,
loop_t_mpc=self.loop_t_mpc,
@@ -701,8 +697,6 @@ class LoggerControl():
loop_h_v_filt_mpc=self.loop_h_v_filt_mpc,
loop_vref_filt_mpc=self.loop_vref_filt_mpc,
- planner_q_static=self.planner_q_static,
- planner_RPY_static=self.planner_RPY_static,
planner_xref=self.planner_xref,
planner_fsteps=self.planner_fsteps,
planner_gait=self.planner_gait,
@@ -719,8 +713,11 @@ class LoggerControl():
wbc_D=self.wbc_D,
wbc_q_des=self.wbc_q_des,
wbc_v_des=self.wbc_v_des,
+ wbc_FF=self.wbc_FF,
wbc_tau_ff=self.wbc_tau_ff,
+ wbc_ddq_IK=self.wbc_ddq_IK,
wbc_f_ctc=self.wbc_f_ctc,
+ wbc_ddq_QP=self.wbc_ddq_QP,
wbc_feet_pos=self.wbc_feet_pos,
wbc_feet_pos_target=self.wbc_feet_pos_target,
wbc_feet_err=self.wbc_feet_err,
@@ -778,10 +775,9 @@ class LoggerControl():
self.esti_LP_alpha = data["esti_LP_alpha"]
self.esti_LP_filt_x = data["esti_LP_filt_x"]
- self.loop_o_q_int = data["loop_o_q_int"]
+ self.loop_o_q = data["loop_o_q"]
self.loop_o_v = data["loop_o_v"]
self.loop_h_v = data["loop_h_v"]
- self.loop_pos_virtual_world = data["loop_pos_virtual_world"]
self.loop_t_filter = data["loop_t_filter"]
self.loop_t_planner = data["loop_t_planner"]
self.loop_t_mpc = data["loop_t_mpc"]
@@ -792,8 +788,6 @@ class LoggerControl():
self.loop_h_v_filt_mpc = data["loop_h_v_filt_mpc"]
self.loop_vref_filt_mpc = data["loop_vref_filt_mpc"]
- self.planner_q_static = data["planner_q_static"]
- self.planner_RPY_static = data["planner_RPY_static"]
self.planner_xref = data["planner_xref"]
self.planner_fsteps = data["planner_fsteps"]
self.planner_gait = data["planner_gait"]
@@ -810,8 +804,11 @@ class LoggerControl():
self.wbc_D = data["wbc_D"]
self.wbc_q_des = data["wbc_q_des"]
self.wbc_v_des = data["wbc_v_des"]
+ self.wbc_FF = data["wbc_FF"]
self.wbc_tau_ff = data["wbc_tau_ff"]
+ self.wbc_ddq_IK = data["wbc_ddq_IK"]
self.wbc_f_ctc = data["wbc_f_ctc"]
+ self.wbc_ddq_QP = data["wbc_ddq_QP"]
self.wbc_feet_pos = data["wbc_feet_pos"]
self.wbc_feet_pos_target = data["wbc_feet_pos_target"]
self.wbc_feet_err = data["wbc_feet_err"]
@@ -977,11 +974,11 @@ class LoggerControl():
fsteps = self.planner_fsteps[0]
o_step = np.zeros((3*int(fsteps.shape[0]), 1))
- RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q_int[0, 3:7]).toRotationMatrix())
+ RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q[0, 3:7]).toRotationMatrix())
quat[:, 0] = utils_mpc.EulerToQuaternion([0.0, 0.0, RPY[2]])
oRh = pin.Quaternion(quat).toRotationMatrix()
for j in range(4):
- o_step[0:3, 0:1] = oRh @ fsteps[0:1, (j*3):((j+1)*3)].transpose() + self.loop_o_q_int[0:1, 0:3].transpose()
+ o_step[0:3, 0:1] = oRh @ fsteps[0:1, (j*3):((j+1)*3)].transpose() + self.loop_o_q[0:1, 0:3].transpose()
h1, = plt.plot(o_step[0::3, 0], o_step[1::3, 0], linestyle=None, linewidth=0, marker="o", color=f_c[j])
h1s.append(h1)
@@ -1006,12 +1003,12 @@ class LoggerControl():
rounded = int(np.round(time_slider.val / self.dt, decimals=0))
fsteps = self.planner_fsteps[rounded]
o_step = np.zeros((3*int(fsteps.shape[0]), 1))
- RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q_int[rounded, 3:7]).toRotationMatrix())
+ RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q[rounded, 3:7]).toRotationMatrix())
quat[:, 0] = utils_mpc.EulerToQuaternion([0.0, 0.0, RPY[2]])
oRh = pin.Quaternion(quat).toRotationMatrix()
for j in range(4):
for k in range(int(fsteps.shape[0])):
- o_step[(3*k):(3*(k+1)), 0:1] = oRh @ fsteps[(k):(k+1), (j*3):((j+1)*3)].transpose() + self.loop_o_q_int[rounded:(rounded+1), 0:3].transpose()
+ o_step[(3*k):(3*(k+1)), 0:1] = oRh @ fsteps[(k):(k+1), (j*3):((j+1)*3)].transpose() + self.loop_o_q[rounded:(rounded+1), 0:3].transpose()
h1s[j].set_xdata(o_step[0::3, 0].copy())
h1s[j].set_ydata(o_step[1::3, 0].copy())
fig.canvas.draw_idle()
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 5b402ad2..b8403418 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -197,7 +197,7 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
device.joints.set_velocity_gains(controller.result.D)
device.joints.set_desired_positions(controller.result.q_des)
device.joints.set_desired_velocities(controller.result.v_des)
- device.joints.set_torques(controller.result.tau_ff.ravel())
+ device.joints.set_torques(controller.result.FF * controller.result.tau_ff.ravel())
# Call logger
if params.LOGGING or params.PLOTTING:
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 1a2623b5..fe943803 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -42,7 +42,6 @@ Vector3 ComplementaryFilter::compute(Vector3 const& x, Vector3 const& dx, Vector
Estimator::Estimator()
: dt_wbc(0.0),
- alpha_v_(0.0),
alpha_secu_(0.0),
offset_yaw_IMU_(0.0),
perfect_estimator(false),
@@ -71,6 +70,7 @@ Estimator::Estimator()
v_filt_dyn_(VectorN::Zero(18, 1)),
v_secu_dyn_(VectorN::Zero(12, 1)),
q_up_(VectorN::Zero(18)),
+ v_up_(VectorN::Zero(18)),
v_ref_(VectorN::Zero(6)),
h_v_(VectorN::Zero(6)),
oRh_(Matrix3::Identity()),
@@ -86,11 +86,6 @@ void Estimator::initialize(Params& params) {
perfect_estimator = params.perfect_estimator;
// Filtering estimated linear velocity
- double fc = params.fc_v_esti; // Cut frequency
- double y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
- alpha_v_ = -y + std::sqrt(y * y + 2 * y);
- alpha_v_ = 1.0;
-
N_queue_ = static_cast<int>(std::round(params.T_gait / dt_wbc));
vx_queue_.resize(N_queue_, 0.0); // List full of 0.0
vy_queue_.resize(N_queue_, 0.0); // List full of 0.0
@@ -100,8 +95,8 @@ void Estimator::initialize(Params& params) {
wY_queue_.resize(N_queue_, 0.0); // List full of 0.0
// Filtering velocities used for security checks
- fc = 6.0; // Cut frequency
- y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
+ double fc = 6.0; // Cut frequency
+ double y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
alpha_secu_ = -y + std::sqrt(y * y + 2 * y);
FK_xyz_(2, 0) = params.h_ref;
@@ -260,7 +255,7 @@ Vector3 Estimator::BaseVelocityFromKinAndIMU(int contactFrameId) {
void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN const& baseLinearAcceleration,
VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q_mes,
- VectorN const& v_mes, VectorN const& dummyPos, VectorN const& b_baseVel) {
+ VectorN const& v_mes, VectorN const& dummyPos, Vector3 const& b_baseVel) {
feet_status_ = gait.block(0, 0, 1, 4);
feet_goals_ = goals;
@@ -344,20 +339,17 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
q_filt_.tail(12) = actuators_pos_; // Actuators pos are already directly from PyBullet
// Output filtered velocity vector (18 x 1)
- if (perfect_estimator) { // Linear velocities directly from PyBullet
- v_filt_.head(3) = (1 - alpha_v_) * v_filt_.head(3) + alpha_v_ * b_baseVel;
- } else {
- v_filt_.head(3) = (1 - alpha_v_) * v_filt_.head(3) + alpha_v_ * b_filt_lin_vel_;
- }
+ // Linear velocities directly from PyBullet if perfect estimator
+ v_filt_.head(3) = perfect_estimator ? b_baseVel : b_filt_lin_vel_;
v_filt_.block(3, 0, 3, 1) = filt_ang_vel; // Angular velocities are already directly from PyBullet
v_filt_.tail(12) = actuators_vel_; // Actuators velocities are already directly from PyBullet
vx_queue_.pop_back();
vy_queue_.pop_back();
vz_queue_.pop_back();
- vx_queue_.push_front(b_filt_lin_vel_(0));
- vy_queue_.push_front(b_filt_lin_vel_(1));
- vz_queue_.push_front(b_filt_lin_vel_(2));
+ vx_queue_.push_front(perfect_estimator ? b_baseVel(0) : b_filt_lin_vel_(0));
+ vy_queue_.push_front(perfect_estimator ? b_baseVel(1) : b_filt_lin_vel_(1));
+ vz_queue_.push_front(perfect_estimator ? b_baseVel(2) : b_filt_lin_vel_(2));
v_filt_bis_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.0) / N_queue_;
v_filt_bis_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.0) / N_queue_;
v_filt_bis_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.0) / N_queue_;
@@ -424,12 +416,14 @@ void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait) {
Matrix2 Ryaw;
Ryaw << cos(yaw_estim_), -sin(yaw_estim_), sin(yaw_estim_), cos(yaw_estim_);
- q_up_.head(2) = q_up_.head(2) + Ryaw * v_ref_.head(2) * dt_wbc;
+ v_up_.head(2) = Ryaw * v_ref_.head(2);
+ q_up_.head(2) = q_up_.head(2) + v_up_.head(2) * dt_wbc;
// Mix perfect x and y with height measurement
q_up_[2] = q_filt_dyn_[2];
// Mix perfect yaw with pitch and roll measurements
+ v_up_[5] = v_ref_[5];
yaw_estim_ += v_ref_[5] * dt_wbc;
q_up_.block(3, 0, 3, 1) << IMU_RPY_[0], IMU_RPY_[1], yaw_estim_;
@@ -438,12 +432,13 @@ void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait) {
// Actuators measurements
q_up_.tail(12) = q_filt_dyn_.tail(12);
+ v_up_.tail(12) = v_filt_dyn_.tail(12);
// Velocities are the one estimated by the estimator
Matrix3 hRb = pinocchio::rpy::rpyToMatrix(IMU_RPY_[0], IMU_RPY_[1], 0.0);
- h_v_.head(3) = hRb * v_filt_dyn_.block(0, 0, 3, 1);
- h_v_.tail(3) = hRb * v_filt_dyn_.block(3, 0, 3, 1);
+ h_v_.head(3) = hRb * v_filt_.block(0, 0, 3, 1);
+ h_v_.tail(3) = hRb * v_filt_.block(3, 0, 3, 1);
h_v_windowed_.head(3) = hRb * v_filt_bis_.block(0, 0, 3, 1);
h_v_windowed_.tail(3) = hRb * v_filt_bis_.block(3, 0, 3, 1);
} else {
--
GitLab