266 lines
8.9 KiB
Python
266 lines
8.9 KiB
Python
import pybullet as p1
|
|
from pybullet_utils import bullet_client
|
|
import pybullet_data
|
|
from pybullet_utils import pd_controller_stable
|
|
|
|
import time
|
|
import motion_capture_data
|
|
import quadrupedPoseInterpolator
|
|
|
|
useConstraints = False
|
|
|
|
p = bullet_client.BulletClient(connection_mode=p1.GUI)
|
|
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
|
|
|
plane = p.loadURDF("plane.urdf")
|
|
p.setGravity(0, 0, -10)
|
|
timeStep = 1. / 500
|
|
p.setTimeStep(timeStep)
|
|
#p.setDefaultContactERP(0)
|
|
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
|
urdfFlags = p.URDF_USE_SELF_COLLISION
|
|
|
|
startPos = [0.007058990464444105, 0.03149299192130908, 0.4918981912395484]
|
|
startOrn = [0.005934649695708604, 0.7065453990917289, 0.7076373820553712, -0.0027774940359030264]
|
|
quadruped = p.loadURDF("laikago/laikago.urdf",
|
|
startPos,
|
|
startOrn,
|
|
flags=urdfFlags,
|
|
useFixedBase=False)
|
|
p.resetBasePositionAndOrientation(quadruped, startPos, startOrn)
|
|
if not useConstraints:
|
|
for j in range(p.getNumJoints(quadruped)):
|
|
p.setJointMotorControl2(quadruped, j, p.POSITION_CONTROL, force=0)
|
|
|
|
#This cube is added as a soft constraint to keep the laikago from falling
|
|
#since we didn't train it yet, it doesn't balance
|
|
cube = p.loadURDF("cube_no_rotation.urdf", [0, 0, -0.5], [0, 0.5, 0.5, 0])
|
|
p.setCollisionFilterGroupMask(cube, -1, 0, 0)
|
|
for j in range(p.getNumJoints(cube)):
|
|
p.setJointMotorControl2(cube, j, p.POSITION_CONTROL, force=0)
|
|
p.setCollisionFilterGroupMask(cube, j, 0, 0)
|
|
p.changeVisualShape(cube, j, rgbaColor=[1, 0, 0, 0])
|
|
cid = p.createConstraint(cube,
|
|
p.getNumJoints(cube) - 1, quadruped, -1, p.JOINT_FIXED, [0, 0, 0],
|
|
[0, 1, 0], [0, 0, 0])
|
|
p.changeConstraint(cid, maxForce=10)
|
|
|
|
jointIds = []
|
|
paramIds = []
|
|
jointOffsets = []
|
|
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
|
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
|
|
|
for i in range(4):
|
|
jointOffsets.append(0)
|
|
jointOffsets.append(-0.7)
|
|
jointOffsets.append(0.7)
|
|
|
|
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
|
|
|
for j in range(p.getNumJoints(quadruped)):
|
|
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
|
info = p.getJointInfo(quadruped, j)
|
|
#print(info)
|
|
jointName = info[1]
|
|
jointType = info[2]
|
|
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
|
jointIds.append(j)
|
|
|
|
startQ = [
|
|
0.08389, 0.8482, -1.547832, -0.068933, 0.625726, -1.272086, 0.074398, 0.61135, -1.255892,
|
|
-0.068262, 0.836745, -1.534517
|
|
]
|
|
for j in range(p.getNumJoints(quadruped)):
|
|
p.resetJointState(quadruped, jointIds[j], jointDirections[j] * startQ[j] + jointOffsets[j])
|
|
|
|
qpi = quadrupedPoseInterpolator.QuadrupedPoseInterpolator()
|
|
|
|
#enable collision between lower legs
|
|
|
|
for j in range(p.getNumJoints(quadruped)):
|
|
print(p.getJointInfo(quadruped, j))
|
|
|
|
#2,5,8 and 11 are the lower legs
|
|
lower_legs = [2, 5, 8, 11]
|
|
for l0 in lower_legs:
|
|
for l1 in lower_legs:
|
|
if (l1 > l0):
|
|
enableCollision = 1
|
|
print("collision for pair", l0, l1,
|
|
p.getJointInfo(quadruped, l0)[12],
|
|
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
|
|
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
|
|
|
|
jointIds = []
|
|
paramIds = []
|
|
jointOffsets = []
|
|
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
|
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
|
|
|
for i in range(4):
|
|
jointOffsets.append(0)
|
|
jointOffsets.append(-0.7)
|
|
jointOffsets.append(0.7)
|
|
|
|
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
|
|
|
for j in range(p.getNumJoints(quadruped)):
|
|
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
|
info = p.getJointInfo(quadruped, j)
|
|
#print(info)
|
|
jointName = info[1]
|
|
jointType = info[2]
|
|
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
|
jointIds.append(j)
|
|
|
|
p.getCameraImage(480, 320)
|
|
p.setRealTimeSimulation(0)
|
|
|
|
joints = []
|
|
|
|
mocapData = motion_capture_data.MotionCaptureData()
|
|
|
|
motionPath = pybullet_data.getDataPath() + "/data/motions/laikago_walk.txt"
|
|
|
|
mocapData.Load(motionPath)
|
|
print("mocapData.NumFrames=", mocapData.NumFrames())
|
|
print("mocapData.KeyFrameDuraction=", mocapData.KeyFrameDuraction())
|
|
print("mocapData.getCycleTime=", mocapData.getCycleTime())
|
|
print("mocapData.computeCycleOffset=", mocapData.computeCycleOffset())
|
|
|
|
stablePD = pd_controller_stable.PDControllerStable(p)
|
|
|
|
cycleTime = mocapData.getCycleTime()
|
|
t = 0
|
|
|
|
while t < 10. * cycleTime:
|
|
#get interpolated joint
|
|
keyFrameDuration = mocapData.KeyFrameDuraction()
|
|
cycleTime = mocapData.getCycleTime()
|
|
cycleCount = mocapData.calcCycleCount(t, cycleTime)
|
|
|
|
#print("cycleTime=",cycleTime)
|
|
#print("cycleCount=",cycleCount)
|
|
|
|
#print("cycles=",cycles)
|
|
frameTime = t - cycleCount * cycleTime
|
|
#print("frameTime=",frameTime)
|
|
if (frameTime < 0):
|
|
frameTime += cycleTime
|
|
|
|
frame = int(frameTime / keyFrameDuration)
|
|
frameNext = frame + 1
|
|
if (frameNext >= mocapData.NumFrames()):
|
|
frameNext = frame
|
|
frameFraction = (frameTime - frame * keyFrameDuration) / (keyFrameDuration)
|
|
#print("frame=",frame)
|
|
#print("frameFraction=",frameFraction)
|
|
frameData = mocapData._motion_data['Frames'][frame]
|
|
frameDataNext = mocapData._motion_data['Frames'][frameNext]
|
|
|
|
jointsStr, qdot = qpi.Slerp(frameFraction, frameData, frameDataNext, p)
|
|
|
|
maxForce = p.readUserDebugParameter(maxForceId)
|
|
print("jointIds=", jointIds)
|
|
|
|
if useConstraints:
|
|
for j in range(12):
|
|
#skip the base positional dofs
|
|
targetPos = float(jointsStr[j + 7])
|
|
p.setJointMotorControl2(quadruped,
|
|
jointIds[j],
|
|
p.POSITION_CONTROL,
|
|
jointDirections[j] * targetPos + jointOffsets[j],
|
|
force=maxForce)
|
|
|
|
else:
|
|
desiredPositions = []
|
|
for j in range(7):
|
|
targetPosUnmodified = float(jointsStr[j])
|
|
desiredPositions.append(targetPosUnmodified)
|
|
for j in range(12):
|
|
targetPosUnmodified = float(jointsStr[j + 7])
|
|
targetPos = jointDirections[j] * targetPosUnmodified + jointOffsets[j]
|
|
desiredPositions.append(targetPos)
|
|
numBaseDofs = 6
|
|
totalDofs = 12 + numBaseDofs
|
|
desiredVelocities = None
|
|
if desiredVelocities == None:
|
|
desiredVelocities = [0] * totalDofs
|
|
taus = stablePD.computePD(bodyUniqueId=quadruped,
|
|
jointIndices=jointIds,
|
|
desiredPositions=desiredPositions,
|
|
desiredVelocities=desiredVelocities,
|
|
kps=[4000] * totalDofs,
|
|
kds=[40] * totalDofs,
|
|
maxForces=[500] * totalDofs,
|
|
timeStep=timeStep)
|
|
|
|
dofIndex = 6
|
|
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
|
|
time.sleep(timeStep)
|
|
|
|
useOrgData = False
|
|
if useOrgData:
|
|
with open("data1.txt", "r") as filestream:
|
|
for line in filestream:
|
|
maxForce = p.readUserDebugParameter(maxForceId)
|
|
currentline = line.split(",")
|
|
frame = currentline[0]
|
|
t = currentline[1]
|
|
joints = currentline[2:14]
|
|
for j in range(12):
|
|
targetPos = float(joints[j])
|
|
p.setJointMotorControl2(quadruped,
|
|
jointIds[j],
|
|
p.POSITION_CONTROL,
|
|
jointDirections[j] * targetPos + jointOffsets[j],
|
|
force=maxForce)
|
|
p.stepSimulation()
|
|
for lower_leg in lower_legs:
|
|
#print("points for ", quadruped, " link: ", lower_leg)
|
|
pts = p.getContactPoints(quadruped, -1, lower_leg)
|
|
#print("num points=",len(pts))
|
|
#for pt in pts:
|
|
# print(pt[9])
|
|
time.sleep(1. / 500.)
|
|
|
|
for j in range(p.getNumJoints(quadruped)):
|
|
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
|
info = p.getJointInfo(quadruped, j)
|
|
js = p.getJointState(quadruped, j)
|
|
#print(info)
|
|
jointName = info[1]
|
|
jointType = info[2]
|
|
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
|
paramIds.append(
|
|
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
|
|
(js[0] - jointOffsets[j]) / jointDirections[j]))
|
|
|
|
p.setRealTimeSimulation(1)
|
|
|
|
while (1):
|
|
|
|
for i in range(len(paramIds)):
|
|
c = paramIds[i]
|
|
targetPos = p.readUserDebugParameter(c)
|
|
maxForce = p.readUserDebugParameter(maxForceId)
|
|
p.setJointMotorControl2(quadruped,
|
|
jointIds[i],
|
|
p.POSITION_CONTROL,
|
|
jointDirections[i] * targetPos + jointOffsets[i],
|
|
force=maxForce)
|