diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 43319cbeb8845149aad7b13375e78965ebf1fdc3..ff8120266b7b1a1465f14a4f651bd0e2378c0682 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -301,6 +301,9 @@ struct WbcWrapperPythonVisitor : public bp::def_visitor<WbcWrapperPythonVisitor<
.def("get_qdes", &WbcWrapper::get_qdes, "Get qdes_.\n")
.def("get_vdes", &WbcWrapper::get_vdes, "Get vdes_.\n")
.def("get_tau_ff", &WbcWrapper::get_tau_ff, "Get tau_ff_.\n")
+ .def("get_tasks_acc", &WbcWrapper::get_tasks_acc, "Get tasks acceleration.\n")
+ .def("get_tasks_vel", &WbcWrapper::get_tasks_vel, "Get tasks velocity.\n")
+ .def("get_tasks_err", &WbcWrapper::get_tasks_err, "Get tasks error.\n")
.def_readonly("bdes", &WbcWrapper::get_bdes)
.def_readonly("qdes", &WbcWrapper::get_qdes)
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 2056dd705c59dfd2fbb05b30112d48be78c2823a..550cb694713b7f91c700b1638a53df54d020506d 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -213,7 +213,7 @@ class Controller:
t_start = time.time()
# Update the reference velocity coming from the gamepad
- self.joystick.update_v_ref(self.k, self.velID, self.gait.getIsStatic(), self.h_v_windowed[0:6, 0:1])
+ self.joystick.update_v_ref(self.k, self.velID, self.gait.getIsStatic())
# Process state estimator
self.estimator.run_filter(self.gait.getCurrentGait(),
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index ab77a068706861df5d752926d2045a1b1ee3d83c..b9b095826c6ccb469243b0b501efa02ce4cc7240 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -127,19 +127,19 @@ class LoggerControl():
return
# Logging from joystick
- self.joy_v_ref[self.i] = joystick.v_ref[:, 0]
+ self.joy_v_ref[self.i] = joystick.getVRef()
# Logging from estimator
self.esti_feet_status[self.i] = estimator.getFeetStatus()
self.esti_feet_goals[self.i] = estimator.getFeetGoals()
self.esti_q_filt[self.i] = estimator.getQFilt()
- self.esti_q_up[self.i] = self.estimator.getQUpdated()
+ self.esti_q_up[self.i] = estimator.getQUpdated()
self.esti_v_filt[self.i] = estimator.getVFilt()
- self.esti_v_filt_bis[self.i] = estimator.getVFiltBis()
- self.esti_v_up[self.i] = self.estimator.getVUpdated()
- self.esti_v_ref[self.i] = self.estimator.getVRef()
+ self.esti_v_filt_bis[self.i, :6] = estimator.getVFiltBis()
+ self.esti_v_up[self.i] = estimator.getVUpdated()
+ self.esti_v_ref[self.i] = estimator.getVRef()
self.esti_v_secu[self.i] = estimator.getVSecu()
- self.esti_a_ref[self.i] = self.estimator.getARef()
+ self.esti_a_ref[self.i] = estimator.getARef()
self.esti_FK_lin_vel[self.i] = estimator.getFKLinVel()
self.esti_FK_xyz[self.i] = estimator.getFKXYZ()
@@ -203,9 +203,9 @@ class LoggerControl():
self.wbc_feet_vel[self.i] = wbc.feet_vel
self.wbc_feet_vel_target[self.i] = wbc.feet_vel_target
self.wbc_feet_acc_target[self.i] = wbc.feet_acc_target
- self.wbc_tasks_acc[self.i] = wbc.getTasksAcc()
- self.wbc_tasks_vel[self.i] = wbc.getTasksVel()
- self.wbc_tasks_err[self.i] = wbc.getTasksErr()
+ self.wbc_tasks_acc[self.i] = wbc.get_tasks_acc()
+ self.wbc_tasks_vel[self.i] = wbc.get_tasks_vel()
+ self.wbc_tasks_err[self.i] = wbc.get_tasks_err()
# Logging timestamp
self.tstamps[self.i] = time()
diff --git a/scripts/PyBulletSimulator.py b/scripts/PyBulletSimulator.py
index 74015036b52568d0d4c75d054f45a4cf1f32496d..81f961112a4228e7275efbc5c6931ac1856340cd 100644
--- a/scripts/PyBulletSimulator.py
+++ b/scripts/PyBulletSimulator.py
@@ -544,7 +544,7 @@ class IMU():
self.accelerometer = np.zeros((3, ))
self.gyroscope = np.zeros((3, ))
self.attitude_euler = np.zeros((3, ))
- self.attitude_quaternion = np.array([[0.0, 0.0, 0.0, 1.0]]).transpose()
+ self.attitude_quaternion = np.array([0.0, 0.0, 0.0, 1.0])
class Powerboard():
"""Dummy class that simulates the Powerboard class used to communicate with the real masterboard"""
@@ -699,7 +699,7 @@ class PyBulletSimulator():
# print("baseVel: ", np.array([self.baseVel[0]]))
# Orientation of the base (quaternion)
- self.imu.attitude_quaternion[:, 0] = np.array(self.baseState[1])
+ self.imu.attitude_quaternion[:] = np.array(self.baseState[1])
self.imu.attitude_euler[:] = pin.rpy.matrixToRpy(pin.Quaternion(self.imu.attitude_quaternion).toRotationMatrix())
self.rot_oMb = pin.Quaternion(self.imu.attitude_quaternion).toRotationMatrix()
self.oMb = pin.SE3(self.rot_oMb, np.array([self.dummyHeight]).transpose())
diff --git a/src/Joystick.cpp b/src/Joystick.cpp
index 3e060d0d21af92b281d1f4bea8342277ab4623be..62fb7c8f2a3acd48d20c208274d2fc608cad06da 100644
--- a/src/Joystick.cpp
+++ b/src/Joystick.cpp
@@ -47,12 +47,12 @@ VectorN Joystick::handle_v_switch_py(double k, VectorN const& k_switch_py, Matri
void Joystick::handle_v_switch(int k) {
int i = 1;
- while ((i < k_switch.rows()) && k_switch[i] <= k) {
+ while ((i < k_switch.cols()) && k_switch(0, i) <= k) {
i++;
}
- if (i != k_switch.rows()) {
- double ev = k - k_switch[i - 1];
- double t1 = k_switch[i] - k_switch[i - 1];
+ if (i != k_switch.cols()) {
+ double ev = k - k_switch(0, i - 1);
+ double t1 = k_switch(0, i) - k_switch(0, i - 1);
A3_ = 2 * (v_switch.col(i - 1) - v_switch.col(i)) / pow(t1, 3);
A2_ = (-3.0 / 2.0) * t1 * A3_;
v_ref_ = v_switch.col(i - 1) + A2_ * pow(ev, 2) + A3_ * pow(ev, 3);
@@ -261,7 +261,7 @@ void Joystick::update_v_ref_predefined(int k, int velID) {
t_switch = MatrixN::Zero(1, 7);
t_switch << 0, 2, 4, 6, 8, 10, 15;
v_switch = MatrixN::Zero(6, 7);
- v_switch.row(0) << 0, 2, 4, 6, 8, 10, 15;
+ v_switch.row(0) << 0.0, 0.4, 0.8, 1.0, 1.0, 1.0, 1.0;
break;
default:
throw std::runtime_error("Unknown velocity ID for the polynomial interpolation.");