TORQUE_CONTROL Not working

2021-11-25T16:38:06+00:00

I am using pybullet to simulate a bipedal robot using pre calculated trajectory. I have joints angles for the whole trajectory. It works perfectly using POSITION_CONTROL with "setJointMotorControl2". However, using a torque input with TORQUE_CONTROL the robot gets loose and the joints don't move.

I am also using the pdControllerExplicit from the examples provided in bullet3 repository by pybullet to generate torque values from the desired angles and error. I have also made sure to turn off the velocity control giving zero force.

1. Why is the TORQUE_CONTROL failing to work? I have tried the following to troubleshoot it am I missing something else?
-Changing the masses and inertias (I tried both using inertia values from the URDF file and letting pybullet calculate them)
-enabled the JointForceTorqueSensor and recorded the values for the torque and they gave zeros or they give constant values throughout the simulation.
-checked the code for bugs as well, here is the code and the URDF file below.
-Changing the timestep and sleeptime values.
Is there a problem with the TORQUE_CONTROL command or something I mentioned or something else entirely.

2. Also, How does the "setJointMotorControl2" work? Is there an open access source code for it?

Code:

import pybullet as pbimport timefrom numpy import *import pybullet_dataimport struct def computePD(bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):    # numJoints = pb.getNumJoints(bodyUniqueId)    numJoints = len(jointIndices)    jointStates = pb.getJointStates(bodyUniqueId, jointIndices)    q1 = []    qdot1 = []    for i in range(numJoints):      q1.append(jointStates[i][0])      qdot1.append(jointStates[i][1])    q = array(q1)    qdot = array(qdot1)    qdes = array(desiredPositions)    qdotdes = array(desiredVelocities)    qError = qdes - q    qdotError = qdotdes - qdot    Kp = diagflat(kps)    Kd = diagflat(kds)    forces = Kp.dot(qError) + Kd.dot(qdotError)    maxF = array(maxForces)    forces = clip(forces, -maxF, maxF)    return forces#______________________________________________________________________________#joint axis matrixjointIdListR = [0,1,2,3,4,5]jointIdListL = [6,7,8,9,10,11]LEG_DOF = 6maxForce = 10e20maxForceListForLeg = [maxForce]*LEG_DOFwith open("JointsLeftTotalMATLAB6.csv") as jointslefttotal:    jointslefttotal = loadtxt(jointslefttotal, delimiter=",")with open("JointsRightTotalMATLAB6.csv") as jointsrighttotal:    jointsrighttotal = loadtxt(jointsrighttotal, delimiter=",")    with open("torright.csv") as torright:    torright = loadtxt(torright, delimiter=",")with open("torleft.csv") as torleft:    torleft = loadtxt(torleft, delimiter=",")    with open("velleft.csv") as velleft:    velleft = loadtxt(velleft, delimiter=",")with open("velright.csv") as velright:    velright = loadtxt(velright, delimiter=",")    #______________________________________________________________________________pb.connect(pb.GUI)#Add earth like gravitypb.setGravity(0,0,-9.807)pb.setRealTimeSimulation(1)#Point the camera at the robot at the desired angle and distancepb.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=135, cameraPitch=-30, cameraTargetPosition=[0.0, 0.0, 0.25])if (0):    t0=time.time()    t=time.time()    while ((t-t0)<10):        t=time.time()        pb.disconnect()        break  #______________________________________________________________________________# controlMode=pb.POSITION_CONTROLpb.setAdditionalSearchPath(pybullet_data.getDataPath())planeId = pb.loadURDF("plane.urdf")robotId = pb.loadURDF("urdf/bipedalMini.urdf",[0,0,0.8],useFixedBase=True)for i in range (1):    pb.stepSimulation()        time.sleep(1./240.)  # qua = pb.getBasePositionAndOrientation(robotId)position, orientation = pb.getBasePositionAndOrientation(robotId)pb.resetBasePositionAndOrientation(robotId, position, orientation)numJoint = pb.getNumJoints(robotId)jointIdList = list(range(numJoint))#______________________________________________________________________________TorLHY = array([])TorLHR = array([])TorLHP = array([])TorLKP = array([])TorLAP = array([])TorLAR = array([])TorRHY = array([])TorRHR = array([])TorRHP = array([])TorRKP = array([])TorRAP = array([])TorRAR = array([])TorLHY1 = transpose(array([0,0,0,0,0,0]))TorLHR1 = transpose(array([0,0,0,0,0,0]))TorLHP1 = transpose(array([0,0,0,0,0,0]))TorLKP1 = transpose(array([0,0,0,0,0,0]))TorLAP1 = transpose(array([0,0,0,0,0,0]))TorLAR1 = transpose(array([0,0,0,0,0,0]))TorRHY1 = transpose(array([0,0,0,0,0,0]))TorRHR1 = transpose(array([0,0,0,0,0,0]))TorRHP1 = transpose(array([0,0,0,0,0,0]))TorRKP1 = transpose(array([0,0,0,0,0,0]))TorRAP1 = transpose(array([0,0,0,0,0,0]))TorRAR1 = transpose(array([0,0,0,0,0,0]))#______________________________________________________________________________for i in arange(len(jointIdList)):    pb.enableJointForceTorqueSensor(robotId, jointIndex=i)    #______________________________________________________________________________for i in arange(len(jointIdList)):        pb.setJointMotorControl2(robotId, jointIndex = i, controlMode=pb.VELOCITY_CONTROL, force=0)    #______________________________________________________________________________pb.setTimeStep(1/240)kp = [1000]*LEG_DOFkd = [300]*LEG_DOFtimeStep = 1./240.#______________________________________________________________________________for i in arange(len(jointsrighttotal[0])):        tausright = computePD(robotId, jointIdListR, jointsrighttotal[:,i], velright[:,i], kp, kd, maxForceListForLeg, timeStep)      tausleft = computePD(robotId, jointIdListL, jointslefttotal[:,i], velleft[:,i], kp, kd, maxForceListForLeg, timeStep)    print(tausright[2])    pb.setJointMotorControl2(robotId, jointIndex=0, controlMode=pb.TORQUE_CONTROL,                                  force=tausright[0])        pb.setJointMotorControl2(robotId, jointIndex=1, controlMode=pb.TORQUE_CONTROL,                                  force=tausright[1])        pb.setJointMotorControl2(robotId, jointIndex=2, controlMode=pb.TORQUE_CONTROL,                                  force=tausright[2])        pb.setJointMotorControl2(robotId, jointIndex=3, controlMode=pb.TORQUE_CONTROL,                                  force=tausright[3])        pb.setJointMotorControl2(robotId, jointIndex=4, controlMode=pb.TORQUE_CONTROL,                                  force=tausright[4])        pb.setJointMotorControl2(robotId, jointIndex=5, controlMode=pb.TORQUE_CONTROL,                                  force=tausright[5])        pb.setJointMotorControl2(robotId, jointIndex=6, controlMode=pb.TORQUE_CONTROL,                                  force=tausleft[0])        pb.setJointMotorControl2(robotId, jointIndex=7, controlMode=pb.TORQUE_CONTROL,                                  force=tausleft[1])        pb.setJointMotorControl2(robotId, jointIndex=8, controlMode=pb.TORQUE_CONTROL,                                  force=tausleft[2])        pb.setJointMotorControl2(robotId, jointIndex=9, controlMode=pb.TORQUE_CONTROL,                                  force=tausleft[3])        pb.setJointMotorControl2(robotId, jointIndex=10, controlMode=pb.TORQUE_CONTROL,                                  force=tausleft[4])         pb.setJointMotorControl2(robotId, jointIndex=11, controlMode=pb.TORQUE_CONTROL,                                  force=tausleft[5])        TorLHY = append(TorLHY,pb.getJointState(robotId, jointIndex=0)[-1])    TorLHR = append(TorLHR,pb.getJointState(robotId, jointIndex=1)[-1])    TorLHP = append(TorLHP,pb.getJointState(robotId, jointIndex=2)[-1])    TorLKP = append(TorLKP,pb.getJointState(robotId, jointIndex=3)[-1])    TorLAP = append(TorLAP,pb.getJointState(robotId, jointIndex=4)[-1])    TorLAR = append(TorLAR,pb.getJointState(robotId, jointIndex=5)[-1])    TorRHY = append(TorRHY,pb.getJointState(robotId, jointIndex=6)[-1])    TorRHR = append(TorRHR,pb.getJointState(robotId, jointIndex=7)[-1])    TorRHP = append(TorRHP,pb.getJointState(robotId, jointIndex=8)[-1])    TorRKP = append(TorRKP,pb.getJointState(robotId, jointIndex=9)[-1])    TorRAP = append(TorRAP,pb.getJointState(robotId, jointIndex=10)[-1])    TorRAR = append(TorRAR,pb.getJointState(robotId, jointIndex=11)[-1])        TorLHY1 = vstack((TorLHY1,pb.getJointState(robotId, jointIndex=0)[-2]))    TorLHR1 = vstack((TorLHR1,pb.getJointState(robotId, jointIndex=1)[-2]))    TorLHP1 = vstack((TorLHP1,pb.getJointState(robotId, jointIndex=2)[-2]))    TorLKP1 = vstack((TorLKP1,pb.getJointState(robotId, jointIndex=3)[-2]))    TorLAP1 = vstack((TorLAP1,pb.getJointState(robotId, jointIndex=4)[-2]))    TorLAR1 = vstack((TorLAR1,pb.getJointState(robotId, jointIndex=5)[-2]))    TorRHY1 = vstack((TorRHY1,pb.getJointState(robotId, jointIndex=6)[-2]))    TorRHR1 = vstack((TorRHR1,pb.getJointState(robotId, jointIndex=7)[-2]))    TorRHP1 = vstack((TorRHP1,pb.getJointState(robotId, jointIndex=8)[-2]))    TorRKP1 = vstack((TorRKP1,pb.getJointState(robotId, jointIndex=9)[-2]))    TorRAP1 = vstack((TorRAP1,pb.getJointState(robotId, jointIndex=10)[-2]))    TorRAR1 = vstack((TorRAR1,pb.getJointState(robotId, jointIndex=11)[-2]))        pb.stepSimulation()    time.sleep(1./960.)  #______________________________________________________________________________    pb.disconnect()

URDF File

Code:

                                                                                                                                transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                    transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1                                                                                                                                  transmission_interface/SimpleTransmission          hardware_interface/EffortJointInterface              hardware_interface/EffortJointInterface      1

Statistics: Posted by exo — Thu Nov 25, 2021 4:38 pm

Real-Time Physics Simulation Forum

TORQUE_CONTROL Not working