From e60daa8a26127958882b991628c2533ecdcf72e0 Mon Sep 17 00:00:00 2001
From: dmronga <dennis.mronga@dfki.de>
Date: Sun, 9 Jun 2024 13:49:22 +0200
Subject: [PATCH] Allow configuration of interpolator wrt. whether it should
 use internal (estimated) state or current joint state as a starting point

---
 src/tools/JointIntegrator.cpp | 24 ++++++++++++++++++------
 src/tools/JointIntegrator.hpp | 11 ++++++++---
 2 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/src/tools/JointIntegrator.cpp b/src/tools/JointIntegrator.cpp
index d4c42cf1..56dcd38c 100644
--- 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
index 8c650e8b..cf6e8b22 100644
--- 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
      */