AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

fix testlaikago and pd_controller_stable

Browse Source
This commit is contained in:
bla 2019-06-06 21:54:22 +00:00
parent 093986f1ee
commit 6ab09fe06e
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -56,7 +56,7 @@ for i in range(4):
jointOffsets.append(-0.7) jointOffsets.append(-0.7)
jointOffsets.append(0.7) jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20) maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 120)
for j in range(p.getNumJoints(quadruped)): for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0) p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
@ -103,7 +103,7 @@ for i in range(4):
jointOffsets.append(-0.7) jointOffsets.append(-0.7)
jointOffsets.append(0.7) jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20) maxUpForceId = p.addUserDebugParameter("maxUpForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)): for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0) p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
@ -162,7 +162,11 @@ while t < 10. * cycleTime:
jointsStr, qdot = qpi.Slerp(frameFraction, frameData, frameDataNext, p) jointsStr, qdot = qpi.Slerp(frameFraction, frameData, frameDataNext, p)
maxForce = p.readUserDebugParameter(maxForceId) maxForce = p.readUserDebugParameter(maxForceId)
print("jointIds=", jointIds) #print("jointIds=", jointIds)
maxUpForce = p.readUserDebugParameter(maxUpForceId)
p.changeConstraint(cid, maxForce=maxUpForce)
if useConstraints: if useConstraints:
for j in range(12): for j in range(12):
@ -194,7 +198,7 @@ while t < 10. * cycleTime:
desiredVelocities=desiredVelocities, desiredVelocities=desiredVelocities,
kps=[4000] * totalDofs, kps=[4000] * totalDofs,
kds=[40] * totalDofs, kds=[40] * totalDofs,
maxForces=[500] * totalDofs, maxForces=[maxForce] * totalDofs,
timeStep=timeStep) timeStep=timeStep)
dofIndex = 6 dofIndex = 6
@ -202,7 +206,7 @@ while t < 10. * cycleTime:
for index in range(len(jointIds)): for index in range(len(jointIds)):
jointIndex = jointIds[index] jointIndex = jointIds[index]
force = [scaling * taus[dofIndex]] force = [scaling * taus[dofIndex]]
print("force[", jointIndex, "]=", force) #print("force[", jointIndex, "]=", force)
p.setJointMotorControlMultiDof(quadruped, p.setJointMotorControlMultiDof(quadruped,
jointIndex, jointIndex,
controlMode=p.TORQUE_CONTROL, controlMode=p.TORQUE_CONTROL,
@ -231,8 +235,9 @@ if useOrgData:
force=maxForce) force=maxForce)
p.stepSimulation() p.stepSimulation()
for lower_leg in lower_legs: for lower_leg in lower_legs:
pass
#print("points for ", quadruped, " link: ", lower_leg) #print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg) #pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts)) #print("num points=",len(pts))
#for pt in pts: #for pt in pts:
# print(pt[9]) # print(pt[9])

2
examples/pybullet/gym/pybullet_utils/pd_controller_stable.py
View File

@ -227,6 +227,6 @@ class PDControllerStable(object):
qddot = np.linalg.solve(A, b) qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep tau = p + d - Kd.dot(qddot) * timeStep
maxF = np.array(maxForces) maxF = np.array(maxForces)
forces = np.clip(tau, -maxF, maxF) tau = np.clip(tau, -maxF, maxF)
#print("c=",c) #print("c=",c)
return tau return tau