jointReactionForces torque two times the applied MotorTorque

[marvinalles]

Hi @erwincoumans,

I would like to obtain joint reaction torques since they vary from the MotorTorque during collisions, but have some troubles understanding the sensor values.

Using p.TORQUE_CONTROL with force=2 results in a torque of 4 in the jointReactionForces (0.0, -6.8181816792362735, -8.070764462809916e-06, 4.0, 0.0, 0.0). Why is the sensor torque two times the motor torque?

I would be grateful for any advice!

import pybullet as p

hinge = p.loadURDF("hinge.urdf")
hingeJointIndex = 0

p.setPhysicsEngineParameter(jointFeedbackMode=p.JOINT_FEEDBACK_IN_JOINT_FRAME)

p.setJointMotorControl2(hinge, hingeJointIndex,p.VELOCITY_CONTROL,0,0,0)
p.setGravity(0,0,0)

p.enableJointForceTorqueSensor(hinge,hingeJointIndex)

for _ in range(100):
    p.setJointMotorControl2(hinge,hingeJointIndex,p.TORQUE_CONTROL,force=10)
    p.stepSimulation()
    print(p.getJointState(hinge, hingeJointIndex)[2])
 

hinge.urdf

<?xml version="1.0" ?>
<robot name="urdf_robot">
  <link name="baseLink">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 -0.175"/>
       <mass value="0.0"/>
       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.35"/>
      <geometry>
      <box size="0.08 0.16 0.2"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.35"/>
      <geometry>
	 <box size="0.08 0.16 0.2"/>
      </geometry>
    </collision>
  </link>
   <link name="childA">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.25"/>
      <mass value="100"/>
      <inertia ixx="0.0" ixy="0" ixz="0" iyy="0.0" iyz="0" izz="0.0"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.25"/>
      <geometry>
        <box size="0.1 0.1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.25"/>
      <geometry>
        <box size="0.1 0.1 1 "/>
      </geometry>
    </collision>
  </link>

  <joint name="panda_joint2" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 1"/>
    <parent link="baseLink"/>
    <child link="childA"/>
    <axis xyz="1 0 0"/>
    <limit effort="87" lower="-100" upper="100" velocity="2.1750"/>
  </joint>

</robot>

[Steven89Liu]

I also have the similar questions.

1. feedback force means all force exert on link i through its inboard joint ?
2. after reviewing the code, the engine calculates the feedback force in two steps, first calculate the feedback force based on the external force + joint actuator, then plus the feedback force based on the constraint force + joint actuator. i guess the second pass is used to calcualte the feedback force caused by the constraint force, such as motor constraint, contact contraint, etc. but i don't understand why we have to add the 'joint actuator' at the second pass. for some special case that the external force and the constraint force all are zero then what we get the final result will be twice.
   thanks.

[marvinalles]

Hi @Steven89Liu,

Could you add the filename/ code, where the feedback force calculation happens? Thanks!

[Steven89Liu]

you can refer to the function computeAccelerationsArticulatedBodyAlgorithmMultiDof() in file btMultiBody.cpp and search the m_jointFeedback.

[Steven89Liu]

Hi @ma-rv,
Do you have any progress?

[marvinalles]

Not really. I didn't have the time to look further into it and just ended up modifying the reaction torques afterwards so they looked reasonable:

For p.TORQUE_CONTROL -> measured_torque = measured_torque - applied_torque
For p.POSITION_CONTROL -> measured_torque = measured_torque + joint_torque

Though its just a workaround to unify the measurements and I still think there is either some miss-understanding from my side or an error in the code..

[Steven89Liu]

thanks for your reply.
Hi @erwincoumans,
i still think we should not add the joint actuator (Q) in the second pass when try to calculate the feedback force caused by the constraint force. what do you think about this? thanks.

[Humhu]

I have observed a similar phenomena with the linear reaction force being exactly double what I expect from hand calculations. This is reproducible with the above single hinge joint moving at constant velocity with no gravity. When gravity is added, the reaction force is m*g larger, but not doubled, which supports @Steven89Liu 's hypothesis about the two dynamics passes being the source of doubling from constraints.

[Steven89Liu]

Hi @ma-rv,
Could you help test my patch with your test cases? or provide another test about the POSITION_CONTROL, i test it myself. thanks.

diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 9862bd2..367933e 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -947,7 +947,10 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
                        //
                        hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];
                        //
-                       Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
+                        btScalar jointTorque = 0;
+                        if (isConstraintPass) jointTorque = 0;
+                        else jointTorque = m_links[i].m_jointTorque[dof];
+                       Y[m_links[i].m_dofOffset + dof] = jointTorque - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
                }
                for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
                {

[marvinalles]

Hi @Steven89Liu,

Sorry for the late response. I have been been quite busy and forgot to answer. Anyway, you could simply change
p.setJointMotorControl2(hinge,hingeJointIndex,p.TORQUE_CONTROL,force=10)
to
p.setJointMotorControl2(hinge,hingeJointIndex,p.POSITION_CONTROL, targetPosition=2, targetVelocity=0, force=10)
and check for reasonable results.