1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
|
import pybullet as p
import time
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
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.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))
#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 = []
with open("data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
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)
|
