This commit is contained in:
Erwin Coumans
2019-06-13 23:24:42 -07:00

View File

@@ -7,6 +7,7 @@ import time
import motion_capture_data
import quadrupedPoseInterpolator
useKinematic = False
useConstraints = False
p = bullet_client.BulletClient(connection_mode=p1.GUI)
@@ -168,6 +169,17 @@ while t < 10. * cycleTime:
p.changeConstraint(cid, maxForce=maxUpForce)
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):
#skip the base positional dofs
targetPos = float(jointsStr[j + 7])
p.resetJointState(quadruped,jointIds[j],jointDirections[j] * targetPos + jointOffsets[j])
else:
if useConstraints:
for j in range(12):
#skip the base positional dofs