-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathcentipede_xml_generator.py
More file actions
155 lines (140 loc) · 8.88 KB
/
centipede_xml_generator.py
File metadata and controls
155 lines (140 loc) · 8.88 KB
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
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
import numpy as np
# All variables needed to define the centipede. Torso and legs are all capsule shaped.
# Not sure what the dimensions are in. TODO: Find it out
# Initially x-axis is forward for the centipede
torsoLength=0.2 # Length of one unit/link of the torso
torsoDiameter=0.06 # Diameter of one unit/link of the torso
appendageLength=0.1
appendageDiameter=0.03
thighLength=0.2
thighDiameter=appendageDiameter
legLength=0.3
legDiameter=appendageDiameter
torsoDistance=0.15 # Distance between 2 links of torso
# Angular freedom for various links (everything in degrees)
appendageAngleMin=-20
appendageAngleMax=-appendageAngleMin
thighAngleMin=-30
thighAngleMax=-thighAngleMin
legAngleMin=20
legAngleMax=90
torsoAngleXMin=-20
torsoAngleXMax=-torsoAngleXMin
torsoAngleYMin=-20
torsoAngleYMax=-torsoAngleYMin
torsoAngleZMin=-20
torsoAngleZMax=-torsoAngleZMin
controlSaturation=0.05 # Determined empirically
N=15 # Number of pairs of legs for the centipede (1 = sad, 5 = happy, 10 = chad, 50 = shai hulud)
torsoArr=["" for x in range(N)]
leftLegArr=["" for x in range(N)]
rightLegArr=["" for x in range(N)]
leftActuatorArr=["" for x in range(N)]
rightActuatorArr=["" for x in range(N)]
# Model name, textures, inertia from geometry, numerical solver and other setup
Initializer="<!-- Model for a "+str(2*N)+"-legged centipede -->\n\
<mujoco model=\"centipede\">\n\
<compiler angle=\"degree\" coordinate=\"local\" inertiafromgeom=\"true\"/>\n\
<option integrator=\"RK4\" timestep=\"0.01\"/>\n\
<default>\n\
<joint armature=\"1\" damping=\"1\" limited=\"true\"/>\n\
<geom conaffinity=\"0\" condim=\"3\" density=\"5.0\" friction=\"2.0 2.0 2.0\" margin=\"0.01\" rgba=\"0.8 0.6 0.4 1\"/>\n\
</default>\n\
<asset>\n\
<texture builtin=\"gradient\" height=\"100\" rgb1=\"1 1 1\" rgb2=\"0 0 0\" type=\"skybox\" width=\"100\"/>\n\
<texture builtin=\"flat\" height=\"1278\" mark=\"cross\" markrgb=\"1 1 1\" name=\"texgeom\" random=\"0.01\" rgb1=\"0.8 0.6 0.4\" rgb2=\"0.8 0.6 0.4\" type=\"cube\" width=\"127\"/>\n\
<texture builtin=\"checker\" height=\"100\" name=\"texplane\" rgb1=\"0 0 0\" rgb2=\"0.8 0.8 0.8\" type=\"2d\" width=\"100\"/>\n\
<material name=\"MatPlane\" reflectance=\"0.5\" shininess=\"1\" specular=\"1\" texrepeat=\"60 60\" texture=\"texplane\"/>\n\
<material name=\"geom\" texture=\"texgeom\" texuniform=\"true\"/>\n\
</asset>"
BeginBody="\n\
<worldbody>\n\
<light cutoff=\"100\" diffuse=\"1 1 1\" dir=\"-0 0 -1.3\" directional=\"true\" exponent=\"1\" pos=\"0 0 1.3\" specular=\".1 .1 .1\"/>\n\
<geom conaffinity=\"1\" condim=\"3\" material=\"MatPlane\" name=\"floor\" pos=\"0 0 0\" rgba=\"0.8 0.9 0.8 1\" size=\"40 40 40\" type=\"plane\"/>"
torsoArr[0]="\n\
<body name=\"torso00\" pos=\"0 0 0.75\">\n\
<camera name=\"track\" mode=\"trackcom\" pos=\"0 -3 0.3\" xyaxes=\"1 0 0 0 0 1\"/>\n\
<geom fromto=\"0 "+str(-torsoLength/2)+" 0 0 "+str(torsoLength/2)+" 0\" name=\"torsoGeom00\" size=\""+str(torsoDiameter)+"\" type=\"capsule\"/>\n\
<joint armature=\"0\" damping=\"0\" limited=\"false\" margin=\"0.01\" name=\"root\" pos=\"0 0 0\" type=\"free\"/>"
for i in range(1,N):
torsoArr[i]="\n"+str(i*" ")+"\
<body name=\"torso"+str("%02d"%i)+"\" pos=\""+str(-torsoDistance)+" 0 0\">\n"+str(i*" ")+"\
<geom fromto=\"0 "+str(-torsoLength/2)+" 0 0 "+str(torsoLength/2)+" 0\" name=\"torsoGeom"+str("%02d"%i)+"\" size=\""+str(torsoDiameter)+"\" type=\"capsule\"/>\n"+str(i*" ")+"\
<joint armature=\"0.02\" axis=\"1 0 0\" damping=\"0\" name=\"torsoX"+str("%02d"%i)+"\" pos=\""+str(-torsoDistance/2)+" 0 0\" range=\""+str(torsoAngleXMin)+" "+str(torsoAngleXMax)+"\" stiffness=\"1\" type=\"hinge\"/>\n"+str(i*" ")+"\
<joint armature=\"0.02\" axis=\"0 1 0\" damping=\"0\" name=\"torsoY"+str("%02d"%i)+"\" pos=\""+str(-torsoDistance/2)+" 0 0\" range=\""+str(torsoAngleYMin)+" "+str(torsoAngleYMax)+"\" stiffness=\"1\" type=\"hinge\"/>\n"+str(i*" ")+"\
<joint armature=\"0.02\" axis=\"0 0 1\" damping=\"0\" name=\"torsoZ"+str("%02d"%i)+"\" pos=\""+str(-torsoDistance/2)+" 0 0\" range=\""+str(torsoAngleZMin)+" "+str(torsoAngleZMax)+"\" stiffness=\"1\" type=\"hinge\"/>"
for i in range(N):
leftLegArr[i]="\n"+str(i*" ")+"\
<body name=\"leftLeg"+str("%02d"%i)+"\" pos=\"0 "+str(torsoLength/2)+" 0\">\n"+str(i*" ")+"\
<joint axis=\"0 0 -1\" name=\"leftHipZ"+str("%02d"%i)+"\" pos=\"0 0 0\" range=\""+str(appendageAngleMin)+" "+str(appendageAngleMax)+"\" type=\"hinge\"/>\n"+str(i*" ")+"\
<geom fromto=\"0 0 0 0 "+str(appendageLength)+" 0\" size=\""+str(appendageDiameter)+"\" type=\"capsule\"/>\n"+str(i*" ")+"\
<body pos=\"0 "+str(appendageLength)+" 0\">\n"+str(i*" ")+"\
<joint axis=\"-1 0 0\" name=\"leftHipX"+str("%02d"%i)+"\" pos=\"0 0 0\" range=\""+str(thighAngleMin)+" "+str(thighAngleMax)+"\" type=\"hinge\"/>\n"+str(i*" ")+"\
<geom fromto=\"0 0 0 0 "+str(thighLength)+" 0\" size=\""+str(thighDiameter)+"\" type=\"capsule\"/>\n"+str(i*" ")+"\
<body pos=\"0 "+str(thighLength)+" 0\">\n"+str(i*" ")+"\
<joint axis=\"-1 0 0\" name=\"leftKnee"+str("%02d"%i)+"\" pos=\"0 0 0\" range=\""+str(legAngleMin)+" "+str(legAngleMax)+"\" type=\"hinge\"/>\n"+str(i*" ")+"\
<geom fromto=\"0 0 0 0 "+str(legLength)+" 0\" size=\""+str(legDiameter)+"\" type=\"capsule\"/>\n"+str(i*" ")+"\
</body>\n"+str(i*" ")+"\
</body>\n"+str(i*" ")+"\
</body>"
rightLegArr[i]="\n"+str(i*" ")+"\
<body name=\"rightLeg"+str("%02d"%i)+"\" pos=\"0 "+str(-torsoLength/2)+" 0\">\n"+str(i*" ")+"\
<joint axis=\"0 0 1\" name=\"rightHipZ"+str("%02d"%i)+"\" pos=\"0 0 0\" range=\""+str(appendageAngleMin)+" "+str(appendageAngleMax)+"\" type=\"hinge\"/>\n"+str(i*" ")+"\
<geom fromto=\"0 0 0 0 "+str(-appendageLength)+" 0\" size=\""+str(appendageDiameter)+"\" type=\"capsule\"/>\n"+str(i*" ")+"\
<body pos=\"0 "+str(-appendageLength)+" 0\">\n"+str(i*" ")+"\
<joint axis=\"1 0 0\" name=\"rightHipX"+str("%02d"%i)+"\" pos=\"0 0 0\" range=\""+str(thighAngleMin)+" "+str(thighAngleMax)+"\" type=\"hinge\"/>\n"+str(i*" ")+"\
<geom fromto=\"0 0 0 0 "+str(-thighLength)+" 0\" size=\""+str(thighDiameter)+"\" type=\"capsule\"/>\n"+str(i*" ")+"\
<body pos=\"0 "+str(-thighLength)+" 0\">\n"+str(i*" ")+"\
<joint axis=\"1 0 0\" name=\"rightKnee"+str("%02d"%i)+"\" pos=\"0 0 0\" range=\""+str(legAngleMin)+" "+str(legAngleMax)+"\" type=\"hinge\"/>\n"+str(i*" ")+"\
<geom fromto=\"0 0 0 0 "+str(-legLength)+" 0\" size=\""+str(legDiameter)+"\" type=\"capsule\"/>\n"+str(i*" ")+"\
</body>\n"+str(i*" ")+"\
</body>\n"+str(i*" ")+"\
</body>"
leftActuatorArr[i]="\
<motor name=\"leftHipZ"+str("%02d"%i)+"\" ctrllimited=\"true\" ctrlrange=\""+str(-controlSaturation)+" "+str(controlSaturation)+"\" joint=\"leftHipZ"+str("%02d"%i)+"\" gear=\"150\"/>\n\
<motor name=\"leftHipX"+str("%02d"%i)+"\" ctrllimited=\"true\" ctrlrange=\""+str(-controlSaturation)+" "+str(controlSaturation)+"\" joint=\"leftHipX"+str("%02d"%i)+"\" gear=\"150\"/>\n\
<motor name=\"leftKnee"+str("%02d"%i)+"\" ctrllimited=\"true\" ctrlrange=\""+str(-controlSaturation)+" "+str(controlSaturation)+"\" joint=\"leftKnee"+str("%02d"%i)+"\" gear=\"150\"/>\n"
rightActuatorArr[i]="\
<motor name=\"rightHipZ"+str("%02d"%i)+"\" ctrllimited=\"true\" ctrlrange=\""+str(-controlSaturation)+" "+str(controlSaturation)+"\" joint=\"rightHipZ"+str("%02d"%i)+"\" gear=\"150\"/>\n\
<motor name=\"rightHipX"+str("%02d"%i)+"\" ctrllimited=\"true\" ctrlrange=\""+str(-controlSaturation)+" "+str(controlSaturation)+"\" joint=\"rightHipX"+str("%02d"%i)+"\" gear=\"150\"/>\n\
<motor name=\"rightKnee"+str("%02d"%i)+"\" ctrllimited=\"true\" ctrlrange=\""+str(-controlSaturation)+" "+str(controlSaturation)+"\" joint=\"rightKnee"+str("%02d"%i)+"\" gear=\"150\"/>\n"
# Remove some legs if you wish
leftLegRemoveIndices=[]
rightLegRemoveIndices=[]
for i in leftLegRemoveIndices:
leftLegArr[i]=""
leftActuatorArr[i]=""
for i in rightLegRemoveIndices:
rightLegArr[i]=""
rightActuatorArr[i]=""
EndBody="\n"
for i in range(N):
EndBody=EndBody+str(" "*(N+1-i))+"</body>\n"
EndBody=EndBody+" </worldbody>"
Actuators="\n <actuator>\n"
for i in range(N):
Actuators=Actuators+leftActuatorArr[i]+rightActuatorArr[i]
Actuators=Actuators+" </actuator>"
# Create the file and write everything in it
file1 = open("/home/kj/virtualEnvironments/cs287/lib/python3.6/site-packages/gym/envs/mujoco/assets/centipede.xml","w")
file1.write(Initializer)
file1.write(BeginBody)
for i in range(N):
file1.write(torsoArr[i])
file1.write(leftLegArr[i])
file1.write(rightLegArr[i])
file1.write(EndBody)
file1.write(Actuators)
file1.write("\n</mujoco>")
file1.close()
file1 = open("centipede.xml","w")
file1.write(Initializer)
file1.write(BeginBody)
for i in range(N):
file1.write(torsoArr[i])
file1.write(leftLegArr[i])
file1.write(rightLegArr[i])
file1.write(EndBody)
file1.write(Actuators)
file1.write("\n</mujoco>")
file1.close()