Some joint torques from getJointState(s) always zeros in pybullet

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
jwhitman
Posts: 4
Joined: Fri Sep 25, 2020 1:49 pm

Some joint torques from getJointState(s) always zeros in pybullet

Post by jwhitman »

Hello,
I am using Pybullet for robotics research but I am having some issues with the joint state torque feedback.
When I use a hexapod model (we use this model in other simulators without issue) I consistently get a block of 0.0 as torque feedback from getJointState(s). In this case it is joints 9-13. I also observe some unrealistically large joint torques (e.g. 240) reported but I am less concerned about those as they are not persistent across all time. The zeros appear to be returned in any joint position and any control mode.
At first I thought it could be an issue with the urdf, but, all of the legs use the same parts and only some seemingly arbitrary subset of the joints have this problem.
I also observed that after enabling the enableJointForceTorqueSensor, I can get readings but the joint moment reported does not seem to have any relation to the joint state torque.
I have tried a few other hexapod models, which are not made from any of the same parts, and observed similar problems.
I have seen the same thing happen across multiple computers when running this script both in python2 and python3, and across multiple versions of pybullet. My system is Ubuntu 16.04 and is currently up to date with pybullet-3.0.4. I have attached the urdf and its stls in question, and code to reproduce the issue is below.
This problem is making it difficult for me to use pybullet for my research. Do you have any suggestions of what might be wrong?


Code: Select all


import pybullet as p
import pybullet_data
import os
import numpy as np
import time
physicsClient = p.connect(p.GUI)
p.resetSimulation() # remove all objects from the world and reset the world to initial conditions. 
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0,physicsClientId=physicsClient)

p.setGravity(0,0,-9.81)
planeId = p.loadURDF(os.path.join(pybullet_data.getDataPath(),
        "plane100.urdf"))
startPosition=[0,0,0.3] # high enough that nothing touches the ground
startOrientationRPY = [0,0,0]
startOrientation = p.getQuaternionFromEuler(startOrientationRPY)   

robotID = p.loadURDF(
             'misc_urdf/m6.urdf', 
                   basePosition=startPosition, baseOrientation=startOrientation,
                   flags= (p.URDF_MAINTAIN_LINK_ORDER 
                    | p.URDF_USE_SELF_COLLISION 
                    | p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES))


# count all joints, including fixed ones
num_joints_total = p.getNumJoints(robotID,
                physicsClientId=physicsClient)

# create lists of the joint names, types, and which ones are actuated
moving_joint_names = []
moving_joint_inds = []
moving_joint_types = []
moving_joint_limits = []
moving_joint_centers = []
for j_ind in range(num_joints_total):
    j_info = p.getJointInfo(robotID, 
        j_ind)
    if j_info[2] != (p.JOINT_FIXED):
        moving_joint_inds.append(j_ind)
        moving_joint_names.append(j_info[1])
        moving_joint_types.append(j_info[2])
        j_limits = [j_info[8], j_info[9]]
        j_center = (j_info[8] + j_info[9])/2
        if j_limits[1]<=j_limits[0]:
            j_limits = [-np.inf, np.inf]
            j_center = 0
        moving_joint_limits.append(j_limits)
        moving_joint_centers.append(j_center)

# set joint states to a standing up pose
num_joints = len(moving_joint_names)
for i in range(num_joints):
    center = moving_joint_centers[i]
    if i in [2,5,8]:
        addition = -np.pi/2
    elif i in [11,14,17]:
        addition = np.pi/2
    else:
        addition = 0
    jind = moving_joint_inds[i]
    p.resetJointState( bodyUniqueId=robotID, 
        jointIndex = jind,
        targetValue= center+addition, 
        targetVelocity = 0 )

# enable joint sensors
for j in moving_joint_inds:
    p.enableJointForceTorqueSensor(robotID, j, 1)
    
# steps to take with no control input before starting up
# This drops to robot down to a physically possible resting starting position
for i in range(200):

    p.stepSimulation()    
    joint_states = p.getJointStates(robotID,
            moving_joint_inds,
            physicsClientId=physicsClient)

    print('----------')
    print([j[3] for j in joint_states])
    # print out the joint moments too
#     print([j[2][3] for j in joint_states])
#     print([j[2][4] for j in joint_states])
#     print([j[2][5] for j in joint_states])
    time.sleep(0.01)

Attachments
m6_urdf.zip
(764.84 KiB) Downloaded 22 times
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Some joint torques from getJointState(s) always zeros in pybullet

Post by steven »

please apply this patch as below and also remove the flag of p.URDF_USE_SELF_COLLISION to avoid too large torque.
hope this can help you.

Code: Select all

diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index ac67d51a5..6e060bf54 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -7562,7 +7562,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
                        }
                        for (int d = 0; d < mb->getLink(l).m_dofCount; d++)
                        {
-                               stateDetails->m_jointMotorForce[totalDegreeOfFreedomU] = 0;
+                               stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = 0;

                                if (mb->getLink(l).m_jointType == btMultibodyLink::eSpherical)
                                {
Post Reply