One function in Joystick switched to C++

CMakeLists.txt

@@ -58,6 +58,7 @@ set(${PROJECT_NAME}_HEADERS
   include/qrw/QPWBC.hpp
   include/qrw/Params.hpp
   include/qrw/Estimator.hpp
+  include/qrw/Joystick.hpp
   include/other/st_to_cc.hpp
   )
 
@@ -73,6 +74,7 @@ set(${PROJECT_NAME}_SOURCES
   src/QPWBC.cpp
   src/Params.cpp
   src/Estimator.cpp
+  src/Joystick.cpp
   )
 
 add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})

python/gepadd.cpp

@@ -7,6 +7,7 @@
 #include "qrw/FootTrajectoryGenerator.hpp"
 #include "qrw/QPWBC.hpp"
 #include "qrw/Estimator.hpp"
+#include "qrw/Joystick.hpp"
 #include "qrw/Params.hpp"
 
 #include <boost/python.hpp>
@@ -334,6 +335,30 @@ struct EstimatorPythonVisitor : public bp::def_visitor<EstimatorPythonVisitor<Es
 };
 void exposeEstimator() { EstimatorPythonVisitor<Estimator>::expose(); }
 
+/////////////////////////////////
+/// Binding Joystick class
+/////////////////////////////////
+template <typename Joystick>
+struct JoystickPythonVisitor : public bp::def_visitor<JoystickPythonVisitor<Joystick>>
+{
+    template <class PyClassJoystick>
+    void visit(PyClassJoystick& cl) const
+    {
+        cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+            .def("handle_v_switch", &Joystick::handle_v_switch, bp::args("k", "k_switch", "v_switch"), "Run security check.\n");
+    }
+
+    static void expose()
+    {
+        bp::class_<Joystick>("Joystick", bp::no_init).def(JoystickPythonVisitor<Joystick>());
+
+        ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+    }
+};
+void exposeJoystick() { JoystickPythonVisitor<Joystick>::expose(); }
+
+
 /////////////////////////////////
 /// Binding Params class
 /////////////////////////////////
@@ -407,5 +432,6 @@ BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
     exposeQPWBC();
     exposeWbcWrapper();
     exposeEstimator();
+    exposeJoystick();
     exposeParams();
 }
\ No newline at end of file

scripts/Joystick.py

@@ -2,7 +2,7 @@
 
 import numpy as np
 import gamepadClient as gC
-
+import libquadruped_reactive_walking as lqrw
 
 class Joystick:
     """Joystick-like controller that outputs the reference velocity in local frame
@@ -57,6 +57,8 @@ class Joystick:
         self.westButton = False
         self.joystick_code = 0  # Code to carry information about pressed buttons
 
+        self.joyCpp = lqrw.Joystick()
+
     def update_v_ref(self, k_loop, velID):
         """Update the reference velocity of the robot along X, Y and Yaw in local frame by
         listening to a gamepad handled by an independent thread
@@ -164,30 +166,7 @@ class Joystick:
             k (int): numero of the current iteration
         """
 
-        i = 1
-        while (i < self.k_switch.shape[0]) and (self.k_switch[i] <= k):
-            i += 1
-        if (i != self.k_switch.shape[0]):
-            self.apply_velocity_change(k, i)
-
-    def apply_velocity_change(self, k, i):
-        """Change the velocity reference sent to the robot
-        4-th order polynomial: zero force and force velocity at start and end
-        (bell-like force trajectory)
-
-        Args:
-            k (int): numero of the current iteration
-            i (int): numero of the active phase of the reference velocity profile
-        """
-
-        ev = k - self.k_switch[i-1]
-        t1 = self.k_switch[i] - self.k_switch[i-1]
-        A3 = 2 * (self.v_switch[:, (i-1):i] -
-                  self.v_switch[:, i:(i+1)]) / t1**3
-        A2 = (-3/2) * t1 * A3
-        self.v_ref = self.v_switch[:, (i-1):i] + A2*ev**2 + A3*ev**3
-
-        return 0
+        self.v_ref[:, 0] = self.joyCpp.handle_v_switch(k, self.k_switch.reshape((-1, 1)), self.v_switch)
 
     def update_v_ref_predefined(self, k_loop, velID):
         """Update the reference velocity of the robot along X, Y and Yaw in local frame