pybullet force/torque sensor can't measure torque correctly #3204

Activity

1. rohit-kumar-j commented on Jan 1, 2021

   

   rohit-kumar-j

   on Jan 1, 2021

   More actions

   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?
2. YaoweiFan commented on Jan 2, 2021

   

   YaoweiFan

   on Jan 2, 2021

   Author

   More actions

   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.
3. rohit-kumar-j commented on Jan 9, 2021

   

   rohit-kumar-j

   on Jan 9, 2021

   Last edited by rohit-kumar-j

   More actions

   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.
4. YaoweiFan commented on Jan 16, 2021

   

   YaoweiFan

   on Jan 16, 2021

   Author

   More actions

   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.
5. erwincoumans

   added

   new feature

   on Apr 26, 2021
6. erwincoumans

   closed this as completedon Apr 26, 2021
7. bulletphysics

   locked and limited conversation to collaborators on Apr 26, 2021