diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a0c3c4fd4..2063722c8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1047,6 +1047,7 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh for (int i = 0; i < state->m_uDofSize; i++) { state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex+i]; + state->m_jointMotorTorqueMultiDof[i] = status->m_sendActualStateArgs.m_jointMotorForceMultiDof[info.m_uIndex + i]; } } else @@ -1058,7 +1059,7 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh { state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii]; } - state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex]; + return 1; } } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9b5f34e14..e60828649 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6425,6 +6425,32 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc } for (int d = 0; d < mb->getLink(l).m_dofCount; d++) { + serverCmd.m_sendActualStateArgs.m_jointMotorForce[totalDegreeOfFreedomU] = 0; + + if (mb->getLink(l).m_jointType == btMultibodyLink::eSpherical) + { + btMultiBodySphericalJointMotor* motor = (btMultiBodySphericalJointMotor*)mb->getLink(l).m_userPtr; + if (motor) + { + btScalar impulse = motor->getAppliedImpulse(d); + btScalar force = impulse / m_data->m_physicsDeltaTime; + serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; + } + } + else + { + if (supportsJointMotor(mb, l)) + { + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr; + + if (motor && m_data->m_physicsDeltaTime > btScalar(0)) + { + btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime; + serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; + } + } + } + serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 1c2b9e00b..b16364295 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -531,6 +531,7 @@ struct SendActualStateArgs double m_jointReactionForces[6 * MAX_DEGREE_OF_FREEDOM]; double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM]; + double m_jointMotorForceMultiDof[MAX_DEGREE_OF_FREEDOM]; double m_linkState[7 * MAX_NUM_LINKS]; double m_linkWorldVelocities[6 * MAX_NUM_LINKS]; //linear velocity and angular velocity in world space (x/y/z each). diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 86e218372..53d29c732 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -369,7 +369,7 @@ struct b3JointSensorState2 double m_jointPosition[4]; double m_jointVelocity[3]; double m_jointReactionForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ - double m_jointMotorTorque; + double m_jointMotorTorqueMultiDof[3]; int m_qDofSize; int m_uDofSize; }; diff --git a/examples/pybullet/examples/humanoidMotionCapture.py b/examples/pybullet/examples/humanoidMotionCapture.py index 3300e121c..852fcda4f 100644 --- a/examples/pybullet/examples/humanoidMotionCapture.py +++ b/examples/pybullet/examples/humanoidMotionCapture.py @@ -1,4 +1,4 @@ -import pybullet as p +import pybullet as p import json import time @@ -10,56 +10,57 @@ else: useZUp = False useYUp = not useZUp +showJointMotorTorques = False if useYUp: - p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1) + p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1) -from pdControllerExplicit import PDControllerExplicitMultiDof -from pdControllerStable import PDControllerStableMultiDof +from pdControllerExplicit import PDControllerExplicitMultiDof +from pdControllerStable import PDControllerStableMultiDof explicitPD = PDControllerExplicitMultiDof(p) stablePD = PDControllerStableMultiDof(p) -p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) +p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) import pybullet_data p.setTimeOut(10000) useMotionCapture=False -useMotionCaptureReset=False#not useMotionCapture -useExplicitPD = True +useMotionCaptureReset=False#not useMotionCapture +useExplicitPD = True p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=30) #p.setPhysicsEngineParameter(solverResidualThreshold=1e-30) -#explicit PD control requires small timestep +#explicit PD control requires small timestep timeStep = 1./600. -#timeStep = 1./240. +#timeStep = 1./240. p.setPhysicsEngineParameter(fixedTimeStep=timeStep) path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt" -#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt" -#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt" +#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt" +#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt" #p.loadURDF("plane.urdf",[0,0,-1.03]) -print("path = ", path) -with open(path, 'r') as f: - motion_dict = json.load(f) -#print("motion_dict = ", motion_dict) +print("path = ", path) +with open(path, 'r') as f: + motion_dict = json.load(f) +#print("motion_dict = ", motion_dict) print("len motion=", len(motion_dict)) print(motion_dict['Loop']) -numFrames = len(motion_dict['Frames']) -print("#frames = ", numFrames) +numFrames = len(motion_dict['Frames']) +print("#frames = ", numFrames) frameId= p.addUserDebugParameter("frame",0,numFrames-1,0) -erpId = p.addUserDebugParameter("erp",0,1,0.2) +erpId = p.addUserDebugParameter("erp",0,1,0.2) -kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2) +kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2) forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000) @@ -70,23 +71,23 @@ jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC", startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]] -p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0]) -p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0]) -p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0]) -p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0]) +p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0]) +p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0]) +p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0]) +p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0]) -humanoid = p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) -humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) -humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) -humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) +humanoid = p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) +humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) +humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) +humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1]) -humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1]) -humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1]) -humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1]) +humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1]) +humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1]) +humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1]) -startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447, +startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447, 1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153, 0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495, 0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348] @@ -95,7 +96,7 @@ startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,- -0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0, -4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973] -p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1]) +p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1]) p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1]) p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1]) p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1]) @@ -103,42 +104,42 @@ p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1]) index0=7 -for j in range (p.getNumJoints(humanoid)): +for j in range (p.getNumJoints(humanoid)): ji = p.getJointInfo(humanoid,j) targetPosition=[0] - jointType = ji[2] - if (jointType == p.JOINT_SPHERICAL): + jointType = ji[2] + if (jointType == p.JOINT_SPHERICAL): targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]] - targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]] + targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]] index0+=4 print("spherical position: ",targetPosition) print("spherical velocity: ",targetVel) p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel) - if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): + if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): targetPosition=[startPose[index0]] - targetVel = [startVel[index0]] + targetVel = [startVel[index0]] index0+=1 print("revolute:", targetPosition) - print("revolute velocity:", targetVel) + print("revolute velocity:", targetVel) p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel) -for j in range (p.getNumJoints(humanoid)): +for j in range (p.getNumJoints(humanoid)): ji = p.getJointInfo(humanoid,j) targetPosition=[0] - jointType = ji[2] - if (jointType == p.JOINT_SPHERICAL): + jointType = ji[2] + if (jointType == p.JOINT_SPHERICAL): targetPosition=[0,0,0,1] p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[0,0,0]) - p.setJointMotorControlMultiDof(humanoid3,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[31,31,31]) - p.setJointMotorControlMultiDof(humanoid4,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[1,1,1]) + p.setJointMotorControlMultiDof(humanoid3,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[31,31,31]) + p.setJointMotorControlMultiDof(humanoid4,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[1,1,1]) - if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): - p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0) + if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): + p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0) p.setJointMotorControl2(humanoid3,j,p.VELOCITY_CONTROL,targetVelocity=0, force=31) p.setJointMotorControl2(humanoid4,j,p.VELOCITY_CONTROL,targetVelocity=0, force=10) @@ -150,18 +151,18 @@ for j in range (p.getNumJoints(humanoid)): jointIds=[] paramIds=[] -for j in range (p.getNumJoints(humanoid)): - #p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0) +for j in range (p.getNumJoints(humanoid)): + #p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0) p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1]) info = p.getJointInfo(humanoid,j) #print(info) - if (not useMotionCapture): - jointName = info[1] - jointType = info[2] - if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): + if (not useMotionCapture): + jointName = info[1] + jointType = info[2] + if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): jointIds.append(j) #paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0)) - #print("jointName=",jointName, "at ", j) + #print("jointName=",jointName, "at ", j) p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1]) chest=1 @@ -181,7 +182,7 @@ leftElbow=13 #rightElbow=4 #leftShoulder=6 #leftElbow = 7 -#rightHip = 9 +#rightHip = 9 #rightKnee=10 #rightAnkle=11 #leftHip = 12 @@ -191,8 +192,8 @@ leftElbow=13 import time -kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300] -kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30] +kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300] +kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30] once=True p.getCameraImage(320,200) @@ -200,105 +201,105 @@ p.getCameraImage(320,200) -while (p.isConnected()): +while (p.isConnected()): if useGUI: - erp = p.readUserDebugParameter(erpId) - kpMotor = p.readUserDebugParameter(kpMotorId) + erp = p.readUserDebugParameter(erpId) + kpMotor = p.readUserDebugParameter(kpMotorId) maxForce=p.readUserDebugParameter(forceMotorId) - frameReal = p.readUserDebugParameter(frameId) + frameReal = p.readUserDebugParameter(frameId) else: - erp = 0.2 - kpMotor = 0.2 + erp = 0.2 + kpMotor = 0.2 maxForce=1000 - frameReal = 0 + frameReal = 0 kp=kpMotor - frame = int(frameReal) - frameNext = frame+1 - if (frameNext >= numFrames): - frameNext = frame + frame = int(frameReal) + frameNext = frame+1 + if (frameNext >= numFrames): + frameNext = frame - frameFraction = frameReal - frame + frameFraction = frameReal - frame #print("frameFraction=",frameFraction) #print("frame=",frame) #print("frameNext=", frameNext) #getQuaternionSlerp - frameData = motion_dict['Frames'][frame] - frameDataNext = motion_dict['Frames'][frameNext] + frameData = motion_dict['Frames'][frame] + frameDataNext = motion_dict['Frames'][frameNext] #print("duration=",frameData[0]) #print(pos=[frameData]) - basePos1Start = [frameData[1],frameData[2],frameData[3]] - basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]] - basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), - basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), + basePos1Start = [frameData[1],frameData[2],frameData[3]] + basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]] + basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), + basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])] - baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] + baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]] baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction) - #pre-rotate to make z-up + #pre-rotate to make z-up if (useZUp): y2zPos=[0,0,0.0] y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) - basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) - p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn) + basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) + p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn) y2zPos=[0,2,0.0] y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) - basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) + basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn) - chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] - chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]] + chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] + chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]] chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction) neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]] - neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]] - neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction) + neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]] + neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction) rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]] rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]] - rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction) + rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction) - rightKneeRotStart = [frameData[20]] - rightKneeRotEnd = [frameDataNext[20]] + rightKneeRotStart = [frameData[20]] + rightKneeRotEnd = [frameDataNext[20]] rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])] rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]] rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]] - rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction) + rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction) - rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]] - rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]] + rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]] + rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]] rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction) rightElbowRotStart = [frameData[29]] rightElbowRotEnd = [frameDataNext[29]] - rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])] + rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])] - leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] - leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]] + leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] + leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]] leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction) leftKneeRotStart = [frameData[34]] leftKneeRotEnd = [frameDataNext[34]] - leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ] + leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ] - leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] - leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]] + leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] + leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]] leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction) leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]] - leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction) - leftElbowRotStart = [frameData[43]] - leftElbowRotEnd = [frameDataNext[43]] + leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction) + leftElbowRotStart = [frameData[43]] + leftElbowRotEnd = [frameDataNext[43]] leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])] if (0):#if (once): @@ -309,11 +310,11 @@ while (p.isConnected()): p.resetJointStateMultiDof(humanoid,rightAnkle,rightAnkleRot) p.resetJointStateMultiDof(humanoid,rightShoulder,rightShoulderRot) p.resetJointStateMultiDof(humanoid,rightElbow, rightElbowRot) - p.resetJointStateMultiDof(humanoid,leftHip, leftHipRot) + p.resetJointStateMultiDof(humanoid,leftHip, leftHipRot) p.resetJointStateMultiDof(humanoid,leftKnee, leftKneeRot) - p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot) + p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot) p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot) - p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot) + p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot) once=False #print("chestRot=",chestRot) p.setGravity(0,0,-10) @@ -321,14 +322,14 @@ while (p.isConnected()): kp=kpMotor if (useExplicitPD): jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1] - #[x,y,z] base position and [x,y,z,w] base orientation! - totalDofs =7 - for dof in jointDofCounts: - totalDofs += dof + #[x,y,z] base position and [x,y,z,w] base orientation! + totalDofs =7 + for dof in jointDofCounts: + totalDofs += dof - jointIndicesAll = [ + jointIndicesAll = [ chest, - neck, + neck, rightHip, rightKnee, rightAnkle, @@ -340,7 +341,7 @@ while (p.isConnected()): leftShoulder, leftElbow ] - basePos,baseOrn = p.getBasePositionAndOrientation(humanoid) + basePos,baseOrn = p.getBasePositionAndOrientation(humanoid) pose = [ basePos[0],basePos[1],basePos[2], baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3], chestRot[0],chestRot[1],chestRot[2],chestRot[3], @@ -358,31 +359,31 @@ while (p.isConnected()): #print("pose=") - #for po in pose: + #for po in pose: # print(po) taus = stablePD.computePD(bodyUniqueId=humanoid, jointIndices = jointIndicesAll, desiredPositions = pose, - desiredVelocities = [0]*totalDofs, - kps = kpOrg, - kds = kdOrg, - maxForces = [maxForce]*totalDofs, + desiredVelocities = [0]*totalDofs, + kps = kpOrg, + kds = kdOrg, + maxForces = [maxForce]*totalDofs, timeStep=timeStep) - taus3 = explicitPD.computePD(bodyUniqueId=humanoid3, + taus3 = explicitPD.computePD(bodyUniqueId=humanoid3, jointIndices = jointIndicesAll, desiredPositions = pose, - desiredVelocities = [0]*totalDofs, - kps = kpOrg, - kds = kdOrg, - maxForces = [maxForce*0.05]*totalDofs, + desiredVelocities = [0]*totalDofs, + kps = kpOrg, + kds = kdOrg, + maxForces = [maxForce*0.05]*totalDofs, timeStep=timeStep) #taus=[0]*43 dofIndex=7 - for index in range (len(jointIndicesAll)): + for index in range (len(jointIndicesAll)): jointIndex = jointIndicesAll[index] if jointDofCounts[index]==4: @@ -392,7 +393,7 @@ while (p.isConnected()): if jointDofCounts[index]==1: p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]]) - p.setJointMotorControlMultiDof(humanoid3, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus3[dofIndex]]) + p.setJointMotorControlMultiDof(humanoid3, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus3[dofIndex]]) dofIndex+=jointDofCounts[index] @@ -400,18 +401,18 @@ while (p.isConnected()): #print("taus=",taus) - p.setJointMotorControlMultiDof(humanoid2,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce]) + p.setJointMotorControlMultiDof(humanoid2,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce]) p.setJointMotorControlMultiDof(humanoid2,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce]) p.setJointMotorControlMultiDof(humanoid2,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce]) p.setJointMotorControlMultiDof(humanoid2,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce]) p.setJointMotorControlMultiDof(humanoid2,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce]) p.setJointMotorControlMultiDof(humanoid2,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce]) - p.setJointMotorControlMultiDof(humanoid2,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce]) - p.setJointMotorControlMultiDof(humanoid2,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce]) - p.setJointMotorControlMultiDof(humanoid2,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce]) - p.setJointMotorControlMultiDof(humanoid2,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce]) - p.setJointMotorControlMultiDof(humanoid2,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce]) - p.setJointMotorControlMultiDof(humanoid2,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce]) + p.setJointMotorControlMultiDof(humanoid2,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce]) + p.setJointMotorControlMultiDof(humanoid2,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce]) + p.setJointMotorControlMultiDof(humanoid2,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce]) + p.setJointMotorControlMultiDof(humanoid2,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce]) + p.setJointMotorControlMultiDof(humanoid2,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce]) + p.setJointMotorControlMultiDof(humanoid2,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce]) kinematicHumanoid4 = True if (kinematicHumanoid4): @@ -421,11 +422,18 @@ while (p.isConnected()): p.resetJointStateMultiDof(humanoid4,rightKnee,rightKneeRot) p.resetJointStateMultiDof(humanoid4,rightAnkle,rightAnkleRot) p.resetJointStateMultiDof(humanoid4,rightShoulder,rightShoulderRot) - p.resetJointStateMultiDof(humanoid4,rightElbow, rightElbowRot) + p.resetJointStateMultiDof(humanoid4,rightElbow, rightElbowRot) p.resetJointStateMultiDof(humanoid4,leftHip, leftHipRot) - p.resetJointStateMultiDof(humanoid4,leftKnee, leftKneeRot) + p.resetJointStateMultiDof(humanoid4,leftKnee, leftKneeRot) p.resetJointStateMultiDof(humanoid4,leftAnkle, leftAnkleRot) - p.resetJointStateMultiDof(humanoid4,leftShoulder, leftShoulderRot) + p.resetJointStateMultiDof(humanoid4,leftShoulder, leftShoulderRot) p.resetJointStateMultiDof(humanoid4,leftElbow, leftElbowRot) p.stepSimulation() + + if showJointMotorTorques: + for j in range(p.getNumJoints(humanoid2)): + jointState = p.getJointStateMultiDof(humanoid2,j) + print("jointStateMultiDof[",j,"].pos=",jointState[0]) + print("jointStateMultiDof[",j,"].vel=",jointState[1]) + print("jointStateMultiDof[",j,"].jointForces=",jointState[3]) time.sleep(timeStep) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b0d72691e..9f226729e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4235,6 +4235,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args, PyObject* pyListJointState; PyObject* pyListPosition; PyObject* pyListVelocity; + PyObject* pyListJointMotorTorque; struct b3JointSensorState2 sensorState; @@ -4296,7 +4297,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args, int i = 0; pyListPosition = PyTuple_New(sensorState.m_qDofSize); pyListVelocity = PyTuple_New(sensorState.m_uDofSize); - + pyListJointMotorTorque = PyTuple_New(sensorState.m_uDofSize); PyTuple_SetItem(pyListJointState, 0, pyListPosition); PyTuple_SetItem(pyListJointState, 1, pyListVelocity); @@ -4310,6 +4311,9 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args, { PyTuple_SetItem(pyListVelocity, i, PyFloat_FromDouble(sensorState.m_jointVelocity[i])); + + PyTuple_SetItem(pyListJointMotorTorque, i, + PyFloat_FromDouble(sensorState.m_jointMotorTorqueMultiDof[i])); } for (j = 0; j < forceTorqueSize; j++) @@ -4320,8 +4324,8 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args, PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); - PyTuple_SetItem(pyListJointState, 3, - PyFloat_FromDouble(sensorState.m_jointMotorTorque)); + PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque); + return pyListJointState; }