Reimplement `computeTorque()` to remove ±55Nm clamp

2024-02-27 16:02:27 -05:00 · 2024-02-27 16:02:27 -05:00 · 0ec2c9cfb9
parent 67ac212449
commit 0ec2c9cfb9
1 changed files with 2 additions and 1 deletions
--- a/unitree_legged_control/src/joint_controller.cpp
+++ b/unitree_legged_control/src/joint_controller.cpp
@ -177,7 +177,8 @@ namespace unitree_legged_control

        currentPos = joint.getPosition();
        currentVel = computeVel(currentPos, (double)lastState.q, (double)lastState.dq, period.toSec());
-        calcTorque = computeTorque(currentPos, currentVel, servoCmd);      
+        calcTorque = servoCmd.torque + servoCmd.posStiffness*(servoCmd.pos - currentPos)
+            + servoCmd.velStiffness*(servoCmd.vel - currentVel);
        effortLimits(calcTorque);

        joint.setCommand(calcTorque);