pybullet force/torque sensor can't measure torque correctly

[YaoweiFan]

I want to measure the external force/torque of the end-effector of a franka panda arm--panda.urdf. So, I enable joint force/torque sensor on a fixed joint --"panda_hand_joint":

p.enableJointForceTorqueSensor(self.pandaUid, 8, True)

Then I drive arm into the following configuration, but the measured torque does't meet my expection. The measured torque Mx is too small while my expection is around 0.32N*m:

fm = np.array(self.p.getJointState(self.uid, 8)[2])

Why the measured torque is not correct? Could you give me some advice?

[rohit-kumar-j]

It does seem to reach the expected value right? Like at the end of the graph, the motion stops, and the value is recorded.

You did say that you 'drive the arm' to the position. IMO, the torque was recorded as a consequence of the accelerations that the end effector faced during the motion. After that, once the motion stops, only the force/ torque flatten out.

Altought I do not understand the orange line's qty (<fm_fy>)

Could you please elaborate the data?

[YaoweiFan]

Thank you for your reply @rohit-kumar-j. I'm so sorry that I have not elaborate the data. Here is what these data mean:

fm_fx, fm_fy, fm_fz -- measured force in link frame
fm_fx, fm_fy, fm_fz -- measured torque in link frame

After the motion stops, only the gravity of the end-effector produce force and torque. After the curve flattens out, one of the measured torque < fm_mx > should not equal to zero ( but the curve shows that it really is zero ), which should be around 0.32N*m as my expect. The force measured was in line with my expectations.

[rohit-kumar-j]

Thank you for the description, there may be a few indexing or labeling problems

< fm_mx > should not equal to zero ( but the curve shows that it really is zero )

If you have manually mapped these values in the code, then you could try this:

1. Could you recheck if the values are mapped correctly to the labels in the graph?
2. Are the axes in the link frame are aligned as expected? (perhaps running a 1D motion individually in X, Y, Z axes could help differentiate the correct child-link frame orientation)

If there is an inbuilt function to generate the values on the graph, then most likely the axes in the link frame may not be aligned as expected. then you can try No.2. (What I mean is, the dark green line is perhaps not labeled correctly).

Also, it would be helpful if you could provide the code.

[YaoweiFan]

Thank you very much for your kind reply @rohit-kumar-j . I simplify my code. Here is the code:

import pybullet as p
import gym_tmwk as gt
import numpy as np
import pybullet_data
import os

p.connect(p.GUI)
p.resetDebugVisualizerCamera(cameraDistance=0.9, cameraYaw=180, cameraPitch=0, cameraTargetPosition=[0.,0.3,0.1])

p.resetSimulation()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.setGravity(0,0,-10)
urdfRootPath=pybullet_data.getDataPath()
planeUid = p.loadURDF(os.path.join(urdfRootPath,"plane.urdf"), basePosition=[0,0,-0.65])
rest_poses = [0, 0.32, 0, -2.41, 0, 1.16, 2.356, 0, 0]
panda1Uid = p.loadURDF(os.path.join(urdfRootPath, "franka_panda/panda.urdf"),useFixedBase=True, 
                                                                basePosition=[0,0,0], baseOrientation=[0, 0, 0.707, 0.707])
p.enableJointForceTorqueSensor(panda1Uid, 8, True)

for i in range(7):
    p.resetJointState(panda1Uid,i, rest_poses[i])

p.resetJointState(panda1Uid, 9, 0.08)
p.resetJointState(panda1Uid,10, 0.08)

tableUid = p.loadURDF(os.path.join(urdfRootPath, "table/table.urdf"),basePosition=[0.5,0.2,-0.65])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)

position = [0.,0.3,0.2]
orientation = [0.717,0.,0.,0.717]
fm_list = []
p.addUserDebugLine([0,0,0],[0.3,0,0],[1,0,0],parentObjectUniqueId=panda1Uid, parentLinkIndex=8)
p.addUserDebugLine([0,0,0],[0,0.3,0],[0,1,0],parentObjectUniqueId=panda1Uid, parentLinkIndex=8)
p.addUserDebugLine([0,0,0],[0,0,0.3],[0,0,1],parentObjectUniqueId=panda1Uid, parentLinkIndex=8)
for i in range(10000):
    jointPoses = p.calculateInverseKinematics(panda1Uid, 8, position, orientation)[0:7]
    p.setJointMotorControlArray(panda1Uid, list(range(7))+[9,10], p.POSITION_CONTROL, list(jointPoses)+2*[0])
    #get measured force and torque
    fm = np.array(p.getJointState(panda1Uid, 8)[2])
    fm_list.append(fm)
    p.stepSimulation()

import matplotlib.pyplot as plt
list_size = len(fm_list)
fig = plt.figure(figsize=(20,7))
ax = fig.add_subplot(111)
plt.plot(range(list_size), np.array([elem[0] for elem in fm_list]), label='fm_fx')
plt.plot(range(list_size), np.array([elem[1] for elem in fm_list]), label='fm_fy')
plt.plot(range(list_size), np.array([elem[2] for elem in fm_list]), label='fm_fz')
plt.plot(range(list_size), np.array([elem[3] for elem in fm_list]), label='fm_mx')
plt.plot(range(list_size), np.array([elem[4] for elem in fm_list]), label='fm_my')
plt.plot(range(list_size), np.array([elem[5] for elem in fm_list]), label='fm_mz')
plt.title("measured force/torque")
plt.legend()
plt.show()

Here is the ouput:

1.Could you recheck if the values are mapped correctly to the labels in the graph?

The values show in graph are just the return of pybullet function getJointState().

2.Are the axes in the link frame are aligned as expected? (perhaps running a 1D motion individually in X, Y, Z axes could help differentiate the correct child-link frame orientation)

The dark blue line represent the measured force in y direction of the sensor frame. It should be zero but not. I think this may be caused by inaccurate movement. I don't care about it so much, what I'm really care about is the measured torque.
I have checked the frame.The frame of force/torque sensor is different from the link frame.The reason why I rotate the end-effector rather than run a 1D motion individually is that I want to test whether the output of force/torque sensor is correct or not. And the graph shows that the output torque is not correct--the measured torques are all equal to zero but according to the common sense, they shouldn't.

I have thought it might be because the mass of finger (equipped on the hand) is too small so the torque is close to zero. So I modified the mass of fingers in <panda.urdf>, but the measured torques were still out of my expection.