add kinematic version that also updates the base position/orientation from the base

This commit is contained in:
Erwin Coumans
2019-06-13 13:26:52 -07:00
parent dc9549b2b4
commit 8f9fac99bd

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