Reimplement computeTorque() for Accurate B1 Motor Simulation

[paulblum]

Description:

This pull request addresses issue #66, which identifies the clamping of joint torque commands to ±55 Nm by UnitreeJointController. This limitation hinders the accurate simulation of B1 motors, which have ±140 Nm capability.

The impact is evident in the following visualizations, where B1 is commanded to trot in place:

B1 attempting to trot in Gazebo	Torque clamping to 55 Nm

Problem:

The issue lies in UnitreeJointController's usage (line 180, below) of the computeTorque() function from unitree_ros/unitree_legged_control/lib, which inadvertently clamps its output to ±55 Nm.

unitree_ros/unitree_legged_control/src/joint_controller.cpp

Lines 178 to 181 in 67ac212

	currentPos = joint.getPosition();
	currentVel = computeVel(currentPos, (double)lastState.q, (double)lastState.dq, period.toSec());
	calcTorque = computeTorque(currentPos, currentVel, servoCmd);
	effortLimits(calcTorque);

Notably, this clamping is redundant, as the joint torque computation is already clamped to URDF specifications (line 181, above) by the effortLimits() function.

Solution:

This pull request proposes the removal of the ±55 Nm clamp from computeTorque(), introducing the following implementation:

calcTorque = servoCmd.torque + servoCmd.posStiffness*(servoCmd.pos - currentPos)
    + servoCmd.velStiffness*(servoCmd.vel - currentVel);

Rationale:

This modification allows for the utilization of UnitreeJointController to accurately simulate B1 control, while preserving its original functionality for smaller quadrupeds.

B1 trotting in Gazebo via updated code	Update enables accurate B1 torque computation

A1 trotting in Gazebo via updated code	Update maintains identical torque computation in the [-55,55] Nm range