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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans
2019-06-13 23:24:42 -07:00
parent 6160b52fd7 8f9fac99bd
commit 95a79800ec
1 changed files with 56 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py vendored
View File

@@ -7,6 +7,7 @@ import time
import motion_capture_data import motion_capture_data
import quadrupedPoseInterpolator import quadrupedPoseInterpolator
useKinematic = False
useConstraints = False useConstraints = False
p = bullet_client.BulletClient(connection_mode=p1.GUI) p = bullet_client.BulletClient(connection_mode=p1.GUI)
@@ -168,52 +169,63 @@ while t < 10. * cycleTime:
p.changeConstraint(cid, maxForce=maxUpForce) p.changeConstraint(cid, maxForce=maxUpForce)
if useConstraints: if useKinematic:
basePos = startPos
basePos = [float(-jointsStr[0]),-float(jointsStr[1]),float(jointsStr[2])]
baseOrn = [float(jointsStr[4]),float(jointsStr[5]),float(jointsStr[6]), float(jointsStr[3])]
p.resetBasePositionAndOrientation(quadruped, basePos,baseOrn)
for j in range(12): for j in range(12):
#skip the base positional dofs #skip the base positional dofs
targetPos = float(jointsStr[j + 7]) targetPos = float(jointsStr[j + 7])
p.setJointMotorControl2(quadruped, p.resetJointState(quadruped,jointIds[j],jointDirections[j] * targetPos + jointOffsets[j])
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos + jointOffsets[j],
force=maxForce)
else: else:
desiredPositions = [] if useConstraints:
for j in range(7): for j in range(12):
targetPosUnmodified = float(jointsStr[j]) #skip the base positional dofs
desiredPositions.append(targetPosUnmodified) targetPos = float(jointsStr[j + 7])
for j in range(12): p.setJointMotorControl2(quadruped,
targetPosUnmodified = float(jointsStr[j + 7]) jointIds[j],
targetPos = jointDirections[j] * targetPosUnmodified + jointOffsets[j] p.POSITION_CONTROL,
desiredPositions.append(targetPos) jointDirections[j] * targetPos + jointOffsets[j],
numBaseDofs = 6 force=maxForce)
totalDofs = 12 + numBaseDofs
desiredVelocities = None else:
if desiredVelocities == None: desiredPositions = []
desiredVelocities = [0] * totalDofs for j in range(7):
taus = stablePD.computePD(bodyUniqueId=quadruped, targetPosUnmodified = float(jointsStr[j])
jointIndices=jointIds, desiredPositions.append(targetPosUnmodified)
desiredPositions=desiredPositions, for j in range(12):
desiredVelocities=desiredVelocities, targetPosUnmodified = float(jointsStr[j + 7])
kps=[4000] * totalDofs, targetPos = jointDirections[j] * targetPosUnmodified + jointOffsets[j]
kds=[40] * totalDofs, desiredPositions.append(targetPos)
maxForces=[maxForce] * totalDofs, numBaseDofs = 6
timeStep=timeStep) totalDofs = 12 + numBaseDofs
desiredVelocities = None
dofIndex = 6 if desiredVelocities == None:
scaling = 1 desiredVelocities = [0] * totalDofs
for index in range(len(jointIds)): taus = stablePD.computePD(bodyUniqueId=quadruped,
jointIndex = jointIds[index] jointIndices=jointIds,
force = [scaling * taus[dofIndex]] desiredPositions=desiredPositions,
#print("force[", jointIndex, "]=", force) desiredVelocities=desiredVelocities,
p.setJointMotorControlMultiDof(quadruped, kps=[4000] * totalDofs,
jointIndex, kds=[40] * totalDofs,
controlMode=p.TORQUE_CONTROL, maxForces=[maxForce] * totalDofs,
force=force) timeStep=timeStep)
dofIndex += 1
dofIndex = 6
p.stepSimulation() scaling = 1
for index in range(len(jointIds)):
jointIndex = jointIds[index]
force = [scaling * taus[dofIndex]]
#print("force[", jointIndex, "]=", force)
p.setJointMotorControlMultiDof(quadruped,
jointIndex,
controlMode=p.TORQUE_CONTROL,
force=force)
dofIndex += 1
p.stepSimulation()
t += timeStep t += timeStep
time.sleep(timeStep) time.sleep(timeStep)