Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Expose motor drive torque reporting for motors in spherical joints in getJointStateMultiDof.

Browse Source
This commit is contained in:
erwincoumans
2019-02-27 09:54:12 -08:00
parent 09dbb8ba1b
commit 8e1c1448ab
6 changed files with 291 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1047,6 +1047,7 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh
for (int i = 0; i < state->m_uDofSize; i++) for (int i = 0; i < state->m_uDofSize; i++)
{ {
state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex+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 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_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
} }
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1; return 1;
} }
} }

26
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6425,6 +6425,32 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
} }
for (int d = 0; d < mb->getLink(l).m_dofCount; d++) 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]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
} }

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -531,6 +531,7 @@ struct SendActualStateArgs
double m_jointReactionForces[6 * MAX_DEGREE_OF_FREEDOM]; double m_jointReactionForces[6 * MAX_DEGREE_OF_FREEDOM];
double m_jointMotorForce[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_linkState[7 * MAX_NUM_LINKS];
double m_linkWorldVelocities[6 * MAX_NUM_LINKS]; //linear velocity and angular velocity in world space (x/y/z each). double m_linkWorldVelocities[6 * MAX_NUM_LINKS]; //linear velocity and angular velocity in world space (x/y/z each).

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -369,7 +369,7 @@ struct b3JointSensorState2
double m_jointPosition[4]; double m_jointPosition[4];
double m_jointVelocity[3]; 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_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_qDofSize;
int m_uDofSize; int m_uDofSize;
}; };

548
examples/pybullet/examples/humanoidMotionCapture.py
View File

@@ -4,20 +4,20 @@ import time
useGUI = True useGUI = True
if useGUI: if useGUI:
p.connect(p.GUI) p.connect(p.GUI)
else: else:
p.connect(p.DIRECT) p.connect(p.DIRECT)
useZUp = False useZUp = False
useYUp = not useZUp useYUp = not useZUp
if useYUp: 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 pdControllerStable import PDControllerStableMultiDof
explicitPD = PDControllerExplicitMultiDof(p)
stablePD = PDControllerStableMultiDof(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])
@@ -26,7 +26,7 @@ import pybullet_data
p.setTimeOut(10000) p.setTimeOut(10000)
useMotionCapture=False useMotionCapture=False
useMotionCaptureReset=False#not useMotionCapture useMotionCaptureReset=False#not useMotionCapture
useExplicitPD = True
p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(numSolverIterations=30) p.setPhysicsEngineParameter(numSolverIterations=30)
@@ -66,8 +66,8 @@ forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC", jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"] "JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]] 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("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0])
@@ -76,14 +76,8 @@ p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,s
p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][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) 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]) 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])
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,
@@ -96,73 +90,61 @@ startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-
-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973] -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])
index0=7 index0=7
for j in range (p.getNumJoints(humanoid)): for j in range (p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid,j) ji = p.getJointInfo(humanoid,j)
targetPosition=[0] targetPosition=[0]
jointType = ji[2] jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL): if (jointType == p.JOINT_SPHERICAL):
targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]] 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 index0+=4
print("spherical position: ",targetPosition) print("spherical position: ",targetPosition)
print("spherical velocity: ",targetVel) print("spherical velocity: ",targetVel)
p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=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]]
targetPosition=[startPose[index0]] targetVel = [startVel[index0]]
targetVel = [startVel[index0]] index0+=1
index0+=1 print("revolute:", targetPosition)
print("revolute:", targetPosition) print("revolute velocity:", targetVel)
print("revolute velocity:", targetVel) p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=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) ji = p.getJointInfo(humanoid,j)
targetPosition=[0] targetPosition=[0]
jointType = ji[2] jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL): if (jointType == p.JOINT_SPHERICAL):
targetPosition=[0,0,0,1] 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(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])
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0) 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) #print(ji)
print("joint[",j,"].type=",jointTypes[ji[2]])
#print(ji) print("joint[",j,"].name=",ji[1])
print("joint[",j,"].type=",jointTypes[ji[2]])
print("joint[",j,"].name=",ji[1])
jointIds=[] jointIds=[]
paramIds=[] paramIds=[]
for j in range (p.getNumJoints(humanoid)): for j in range (p.getNumJoints(humanoid)):
#p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0) #p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1]) p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1])
info = p.getJointInfo(humanoid,j) info = p.getJointInfo(humanoid,j)
#print(info) #print(info)
if (not useMotionCapture): if (not useMotionCapture):
jointName = info[1] jointName = info[1]
jointType = info[2] jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j) jointIds.append(j)
#paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0)) #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]) p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1])
chest=1 chest=1
neck=2 neck=2
@@ -199,233 +181,211 @@ p.getCameraImage(320,200)
while (p.isConnected()): while (p.isConnected()):
if useGUI: if useGUI:
erp = p.readUserDebugParameter(erpId) erp = p.readUserDebugParameter(erpId)
kpMotor = p.readUserDebugParameter(kpMotorId) kpMotor = p.readUserDebugParameter(kpMotorId)
maxForce=p.readUserDebugParameter(forceMotorId) maxForce=p.readUserDebugParameter(forceMotorId)
frameReal = p.readUserDebugParameter(frameId) frameReal = p.readUserDebugParameter(frameId)
else: else:
erp = 0.2 erp = 0.2
kpMotor = 0.2 kpMotor = 0.2
maxForce=1000 maxForce=1000
frameReal = 0 frameReal = 0
kp=kpMotor kp=kpMotor
frame = int(frameReal) frame = int(frameReal)
frameNext = frame+1 frameNext = frame+1
if (frameNext >= numFrames): if (frameNext >= numFrames):
frameNext = frame frameNext = frame
frameFraction = frameReal - frame frameFraction = frameReal - frame
#print("frameFraction=",frameFraction)
#print("frame=",frame) frameData = motion_dict['Frames'][frame]
#print("frameNext=", frameNext) frameDataNext = motion_dict['Frames'][frameNext]
#getQuaternionSlerp #print("duration=",frameData[0])
#print(pos=[frameData])
frameData = motion_dict['Frames'][frame]
frameDataNext = motion_dict['Frames'][frameNext] basePos1Start = [frameData[1],frameData[2],frameData[3]]
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
#print("duration=",frameData[0]) basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
#print(pos=[frameData]) basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
basePos1Start = [frameData[1],frameData[2],frameData[3]] baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]] baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), #pre-rotate to make z-up
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]] if (useZUp):
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction) y2zPos=[0,0,0.0]
#pre-rotate to make z-up y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
if (useZUp): basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
y2zPos=[0,0,0.0] p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn) once=False
#print("chestRot=",chestRot)
y2zPos=[0,2,0.0] p.setGravity(0,0,0)
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) kp=kpMotor
p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn)
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]] basePos1Start = [frameData[1],frameData[2],frameData[3]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]] basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction) basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]] basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]] baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction) baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
rightKneeRotStart = [frameData[20]] #pre-rotate to make z-up
rightKneeRotEnd = [frameDataNext[20]] if (useZUp):
rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])] y2zPos=[0,0,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]] basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]] p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]] chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]] chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
rightElbowRotStart = [frameData[29]] neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
rightElbowRotEnd = [frameDataNext[29]] neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])]
rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]] rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
rightKneeRotStart = [frameData[20]]
leftKneeRotStart = [frameData[34]] rightKneeRotEnd = [frameDataNext[20]]
leftKneeRotEnd = [frameDataNext[34]] rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])]
leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ]
rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]] rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]] rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
leftElbowRotStart = [frameData[43]] rightElbowRotStart = [frameData[29]]
leftElbowRotEnd = [frameDataNext[43]] rightElbowRotEnd = [frameDataNext[29]]
leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])] rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])]
if (0):#if (once): leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
p.resetJointStateMultiDof(humanoid,chest,chestRot) leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
p.resetJointStateMultiDof(humanoid,neck,neckRot) leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
p.resetJointStateMultiDof(humanoid,rightKnee,rightKneeRot) leftKneeRotStart = [frameData[34]]
p.resetJointStateMultiDof(humanoid,rightAnkle,rightAnkleRot) leftKneeRotEnd = [frameDataNext[34]]
p.resetJointStateMultiDof(humanoid,rightShoulder,rightShoulderRot) leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ]
p.resetJointStateMultiDof(humanoid,rightElbow, rightElbowRot)
p.resetJointStateMultiDof(humanoid,leftHip, leftHipRot) leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
p.resetJointStateMultiDof(humanoid,leftKnee, leftKneeRot) leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot) leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot)
p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot) leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
once=False leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
#print("chestRot=",chestRot) leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
p.setGravity(0,0,-10) leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]]
kp=kpMotor leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
if (useExplicitPD):
jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1] once=False
#[x,y,z] base position and [x,y,z,w] base orientation!
totalDofs =7
for dof in jointDofCounts:
totalDofs += dof kp=kpMotor
jointIndicesAll = [ basePos,baseOrn = p.getBasePositionAndOrientation(humanoid)
chest, pose = [ basePos[0],basePos[1],basePos[2],
neck, baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],
rightHip, chestRot[0],chestRot[1],chestRot[2],chestRot[3],
rightKnee, neckRot[0],neckRot[1],neckRot[2],neckRot[3],
rightAnkle, rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3],
rightShoulder, rightKneeRot[0],
rightElbow, rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3],
leftHip, rightShoulderRot[0],rightShoulderRot[1],rightShoulderRot[2],rightShoulderRot[3],
leftKnee, rightElbowRot[0],
leftAnkle, leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3],
leftShoulder, leftKneeRot[0],
leftElbow leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3],
] leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3],
basePos,baseOrn = p.getBasePositionAndOrientation(humanoid) leftElbowRot[0] ]
pose = [ basePos[0],basePos[1],basePos[2],
baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3], jointIndicesAll = [
chestRot[0],chestRot[1],chestRot[2],chestRot[3], chest,
neckRot[0],neckRot[1],neckRot[2],neckRot[3], neck,
rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3], rightHip,
rightKneeRot[0], rightKnee,
rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3], rightAnkle,
rightShoulderRot[0],rightShoulderRot[1],rightShoulderRot[2],rightShoulderRot[3], rightShoulder,
rightElbowRot[0], rightElbow,
leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3], leftHip,
leftKneeRot[0], leftKnee,
leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3], leftAnkle,
leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3], leftShoulder,
leftElbowRot[0] ] leftElbow
]
dofIndex=7
#print("pose=") jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
#for po in pose: totalDofs = sum(jointDofCounts)+7
# print(po) print("total=",totalDofs)
useStablePD=False
taus = stablePD.computePD(bodyUniqueId=humanoid, if (useStablePD):
jointIndices = jointIndicesAll, taus = stablePD.computePD(bodyUniqueId=humanoid,
desiredPositions = pose, jointIndices = jointIndicesAll,
desiredVelocities = [0]*totalDofs, desiredPositions = pose,
kps = kpOrg, desiredVelocities = [0]*totalDofs,
kds = kdOrg, kps = kpOrg,
maxForces = [maxForce]*totalDofs, kds = kdOrg,
timeStep=timeStep) maxForces = [maxForce]*totalDofs,
timeStep=timeStep)
taus3 = explicitPD.computePD(bodyUniqueId=humanoid3,
jointIndices = jointIndicesAll,
desiredPositions = pose, #taus=[0]*43
desiredVelocities = [0]*totalDofs,
kps = kpOrg,
kds = kdOrg,
maxForces = [maxForce*0.05]*totalDofs,
timeStep=timeStep)
#taus=[0]*43 for index in range (len(jointIndicesAll)):
dofIndex=7 jointIndex = jointIndicesAll[index]
for index in range (len(jointIndicesAll)): if jointDofCounts[index]==4:
jointIndex = jointIndicesAll[index] p.setJointMotorControlMultiDof(humanoid,jointIndex,p.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]])
if jointDofCounts[index]==4: if jointDofCounts[index]==1:
p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]])
p.setJointMotorControlMultiDof(humanoid,jointIndex,p.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]]) dofIndex+=jointDofCounts[index]
p.setJointMotorControlMultiDof(humanoid3,jointIndex,p.TORQUE_CONTROL,force=[taus3[dofIndex+0],taus3[dofIndex+1],taus3[dofIndex+2]]) else:
p.setJointMotorControlMultiDof(humanoid,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce])
if jointDofCounts[index]==1: p.setJointMotorControlMultiDof(humanoid,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce])
p.setJointMotorControlMultiDof(humanoid,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce])
p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]]) p.setJointMotorControlMultiDof(humanoid,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce])
p.setJointMotorControlMultiDof(humanoid3, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus3[dofIndex]]) p.setJointMotorControlMultiDof(humanoid,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce])
p.setJointMotorControlMultiDof(humanoid,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce])
dofIndex+=jointDofCounts[index] p.setJointMotorControlMultiDof(humanoid,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce])
p.setJointMotorControlMultiDof(humanoid,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce])
#print("len(taus)=",len(taus)) p.setJointMotorControlMultiDof(humanoid,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce])
#print("taus=",taus) p.setJointMotorControlMultiDof(humanoid,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce])
p.setJointMotorControlMultiDof(humanoid,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce])
p.setJointMotorControlMultiDof(humanoid,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce]) p.stepSimulation()
p.setJointMotorControlMultiDof(humanoid2,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce]) for j in range(p.getNumJoints(humanoid)):
p.setJointMotorControlMultiDof(humanoid2,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce]) jointState = p.getJointStateMultiDof(humanoid,j)
p.setJointMotorControlMultiDof(humanoid2,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce]) print("jointStateMultiDof[",j,"].pos=",jointState[0])
p.setJointMotorControlMultiDof(humanoid2,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce]) print("jointStateMultiDof[",j,"].vel=",jointState[1])
p.setJointMotorControlMultiDof(humanoid2,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce]) print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
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]) #j=4
p.setJointMotorControlMultiDof(humanoid2,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce]) #jointState = p.getJointStateMultiDof(humanoid,j)
p.setJointMotorControlMultiDof(humanoid2,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce]) #print("jointStateMultiDof[",j,"].pos=",jointState[0])
p.setJointMotorControlMultiDof(humanoid2,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce]) #print("jointStateMultiDof[",j,"].vel=",jointState[1])
p.setJointMotorControlMultiDof(humanoid2,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce]) #print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
time.sleep(timeStep)
kinematicHumanoid4 = True
if (kinematicHumanoid4):
p.resetJointStateMultiDof(humanoid4,chest,chestRot)
p.resetJointStateMultiDof(humanoid4,neck,neckRot)
p.resetJointStateMultiDof(humanoid4,rightHip,rightHipRot)
p.resetJointStateMultiDof(humanoid4,rightKnee,rightKneeRot)
p.resetJointStateMultiDof(humanoid4,rightAnkle,rightAnkleRot)
p.resetJointStateMultiDof(humanoid4,rightShoulder,rightShoulderRot)
p.resetJointStateMultiDof(humanoid4,rightElbow, rightElbowRot)
p.resetJointStateMultiDof(humanoid4,leftHip, leftHipRot)
p.resetJointStateMultiDof(humanoid4,leftKnee, leftKneeRot)
p.resetJointStateMultiDof(humanoid4,leftAnkle, leftAnkleRot)
p.resetJointStateMultiDof(humanoid4,leftShoulder, leftShoulderRot)
p.resetJointStateMultiDof(humanoid4,leftElbow, leftElbowRot)
p.stepSimulation()
time.sleep(timeStep)

10
examples/pybullet/pybullet.c
View File

@@ -4235,6 +4235,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
PyObject* pyListJointState; PyObject* pyListJointState;
PyObject* pyListPosition; PyObject* pyListPosition;
PyObject* pyListVelocity; PyObject* pyListVelocity;
PyObject* pyListJointMotorTorque;
struct b3JointSensorState2 sensorState; struct b3JointSensorState2 sensorState;
@@ -4296,7 +4297,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
int i = 0; int i = 0;
pyListPosition = PyTuple_New(sensorState.m_qDofSize); pyListPosition = PyTuple_New(sensorState.m_qDofSize);
pyListVelocity = PyTuple_New(sensorState.m_uDofSize); pyListVelocity = PyTuple_New(sensorState.m_uDofSize);
pyListJointMotorTorque = PyTuple_New(sensorState.m_uDofSize);
PyTuple_SetItem(pyListJointState, 0, pyListPosition); PyTuple_SetItem(pyListJointState, 0, pyListPosition);
PyTuple_SetItem(pyListJointState, 1, pyListVelocity); PyTuple_SetItem(pyListJointState, 1, pyListVelocity);
@@ -4310,6 +4311,9 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
{ {
PyTuple_SetItem(pyListVelocity, i, PyTuple_SetItem(pyListVelocity, i,
PyFloat_FromDouble(sensorState.m_jointVelocity[i])); PyFloat_FromDouble(sensorState.m_jointVelocity[i]));
PyTuple_SetItem(pyListJointMotorTorque, i,
PyFloat_FromDouble(sensorState.m_jointMotorTorqueMultiDof[i]));
} }
for (j = 0; j < forceTorqueSize; j++) 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, 2, pyListJointForceTorque);
PyTuple_SetItem(pyListJointState, 3, PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque);
PyFloat_FromDouble(sensorState.m_jointMotorTorque));
return pyListJointState; return pyListJointState;
} }