Fix torque control law

isorrentino · Jun 25, 2024 · 625f9c8 · 625f9c8
1 parent 089ff16
commit 625f9c8
Showing 1 changed file with 6 additions and 5 deletions.
diff --git a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp b/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp
@@ -230,11 +230,12 @@ void JointTorqueControlDevice::computeDesiredCurrents()
                 estimatedFrictionTorques[j] = computeFrictionTorque(j);
             }
 
-            desiredMotorCurrents[j] = (motorTorqueCurrentParameters[j].kp * (desiredJointTorques[j] - measuredJointTorques[j])
-                                      + motorTorqueCurrentParameters[j].kfc * estimatedFrictionTorques[j]) / motorTorqueCurrentParameters[j].kt;
-
-            desiredMotorCurrents[j] = (motorTorqueCurrentParameters[j].kp * desiredJointTorques[j]
-                                      + motorTorqueCurrentParameters[j].kfc * estimatedFrictionTorques[j]) / motorTorqueCurrentParameters[j].kt;
+            desiredMotorCurrents[j]
+                = (desiredJointTorques[j]
+                   + motorTorqueCurrentParameters[j].kp
+                         * (desiredJointTorques[j] - measuredJointTorques[j])
+                   + motorTorqueCurrentParameters[j].kfc * estimatedFrictionTorques[j])
+                  / motorTorqueCurrentParameters[j].kt;
 
             desiredMotorCurrents[j] = desiredMotorCurrents[j] / m_gearRatios[j];