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

Merge pull request #2130 from erwincoumans/master

Browse Source 
Expose motor drive torque reporting for motors in spherical joints
This commit is contained in:
erwincoumans
2019-02-27 11:53:06 -08:00
committed by GitHub
parent ebc079c32c 2ae03f50b1
commit dfa66864a3
6 changed files with 175 additions and 135 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++)
{
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;
}
}

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++)
{
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];
}

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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).

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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;
};

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

@@ -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)

10
examples/pybullet/pybullet.c
View File

@@ -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;
}