diff --git a/unitree_legged_control/src/joint_controller.cpp b/unitree_legged_control/src/joint_controller.cpp
index e29866c4..83fd01f4 100644
--- 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);