diff options
Diffstat (limited to 'examples/pybullet/gym/pybullet_envs/examples/laikago.py')
| -rw-r--r-- | examples/pybullet/gym/pybullet_envs/examples/laikago.py | 110 |
1 files changed, 60 insertions, 50 deletions
diff --git a/examples/pybullet/gym/pybullet_envs/examples/laikago.py b/examples/pybullet/gym/pybullet_envs/examples/laikago.py index b256f2a04..081573cc0 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/laikago.py +++ b/examples/pybullet/gym/pybullet_envs/examples/laikago.py @@ -7,58 +7,61 @@ p.connect(p.GUI) p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
-p.setGravity(0,0,-9.8)
-p.setTimeStep(1./500)
+p.setGravity(0, 0, -9.8)
+p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
-quadruped = p.loadURDF("laikago/laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = urdfFlags,useFixedBase=False)
+quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
+ flags=urdfFlags,
+ useFixedBase=False)
#enable collision between lower legs
-for j in range (p.getNumJoints(quadruped)):
- print(p.getJointInfo(quadruped,j))
+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]
+lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
- if (l1>l0):
+ 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):
+ 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)
+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)
+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.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
-joints=[]
+joints = []
-with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream:
+with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
- print("line=",line)
+ print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
@@ -66,31 +69,35 @@ with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream: frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
- joints=currentline[2:14]
+ joints = currentline[2:14]
#print("joints=",joints)
- for j in range (12):
+ 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.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)
+ 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]))
-
+ 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)
@@ -100,5 +107,8 @@ while (1): 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)
-
+ p.setJointMotorControl2(quadruped,
+ jointIds[i],
+ p.POSITION_CONTROL,
+ jointDirections[i] * targetPos + jointOffsets[i],
+ force=maxForce)
|