Merge pull request #104 from dmronga/integrator_use_cur_state

Allow configuration of interpolator wrt. whether it should use
ARC-OPT · Jun 9, 2024 · fe08e35 · fe08e35
2 parents 3d440c7 + e60daa8
commit fe08e35
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 9 deletions.
diff --git a/src/tools/JointIntegrator.cpp b/src/tools/JointIntegrator.cpp
@@ -3,15 +3,16 @@
 
 namespace wbc {
 
-void JointIntegrator::integrate(const base::samples::Joints& joint_state, base::commands::Joints &cmd, double cycle_time, IntegrationMethod method){
+void JointIntegrator::integrate(const base::samples::Joints& joint_state, base::commands::Joints &cmd, double cycle_time, IntegrationMethod method, bool use_cur_state){
 
+    cur_joint_state = joint_state;
     if(initialized){
         switch (method) {
         case RECTANGULAR:
-            integrateRectangular(cmd,cycle_time);
+            integrateRectangular(cmd,cycle_time,use_cur_state);
             break;
         case TRAPEZOIDAL:
-            integrateTrapezoidal(cmd,cycle_time);
+            integrateTrapezoidal(cmd,cycle_time,use_cur_state);
             break;
         default:
             throw std::runtime_error("Invalid integration Method: " + std::to_string(method));
@@ -43,7 +44,13 @@ void JointIntegrator::integrate(const base::samples::Joints& joint_state, base::
     prev_cmd = cmd;
 }
 
-void JointIntegrator::integrateRectangular(base::commands::Joints &cmd, double cycle_time){
+void JointIntegrator::integrateRectangular(base::commands::Joints &cmd, double cycle_time, bool use_cur_state){
+
+    if(use_cur_state){
+        for(uint i = 0; i < cmd.size(); i++)
+            prev_cmd[i] = cur_joint_state[cmd.names[i]];
+    }
+
     for(uint i = 0; i < cmd.size(); i++)
     {
         double h = cycle_time;
@@ -67,15 +74,20 @@ void JointIntegrator::integrateRectangular(base::commands::Joints &cmd, double c
     }
 }
 
-void JointIntegrator::integrateTrapezoidal(base::commands::Joints &cmd, double cycle_time){
+void JointIntegrator::integrateTrapezoidal(base::commands::Joints &cmd, double cycle_time, bool use_cur_state){
+
+    if(use_cur_state){
+        for(uint i = 0; i < cmd.size(); i++)
+            prev_cmd[i] = cur_joint_state[cmd.names[i]];
+    }
     for(uint i = 0; i < cmd.size(); i++)
     {
         double h = cycle_time/2;
         double q_prev = prev_cmd[i].position;
         double qd_prev = prev_cmd[i].speed;
+        double qdd_prev = prev_cmd[i].acceleration;
         double qd = cmd[i].speed;
         double qdd = cmd[i].acceleration;
-        double qdd_prev = cmd[i].acceleration;
 
         switch(cmdMode(cmd[i]))
         {

diff --git a/src/tools/JointIntegrator.hpp b/src/tools/JointIntegrator.hpp
@@ -17,26 +17,31 @@ namespace wbc {
 class JointIntegrator{
     bool initialized;
     base::commands::Joints prev_cmd;
+    base::samples::Joints cur_joint_state;
 public:
     JointIntegrator() : initialized(false){}
     /**
      * @brief Performs numerical from acceleration/velocity to positions
      * @param cmd Input joint command, wil be modified by the method
      * @param cycle_time Period between two consecutive calls in seconds
+     * @param method Integration method to use
+     * @param use_cur_state Integrate from internal (interpolated) state or from current joint state. In the first case, the integrator will not be aware of the current robot state and
+     * integrated position/velocity vs. real robot position/velocity may drift apart. As a rule-of-thumb, for stiff, position-controlled robots, use_cur_state should be false, while for
+     * compliant robots that have environment contacts, e.g., walking robots, use_cur_state should be true.
      */
-    void integrate(const base::samples::Joints& joint_state, base::commands::Joints &cmd, double cycle_time, IntegrationMethod method = TRAPEZOIDAL);
+    void integrate(const base::samples::Joints& joint_state, base::commands::Joints &cmd, double cycle_time, IntegrationMethod method = RECTANGULAR, bool use_cur_state = false);
     /**
      * @brief Performs numerical from acceleration/velocity to positions using rectangular method
      * @param cmd Input joint command, wil be modified by the method
      * @param cycle_time Period between two consecutive calls in seconds
      */
-    void integrateRectangular(base::commands::Joints &cmd, double cycle_time);
+    void integrateRectangular(base::commands::Joints &cmd, double cycle_time, bool use_cur_state = false);
     /**
      * @brief Trapezoidal method for numerical integration
      * @param cmd Input joint command, wil be modified by the method
      * @param cycle_time Period between two consecutive calls in seconds
      */
-    void integrateTrapezoidal(base::commands::Joints &cmd, double cycle_time);
+    void integrateTrapezoidal(base::commands::Joints &cmd, double cycle_time, bool use_cur_state = false);
     /**
      * @brief cmdType Return the control model type POSITION/VELOCITY/ACCELERATION depending on the valid fields
      */