From 0ec2c9cfb9e3a058c4ad33c50a1461dbe6ed6859 Mon Sep 17 00:00:00 2001
From: Paul <paul_d_blum@yahoo.com>
Date: Tue, 27 Feb 2024 16:02:27 -0500
Subject: [PATCH] =?UTF-8?q?Reimplement=20`computeTorque()`=20to=20remove?=
 =?UTF-8?q?=20=C2=B155Nm=20clamp?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 unitree_legged_control/src/joint_controller.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

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);