Disable torque control - test open loop + friction comp

isorrentino · Jun 24, 2024 · 089ff16 · 089ff16
1 parent bb1d809
commit 089ff16
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp b/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp
@@ -233,6 +233,9 @@ void JointTorqueControlDevice::computeDesiredCurrents()
             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] = desiredMotorCurrents[j] / m_gearRatios[j];
 
             desiredMotorCurrents[j] = saturation(desiredMotorCurrents[j],