├── LICENSE
├── README.md
├── graph1.png
├── humanoid_leg_12dof.8.urdf
├── motorController.py
├── play_various_walking.py
├── screenshot1.png
├── screenshot2.png
├── walkGenerator.py
└── walking_simulation_example.py


/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2018 Sunbin Kim
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # bipedal-robot-walking-simulation
 2 | 
 3 | bipedal robot walking simulation using pybullet.
 4 | 
 5 | ## walking_simulation_example.py
 6 | 
 7 | An example of simulation of robot walking.
 8 | 
 9 | ![screenshot1](./screenshot1.png "screenshot1.png")
10 | 
11 | ## play_various_walking.py
12 | 
13 | Youtube video: <https://youtu.be/5MpcD8YFSbI>
14 | 
15 | ![screenshot2](./screenshot2.png "screenshot2.png")
16 | 
17 | ## walkGenerator.py
18 | 
19 | This generates gait points. Has inverse kinematics function.
20 | 
21 | ![graph1](./graph1.png "graph1.png")
22 | 
23 | ## motorController.py
24 | 
25 | Helps to control motors. Time-based control.
26 | 


--------------------------------------------------------------------------------
/graph1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Einsbon/bipedal-robot-walking-simulation/9a3348be92b016b17b71edbce20b5bcb60dc7d15/graph1.png


--------------------------------------------------------------------------------
/humanoid_leg_12dof.8.urdf:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" ?>
  2 | <robot name="humanoid">
  3 |     <link name="waist">
  4 |         <visual>
  5 |             <geometry>
  6 |                 <box size="0.0285 0.093 0.034"/>
  7 |             </geometry>
  8 |             <origin xyz="0 0 0" rpy="0 0 0"/>
  9 |             <axis xyz="0 0 0" />
 10 |             <material name="motorColor">
 11 |                 <color rgba="0.3 0.3 0.3 1.0"/>
 12 |             </material>
 13 |         </visual>
 14 |         <inertial>
 15 |             <mass value="0.116"/>
 16 |             <inertia ixx="9.478166666666668e-05" ixy="0.0" ixz="0.0" iyy="1.902641666666667e-05" iyz="0.0" izz="9.145875000000002e-05"/>
 17 |         </inertial>
 18 |         <collision>
 19 |             <geometry>
 20 |                 <box size="0.0285 0.093 0.034"/>
 21 |             </geometry>
 22 |             <origin xyz="0 0 0" rpy="0 0 0"/>
 23 |         </collision>
 24 |     </link>
 25 | 
 26 |     <joint name="fixed_waist" type="fixed">
 27 |         <parent link="waist"/>
 28 |         <child link="body"/>
 29 |         <origin xyz="0 0 0.092" rpy="0 0 0"/>
 30 |     </joint>
 31 | 
 32 |     <link name="body">
 33 |         <visual>
 34 |             <geometry>
 35 |                 <box size="0.050 0.11 0.150"/>
 36 |             </geometry>
 37 |             <origin xyz="0 0 0" rpy="0 0 0"/>
 38 |             <axis xyz="0 0 0" />
 39 |             <material name="blue">
 40 |                 <color rgba="0.2 0.2 1 1"/>
 41 |             </material>
 42 |         </visual>
 43 |         <inertial>
 44 |             <mass value="0.8"/>
 45 |             <inertia ixx="0.0023066666666666665" ixy="0.0" ixz="0.0" iyy="0.001666666666666667" iyz="0.0" izz="0.0009733333333333334"/>
 46 |         </inertial>
 47 |         <collision>
 48 |             <geometry>
 49 |                 <box size="0.050 0.11 0.150"/>
 50 |             </geometry>
 51 |             <origin xyz="0 0 0" rpy="0 0 0"/>
 52 |         </collision>
 53 |     </link>
 54 | 
 55 |     <!-- Right leg -->
 56 | 
 57 |     <joint name="jointHipRZ" type="revolute">
 58 |         <parent link="waist"/>
 59 |         <child link="hipCylinderRZ"/>
 60 |         <origin xyz="0 -0.03525 -0.053" rpy="0 0 0"/>
 61 |         <axis xyz="0 0 1"/>
 62 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
 63 |         <dynamics damping="0"/>
 64 |     </joint>
 65 | 
 66 |     <link name="hipCylinderRZ">
 67 |         <visual>
 68 |             <geometry>
 69 |                 <cylinder length="0.04" radius="0.01"/>
 70 |             </geometry>
 71 |             <origin xyz="0 0 0.02" rpy="0 0 0"/>
 72 |             <axis xyz="0 0 0" />
 73 |             <material name="red">
 74 |                 <color rgba="1 0.0 0.0 1.0"/>
 75 |             </material>
 76 |         </visual>
 77 |         <inertial>
 78 |             <mass value="0"/>
 79 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
 80 |         </inertial>
 81 |     </link>
 82 | 
 83 |     <joint name="jointHipRX" type="revolute">
 84 |         <parent link="hipCylinderRZ"/>
 85 |         <child link="hipCylinderRX"/>
 86 |         <origin xyz="0 0 0" rpy="0 0 0"/>
 87 |         <axis xyz="1 0 0"/>
 88 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
 89 |         <dynamics damping="0"/>
 90 |     </joint>
 91 | 
 92 |     <link name="hipCylinderRX">
 93 |         <visual>
 94 |             <geometry>
 95 |                 <cylinder length="0.0725" radius="0.01"/>
 96 |             </geometry>
 97 |             <origin xyz="-0.017 0 0" rpy="0 1.57075 0"/>
 98 |             <axis xyz="0 0 0" />
 99 |             <material name="red"/>
100 |         </visual>
101 |         <inertial>
102 |             <mass value="0"/>
103 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
104 |         </inertial>
105 |     </link>
106 | 
107 |     <joint name="jointHipRY" type="revolute">
108 |         <parent link="hipCylinderRX"/>
109 |         <child link="hipCylinderRY"/>
110 |         <origin xyz="0 0 0" rpy="0 0 0"/>
111 |         <axis xyz="0 1 0"/>
112 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
113 |         <dynamics damping="0"/>
114 |     </joint>
115 | 
116 |     <link name="hipCylinderRY">
117 |         <visual>
118 |             <geometry>
119 |                 <cylinder length="0.044" radius="0.01"/>
120 |             </geometry>
121 |             <origin xyz="0 0 0" rpy="1.57075 0 0"/>
122 |             <axis xyz="0 0 0" />
123 |             <material name="red"/>
124 |         </visual>
125 |         <inertial>
126 |             <mass value="0"/>
127 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
128 |         </inertial>
129 |     </link>
130 | 
131 |     <joint name="jointKneeRY" type="revolute">
132 |         <parent link="hipCylinderRY"/>
133 |         <child link="kneeCylinderRY"/>
134 |         <origin xyz="0 0 -0.11" rpy="0 0 0"/>
135 |         <axis xyz="0 1 0"/>
136 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
137 |         <dynamics damping="0"/>
138 |     </joint>
139 | 
140 |     <link name="kneeCylinderRY">
141 |         <visual>
142 |             <geometry>
143 |                 <cylinder length="0.044" radius="0.01"/>
144 |             </geometry>
145 |             <origin xyz="0 0 0" rpy="1.57075 0 0"/>
146 |             <axis xyz="0 0 0" />
147 |             <material name="red"/>
148 |         </visual>
149 |         <inertial>
150 |             <mass value="0"/>
151 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
152 |         </inertial>
153 |     </link>
154 | 
155 |     <joint name="jointankleRY" type="revolute">
156 |         <parent link="kneeCylinderRY"/>
157 |         <child link="ankleCylinderRY"/>
158 |         <origin xyz="0 0 -0.11" rpy="0 0 0"/>
159 |         <axis xyz="0 1 0"/>
160 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
161 |         <dynamics damping="0"/>
162 |     </joint>
163 | 
164 |     <link name="ankleCylinderRY">
165 |         <visual>
166 |             <geometry>
167 |                 <cylinder length="0.044" radius="0.01"/>
168 |             </geometry>
169 |             <origin xyz="0 0 0" rpy="1.57075 0 0"/>
170 |             <axis xyz="0 0 0" />
171 |             <material name="red"/>
172 |         </visual>
173 |         <inertial>
174 |             <mass value="0"/>
175 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
176 |         </inertial>
177 |     </link>
178 | 
179 |     <joint name="jointankleRX" type="revolute">
180 |         <parent link="ankleCylinderRY"/>
181 |         <child link="ankleCylinderRX"/>
182 |         <origin xyz="0 0 0" rpy="0 0 0"/>
183 |         <axis xyz="1 0 0"/>
184 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
185 |         <dynamics damping="0"/>
186 |     </joint>
187 | 
188 |     <link name="ankleCylinderRX">
189 |         <visual>
190 |             <geometry>
191 |                 <cylinder length="0.0725" radius="0.01"/>
192 |             </geometry>
193 |             <origin xyz="-0.017 0 0" rpy="0 1.57075 0"/>
194 |             <axis xyz="0 0 0" />
195 |             <material name="red"/>
196 |         </visual>
197 |         <inertial>
198 |             <mass value="0"/>
199 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
200 |         </inertial>
201 |     </link>
202 | 
203 |     <!-- Left leg -->
204 | 
205 |     <joint name="jointHipLZ" type="revolute">
206 |         <parent link="waist"/>
207 |         <child link="hipCylinderLZ"/>
208 |         <origin xyz="0 0.03525 -0.053" rpy="0 0 0"/>
209 |         <axis xyz="0 0 1"/>
210 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
211 |         <dynamics damping="0"/>
212 |     </joint>
213 | 
214 |     <link name="hipCylinderLZ">
215 |         <visual>
216 |             <geometry>
217 |                 <cylinder length="0.04" radius="0.01"/>
218 |             </geometry>
219 |             <origin xyz="0 0 0.02" rpy="0 0 0"/>
220 |             <axis xyz="0 0 0" />
221 |             <material name="red">
222 |                 <color rgba="1 0.0 0.0 1.0"/>
223 |             </material>
224 |         </visual>
225 |         <inertial>
226 |             <mass value="0"/>
227 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
228 |         </inertial>
229 |     </link>
230 | 
231 |     <joint name="jointHipLX" type="revolute">
232 |         <parent link="hipCylinderLZ"/>
233 |         <child link="hipCylinderLX"/>
234 |         <origin xyz="0 0 0" rpy="0 0 0"/>
235 |         <axis xyz="1 0 0"/>
236 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
237 |         <dynamics damping="0"/>
238 |     </joint>
239 | 
240 |     <link name="hipCylinderLX">
241 |         <visual>
242 |             <geometry>
243 |                 <cylinder length="0.0725" radius="0.01"/>
244 |             </geometry>
245 |             <origin xyz="-0.017 0 0" rpy="0 1.57075 0"/>
246 |             <axis xyz="0 0 0" />
247 |             <material name="red"/>
248 |         </visual>
249 |         <inertial>
250 |             <mass value="0"/>
251 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
252 |         </inertial>
253 |     </link>
254 | 
255 |     <joint name="jointHipLY" type="revolute">
256 |         <parent link="hipCylinderLX"/>
257 |         <child link="hipCylinderLY"/>
258 |         <origin xyz="0 0 0" rpy="0 0 0"/>
259 |         <axis xyz="0 1 0"/>
260 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
261 |         <dynamics damping="0"/>
262 |     </joint>
263 | 
264 |     <link name="hipCylinderLY">
265 |         <visual>
266 |             <geometry>
267 |                 <cylinder length="0.044" radius="0.01"/>
268 |             </geometry>
269 |             <origin xyz="0 0 0" rpy="1.57075 0 0"/>
270 |             <axis xyz="0 0 0" />
271 |             <material name="red"/>
272 |         </visual>
273 |         <inertial>
274 |             <mass value="0"/>
275 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
276 |         </inertial>
277 |     </link>
278 | 
279 |     <joint name="jointKneeLY" type="revolute">
280 |         <parent link="hipCylinderLY"/>
281 |         <child link="kneeCylinderLY"/>
282 |         <origin xyz="0 0 -0.11" rpy="0 0 0"/>
283 |         <axis xyz="0 1 0"/>
284 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
285 |         <dynamics damping="0"/>
286 |     </joint>
287 | 
288 |     <link name="kneeCylinderLY">
289 |         <visual>
290 |             <geometry>
291 |                 <cylinder length="0.044" radius="0.01"/>
292 |             </geometry>
293 |             <origin xyz="0 0 0" rpy="1.57075 0 0"/>
294 |             <axis xyz="0 0 0" />
295 |             <material name="red"/>
296 |         </visual>
297 |         <inertial>
298 |             <mass value="0"/>
299 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
300 |         </inertial>
301 |     </link>
302 | 
303 |     <joint name="jointankleLY" type="revolute">
304 |         <parent link="kneeCylinderLY"/>
305 |         <child link="ankleCylinderLY"/>
306 |         <origin xyz="0 0 -0.11" rpy="0 0 0"/>
307 |         <axis xyz="0 1 0"/>
308 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
309 |         <dynamics damping="0"/>
310 |     </joint>
311 | 
312 |     <link name="ankleCylinderLY">
313 |         <visual>
314 |             <geometry>
315 |                 <cylinder length="0.044" radius="0.01"/>
316 |             </geometry>
317 |             <origin xyz="0 0 0" rpy="1.57075 0 0"/>
318 |             <axis xyz="0 0 0" />
319 |             <material name="red"/>
320 |         </visual>
321 |         <inertial>
322 |             <mass value="0"/>
323 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
324 |         </inertial>
325 |     </link>
326 | 
327 |     <joint name="jointankleLX" type="revolute">
328 |         <parent link="ankleCylinderLY"/>
329 |         <child link="ankleCylinderLX"/>
330 |         <origin xyz="0 0 0" rpy="0 0 0"/>
331 |         <axis xyz="1 0 0"/>
332 |         <limit effort="10" lower="-3" upper="+3" velocity="10"/>
333 |         <dynamics damping="0"/>
334 |     </joint>
335 | 
336 |     <link name="ankleCylinderLX">
337 |         <visual>
338 |             <geometry>
339 |                 <cylinder length="0.0725" radius="0.01"/>
340 |             </geometry>
341 |             <origin xyz="-0.017 0 0" rpy="0 1.57075 0"/>
342 |             <axis xyz="0 0 0" />
343 |             <material name="red"/>
344 |         </visual>
345 |         <inertial>
346 |             <mass value="0"/>
347 |             <inertia ixx="0" ixy="0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/>
348 |         </inertial>
349 |     </link>
350 | 
351 | 
352 | 
353 | 
354 |     <!-- Decorations R -->
355 | 
356 | 
357 |     <joint name="fixed_hipBridgeR" type="fixed">
358 |         <parent link="hipCylinderRZ"/>
359 |         <child link="hipBridgeR"/>
360 |     </joint>
361 | 
362 |     <link name="hipBridgeR">
363 |         <visual>
364 |             <geometry>
365 |                 <box size="0.0685 0.02 0.046"/>
366 |             </geometry>
367 |             <origin xyz="-0.017 0 0.023" rpy="0 0 0"/>
368 |             <material name="linkColor">
369 |                 <color rgba="1 1 1 1"/>
370 |             </material>
371 |         </visual>
372 |         <inertial>
373 |             <mass value="0.03"/>
374 |             <inertia ixx="6.29e-06" ixy="0.0" ixz="0.0" iyy="1.7020625000000003e-05" iyz="0.0" izz="1.2730625000000003e-05"/>
375 |         </inertial>
376 |         <collision>
377 |             <geometry>
378 |                 <box size="0.0685 0.02 0.046"/>
379 |             </geometry>
380 |             <origin xyz="-0.017 0 0.023" rpy="0 0 0"/>
381 |         </collision>
382 |     </link>
383 | 
384 |     <joint name="fixed_hipMotorRF" type="fixed">
385 |         <parent link="hipCylinderRX"/>
386 |         <child link="hipMotorRF"/>
387 |     </joint>
388 | 
389 |     <link name="hipMotorRF">
390 |         <visual>
391 |             <geometry>
392 |                 <box size="0.0285 0.034 0.0465"/>
393 |             </geometry>
394 |             <origin xyz="0 0 -0.01225" rpy="0 0 0"/>
395 |             <axis xyz="0 0 0" />
396 |             <material name="motorColor"/>
397 |         </visual>
398 |         <inertial>
399 |             <mass value="0.058"/>
400 |             <inertia ixx="1.6038208333333333e-05" ixy="0.0" ixz="0.0" iyy="1.4376750000000003e-05" iyz="0.0" izz="9.513208333333336e-06"/>
401 |         </inertial>
402 |         <collision>
403 |             <geometry>
404 |                 <box size="0.0285 0.034 0.0465"/>
405 |             </geometry>
406 |             <origin xyz="0 0 -0.01225" rpy="0 0 0"/>
407 |         </collision>
408 |     </link>
409 | 
410 |     <joint name="fixed_hipMotorRB" type="fixed">
411 |         <parent link="hipCylinderRX"/>
412 |         <child link="hipMotorRB"/>
413 |     </joint>
414 | 
415 |     <link name="hipMotorRB">
416 |         <visual>
417 |             <geometry>
418 |                 <box size="0.034 0.0285 0.0465"/>
419 |             </geometry>
420 |             <origin xyz="-0.0315 0 -0.01225" rpy="0 0 0"/>
421 |             <axis xyz="0 0 0" />
422 |             <material name="motorColor"/>
423 |         </visual>
424 |         <inertial>
425 |             <mass value="0.058"/>
426 |             <inertia ixx="1.4376750000000003e-05" ixy="0.0" ixz="0.0" iyy="1.6038208333333333e-05" iyz="0.0" izz="9.513208333333336e-06"/>
427 |         </inertial>
428 |         <collision>
429 |             <geometry>
430 |                 <box size="0.034 0.0285 0.0465"/>
431 |             </geometry>
432 |             <origin xyz="-0.0315 0 -0.01225" rpy="0 0 0"/>
433 |         </collision>
434 |     </link>
435 | 
436 |     <joint name="fixed_legUpR" type="fixed">
437 |         <parent link="hipCylinderRY"/>
438 |         <child link="legUpR"/>
439 |     </joint>
440 | 
441 |     <link name="legUpR">
442 |         <visual>
443 |             <geometry>
444 |                 <box size="0.02 0.04 0.11"/>
445 |             </geometry>
446 |             <origin xyz="0 0 -0.055"/>
447 |             <axis xyz="0 0 0" />
448 |             <material name="linkColor"/>
449 |         </visual>
450 |         <inertial>
451 |             <mass value="0.03"/>
452 |             <inertia ixx="3.425e-05" ixy="0.0" ixz="0.0" iyy="3.1249999999999994e-05" iyz="0.0" izz="5e-06"/>
453 |         </inertial>
454 |         <collision>
455 |             <geometry>
456 |                 <box size="0.02 0.04 0.11"/>
457 |             </geometry>
458 |             <origin xyz="0 0 -0.055"/>
459 |         </collision>
460 |     </link>
461 | 
462 |     <joint name="fixed_kneeMotorR" type="fixed">
463 |         <parent link="kneeCylinderRY"/>
464 |         <child link="kneeMotorR"/>
465 |     </joint>
466 | 
467 |     <link name="kneeMotorR">
468 |         <visual>
469 |             <geometry>
470 |                 <box size="0.034 0.0285 0.0465"/>
471 |             </geometry>
472 |             <origin xyz="0 0 -0.01225" rpy="0 0 0"/>
473 |             <axis xyz="0 0 0" />
474 |             <material name="motorColor"/>
475 |         </visual>
476 |         <inertial>
477 |             <mass value="0.058"/>
478 |             <inertia ixx="1.4376750000000003e-05" ixy="0.0" ixz="0.0" iyy="1.6038208333333333e-05" iyz="0.0" izz="1.6038208333333333e-05"/>
479 |         </inertial>
480 |         <collision>
481 |             <geometry>
482 |                 <box size="0.034 0.0285 0.0465"/>
483 |             </geometry>
484 |             <origin xyz="0 0 -0.01225" rpy="0 0 0"/>
485 |         </collision>
486 |     </link>
487 | 
488 |     <joint name="fixed_legDownR" type="fixed">
489 |         <parent link="kneeCylinderRY"/>
490 |         <child link="legDownR"/>
491 |     </joint>
492 | 
493 |     <link name="legDownR">
494 |         <visual>
495 |             <geometry>
496 |                 <box size="0.02 0.04 0.07475"/>
497 |             </geometry>
498 |             <origin xyz="0 0 -0.073625"/>
499 |             <axis xyz="0 0 0" />
500 |             <material name="linkColor"/>
501 |         </visual>
502 |         <inertial>
503 |             <mass value="0.03"/>
504 |             <inertia ixx="1.796890625e-05" ixy="0.0" ixz="0.0" iyy="1.4968906249999999e-05" iyz="0.0" izz="5e-06"/>
505 |         </inertial>
506 |         <collision>
507 |             <geometry>
508 |                 <box size="0.02 0.04 0.07475"/>
509 |             </geometry>
510 |             <origin xyz="0 0 -0.073625"/>
511 |         </collision>
512 |     </link>
513 | 
514 |     <joint name="fixed_ankleMotorRF" type="fixed">
515 |         <parent link="ankleCylinderRY"/>
516 |         <child link="ankleMotorRF"/>
517 |     </joint>
518 | 
519 |     <link name="ankleMotorRF">
520 |         <visual>
521 |             <geometry>
522 |                 <box size="0.0285 0.034 0.0465"/>
523 |             </geometry>
524 |             <origin xyz="0 0 0.01225" rpy="0 0 0"/>
525 |             <axis xyz="0 0 0" />
526 |             <material name="motorColor"/>
527 |         </visual>
528 |         <inertial>
529 |             <mass value="0.058"/>
530 |             <inertia ixx="1.6038208333333333e-05" ixy="0.0" ixz="0.0" iyy="1.4376750000000003e-05" iyz="0.0" izz="9.513208333333336e-06"/>
531 |         </inertial>
532 |         <collision>
533 |             <geometry>
534 |                 <box size="0.0285 0.034 0.0465"/>
535 |             </geometry>
536 |             <origin xyz="0 0 0.01225" rpy="0 0 0"/>
537 |         </collision>
538 |     </link>
539 | 
540 |     <joint name="fixed_ankleMotorRB" type="fixed">
541 |         <parent link="ankleCylinderRY"/>
542 |         <child link="ankleMotorRB"/>
543 |     </joint>
544 | 
545 |     <link name="ankleMotorRB">
546 |         <visual>
547 |             <geometry>
548 |                 <box size="0.034 0.0285 0.0465"/>
549 |             </geometry>
550 |             <origin xyz="-0.0315 0 0.01225" rpy="0 0 0"/>
551 |             <axis xyz="0 0 0" />
552 |             <material name="motorColor"/>
553 |         </visual>
554 |         <inertial>
555 |             <mass value="0.058"/>
556 |             <inertia ixx="1.4376750000000003e-05" ixy="0.0" ixz="0.0" iyy="1.6038208333333333e-05" iyz="0.0" izz="9.513208333333336e-06"/>
557 |         </inertial>
558 |         <collision>
559 |             <geometry>
560 |                 <box size="0.034 0.0285 0.0465"/>
561 |             </geometry>
562 |             <origin xyz="-0.0315 0 0.01225" rpy="0 0 0"/>
563 |         </collision>
564 |     </link>
565 | 
566 |     <joint name="fixed_footBridgeR" type="fixed">
567 |         <parent link="ankleCylinderRX"/>
568 |         <child link="footBridgeR"/>
569 |     </joint>
570 | 
571 |     <link name = "footBridgeR">
572 |         <visual>
573 |             <geometry>
574 |                 <box size="0.0685 0.02 0.025"/>
575 |             </geometry>
576 |             <origin xyz="-0.017 0 -0.0125" rpy="0 0 0"/>
577 |             <axis xyz="0 0 0" />
578 |             <material name="linkColor"/>
579 |         </visual>
580 |         <inertial>
581 |             <mass value="0.009"/>
582 |             <inertia ixx="8.541666666666668e-07" ixy="0.0" ixz="0.0" iyy="4.431041666666667e-06" iyz="0.0" izz="4.243541666666668e-06"/>
583 |         </inertial>
584 |         <collision>
585 |             <geometry>
586 |                 <box size="0.0685 0.02 0.025"/>
587 |             </geometry>
588 |             <origin xyz="-0.017 0 -0.0125" rpy="0 0 0"/>
589 |         </collision>
590 |     </link>
591 | 
592 |     <joint name="fixed_footR" type="fixed">
593 |         <parent link="ankleCylinderRX"/>
594 |         <child link="footR"/>
595 |     </joint>
596 | 
597 |     <link name="footR">
598 |         <visual>
599 |             <geometry>
600 |                 <box size="0.10 0.05 0.005"/>
601 |             </geometry>
602 |             <origin xyz="0.0 -0.0 -0.0275" rpy="0 0 0"/>
603 |             <axis xyz="0 0 0" />
604 |             <material name="footColor">
605 |                 <color rgba="0.2 0.2 1 1.0"/>
606 |             </material>
607 |         </visual>
608 |         <inertial>
609 |             <mass value="0.025"/>
610 |             <inertia ixx="1.026041666666667e-05" ixy="0.0" ixz="0.0" iyy="4.692708333333334e-05" iyz="0.0" izz="5.7083333333333336e-05"/>
611 |         </inertial>
612 |         <!--
613 |         <collision>
614 | 			<geometry>
615 | 				<box size="0.15 0.04 0.02"/>
616 | 			</geometry>
617 | 		    <origin xyz="0.02 -0.015 -0.02" rpy="0 0 0"/>
618 |         </collision>
619 |         <collision>
620 | 			<geometry>
621 | 				<box size="0.12 0.07 0.02"/>
622 | 			</geometry>
623 | 		    <origin xyz="0.02 -0.015 -0.02" rpy="0 0 0"/>
624 |         </collision>
625 |         -->
626 |         <collision>
627 |             <geometry>
628 |                 <box size="0.10 0.05 0.02"/>
629 |             </geometry>
630 |             <origin xyz="0.00 -0.00 -0.02" rpy="0 0 0"/>
631 |         </collision>
632 |     </link>
633 | 
634 |     <!-- Decorations L -->
635 | 
636 |     <joint name="fixed_hipBridgeL" type="fixed">
637 |         <parent link="hipCylinderLZ"/>
638 |         <child link="hipBridgeL"/>
639 |     </joint>
640 | 
641 |     <link name="hipBridgeL">
642 |         <visual>
643 |             <geometry>
644 |                 <box size="0.0685 0.02 0.046"/>
645 |             </geometry>
646 |             <origin xyz="-0.017 0 0.023" rpy="0 0 0"/>
647 |             <material name="linkColor"/>
648 |         </visual>
649 |         <inertial>
650 |             <mass value="0.03"/>
651 |             <inertia ixx="6.29e-06" ixy="0.0" ixz="0.0" iyy="1.7020625000000003e-05" iyz="0.0" izz="1.2730625000000003e-05"/>
652 |         </inertial>
653 |         <collision>
654 |             <geometry>
655 |                 <box size="0.0685 0.02 0.046"/>
656 |             </geometry>
657 |             <origin xyz="-0.017 0 0.023" rpy="0 0 0"/>
658 |         </collision>
659 |     </link>
660 | 
661 |     <joint name="fixed_hipMotorLF" type="fixed">
662 |         <parent link="hipCylinderLX"/>
663 |         <child link="hipMotorLF"/>
664 |     </joint>
665 | 
666 |     <link name="hipMotorLF">
667 |         <visual>
668 |             <geometry>
669 |                 <box size="0.0285 0.034 0.0465"/>
670 |             </geometry>
671 |             <origin xyz="0 0 -0.01225" rpy="0 0 0"/>
672 |             <axis xyz="0 0 0" />
673 |             <material name="motorColor"/>
674 |         </visual>
675 |         <inertial>
676 |             <mass value="0.058"/>
677 |             <inertia ixx="1.6038208333333333e-05" ixy="0.0" ixz="0.0" iyy="1.4376750000000003e-05" iyz="0.0" izz="9.513208333333336e-06"/>
678 |         </inertial>
679 |         <collision>
680 |             <geometry>
681 |                 <box size="0.0285 0.034 0.0465"/>
682 |             </geometry>
683 |             <origin xyz="0 0 -0.01225" rpy="0 0 0"/>
684 |         </collision>
685 |     </link>
686 | 
687 |     <joint name="fixed_hipMotorLB" type="fixed">
688 |         <parent link="hipCylinderLX"/>
689 |         <child link="hipMotorLB"/>
690 |     </joint>
691 | 
692 |     <link name="hipMotorLB">
693 |         <visual>
694 |             <geometry>
695 |                 <box size="0.034 0.0285 0.0465"/>
696 |             </geometry>
697 |             <origin xyz="-0.0315 0 -0.01225" rpy="0 0 0"/>
698 |             <axis xyz="0 0 0" />
699 |             <material name="motorColor"/>
700 |         </visual>
701 |         <inertial>
702 |             <mass value="0.058"/>
703 |             <inertia ixx="1.4376750000000003e-05" ixy="0.0" ixz="0.0" iyy="1.6038208333333333e-05" iyz="0.0" izz="9.513208333333336e-06"/>
704 |         </inertial>
705 |         <collision>
706 |             <geometry>
707 |                 <box size="0.034 0.0285 0.0465"/>
708 |             </geometry>
709 |             <origin xyz="-0.0315 0 -0.01225" rpy="0 0 0"/>
710 |         </collision>
711 |     </link>
712 | 
713 |     <joint name="fixed_legUpL" type="fixed">
714 |         <parent link="hipCylinderLY"/>
715 |         <child link="legUpL"/>
716 |     </joint>
717 | 
718 |     <link name="legUpL">
719 |         <visual>
720 |             <geometry>
721 |                 <box size="0.02 0.04 0.11"/>
722 |             </geometry>
723 |             <origin xyz="0 0 -0.055"/>
724 |             <axis xyz="0 0 0" />
725 |             <material name="linkColor"/>
726 |         </visual>
727 |         <inertial>
728 |             <mass value="0.03"/>
729 |             <inertia ixx="3.425e-05" ixy="0.0" ixz="0.0" iyy="3.1249999999999994e-05" iyz="0.0" izz="5e-06"/>
730 |         </inertial>
731 |         <collision>
732 |             <geometry>
733 |                 <box size="0.02 0.04 0.11"/>
734 |             </geometry>
735 |             <origin xyz="0 0 -0.055"/>
736 |         </collision>
737 |     </link>
738 | 
739 |     <joint name="fixed_kneeMotorL" type="fixed">
740 |         <parent link="kneeCylinderLY"/>
741 |         <child link="kneeMotorL"/>
742 |     </joint>
743 | 
744 |     <link name="kneeMotorL">
745 |         <visual>
746 |             <geometry>
747 |                 <box size="0.034 0.0285 0.0465"/>
748 |             </geometry>
749 |             <origin xyz="0 0 -0.01225" rpy="0 0 0"/>
750 |             <axis xyz="0 0 0" />
751 |             <material name="motorColor"/>
752 |         </visual>
753 |         <inertial>
754 |             <mass value="0.058"/>
755 |             <inertia ixx="1.4376750000000003e-05" ixy="0.0" ixz="0.0" iyy="1.6038208333333333e-05" iyz="0.0" izz="1.6038208333333333e-05"/>
756 |         </inertial>
757 |         <collision>
758 |             <geometry>
759 |                 <box size="0.034 0.0285 0.0465"/>
760 |             </geometry>
761 |             <origin xyz="0 0 -0.01225" rpy="0 0 0"/>
762 |         </collision>
763 |     </link>
764 | 
765 |     <joint name="fixed_legDownL" type="fixed">
766 |         <parent link="kneeCylinderLY"/>
767 |         <child link="legDownL"/>
768 |     </joint>
769 | 
770 |     <link name="legDownL">
771 |         <visual>
772 |             <geometry>
773 |                 <box size="0.02 0.04 0.07475"/>
774 |             </geometry>
775 |             <origin xyz="0 0 -0.073625"/>
776 |             <axis xyz="0 0 0" />
777 |             <material name="linkColor"/>
778 |         </visual>
779 |         <inertial>
780 |             <mass value="0.03"/>
781 |             <inertia ixx="1.796890625e-05" ixy="0.0" ixz="0.0" iyy="1.4968906249999999e-05" iyz="0.0" izz="5e-06"/>
782 |         </inertial>
783 |         <collision>
784 |             <geometry>
785 |                 <box size="0.02 0.04 0.07475"/>
786 |             </geometry>
787 |             <origin xyz="0 0 -0.073625"/>
788 |         </collision>
789 |     </link>
790 | 
791 |     <joint name="fixed_ankleMotorLF" type="fixed">
792 |         <parent link="ankleCylinderLY"/>
793 |         <child link="ankleMotorLF"/>
794 |     </joint>
795 | 
796 |     <link name="ankleMotorLF">
797 |         <visual>
798 |             <geometry>
799 |                 <box size="0.0285 0.034 0.0465"/>
800 |             </geometry>
801 |             <origin xyz="0 0 0.01225" rpy="0 0 0"/>
802 |             <axis xyz="0 0 0" />
803 |             <material name="motorColor"/>
804 |         </visual>
805 |         <inertial>
806 |             <mass value="0.058"/>
807 |             <inertia ixx="1.6038208333333333e-05" ixy="0.0" ixz="0.0" iyy="1.4376750000000003e-05" iyz="0.0" izz="9.513208333333336e-06"/>
808 |         </inertial>
809 |         <collision>
810 |             <geometry>
811 |                 <box size="0.0285 0.034 0.0465"/>
812 |             </geometry>
813 |             <origin xyz="0 0 0.01225" rpy="0 0 0"/>
814 |         </collision>
815 |     </link>
816 | 
817 |     <joint name="fixed_ankleMotorLB" type="fixed">
818 |         <parent link="ankleCylinderLY"/>
819 |         <child link="ankleMotorLB"/>
820 |     </joint>
821 | 
822 |     <link name="ankleMotorLB">
823 |         <visual>
824 |             <geometry>
825 |                 <box size="0.034 0.0285 0.0465"/>
826 |             </geometry>
827 |             <origin xyz="-0.0315 0 0.01225" rpy="0 0 0"/>
828 |             <axis xyz="0 0 0" />
829 |             <material name="motorColor"/>
830 |         </visual>
831 |         <inertial>
832 |             <mass value="0.058"/>
833 |             <inertia ixx="1.4376750000000003e-05" ixy="0.0" ixz="0.0" iyy="1.6038208333333333e-05" iyz="0.0" izz="9.513208333333336e-06"/>
834 |         </inertial>
835 |         <collision>
836 |             <geometry>
837 |                 <box size="0.034 0.0285 0.0465"/>
838 |             </geometry>
839 |             <origin xyz="-0.0315 0 0.01225" rpy="0 0 0"/>
840 |         </collision>
841 |     </link>
842 | 
843 |     <joint name="fixed_footBridgeL" type="fixed">
844 |         <parent link="ankleCylinderLX"/>
845 |         <child link="footBridgeL"/>
846 |     </joint>
847 | 
848 |     <link name = "footBridgeL">
849 |         <visual>
850 |             <geometry>
851 |                 <box size="0.0685 0.02 0.025"/>
852 |             </geometry>
853 |             <origin xyz="-0.017 0 -0.0125" rpy="0 0 0"/>
854 |             <axis xyz="0 0 0" />
855 |             <material name="linkColor"/>
856 |         </visual>
857 |         <inertial>
858 |             <mass value="0.009"/>
859 |             <inertia ixx="8.541666666666668e-07" ixy="0.0" ixz="0.0" iyy="4.431041666666667e-06" iyz="0.0" izz="4.243541666666668e-06"/>
860 |         </inertial>
861 |         <collision>
862 |             <geometry>
863 |                 <box size="0.0685 0.02 0.025"/>
864 |             </geometry>
865 |             <origin xyz="-0.017 0 -0.0125" rpy="0 0 0"/>
866 |         </collision>
867 |     </link>
868 | 
869 |     <joint name="fixed_ankleCylinderLX" type="fixed">
870 |         <parent link="ankleCylinderLX"/>
871 |         <child link="footL"/>
872 |     </joint>
873 | 
874 |     <link name="footL">
875 |         <visual>
876 |             <geometry>
877 |                 <box size="0.10 0.05 0.005"/>
878 |             </geometry>
879 |             <origin xyz="0.0 0.0 -0.0275" rpy="0 0 0"/>
880 |             <axis xyz="0 0 0" />
881 |             <material name="footColor"/>
882 |         </visual>
883 |         <inertial>
884 |             <mass value="0.025"/>
885 |             <inertia ixx="1.026041666666667e-05" ixy="0.0" ixz="0.0" iyy="4.692708333333334e-05" iyz="0.0" izz="5.7083333333333336e-05"/>
886 |         </inertial>
887 |         <!--
888 |         <collision>
889 | 			<geometry>
890 | 				<box size="0.15 0.04 0.02"/>
891 | 			</geometry>
892 | 		    <origin xyz="0.02 0.015 -0.02" rpy="0 0 0"/>
893 |         </collision>
894 |         <collision>
895 | 			<geometry>
896 | 				<box size="0.12 0.07 0.02"/>
897 | 			</geometry>
898 | 		    <origin xyz="0.02 0.015 -0.02" rpy="0 0 0"/>
899 |         </collision>
900 |         -->
901 |         <collision>
902 |             <geometry>
903 |                 <box size="0.10 0.05 0.02"/>
904 |             </geometry>
905 |             <origin xyz="0.00 0.000 -0.02" rpy="0 0 0"/>
906 |         </collision>
907 |     </link>
908 | </robot>
909 | 


--------------------------------------------------------------------------------
/motorController.py:
--------------------------------------------------------------------------------
  1 | """
  2 | This helps control motors in pybullet physics simulator.
  3 | https://github.com/Einsbon
  4 | https://blog.naver.com/einsbon
  5 | """
  6 | 
  7 | import numpy as np
  8 | import time
  9 | import pybullet as p
 10 | 
 11 | 
 12 | class MotorController:
 13 | 
 14 |     def __init__(self, robot, physicsClientId, timeStep, kp, kd, torque, max_velocity):
 15 |         self._robot = robot
 16 |         self._physicsClientId = physicsClientId
 17 |         jointNameToId = {}
 18 |         joint_id_list = []
 19 |         joint_pos_list = []
 20 |         self._joint_number = 0
 21 |         for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
 22 |             jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
 23 |             if jointInfo[2] == 0:
 24 |                 joint_id_list.append(jointInfo[0])
 25 |                 joint_pos_list.append(p.getJointState(self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
 26 |                 jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
 27 |                 self._joint_number += 1
 28 |         self._joint_id = np.array(joint_id_list, dtype=np.int32)
 29 |         self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
 30 |         self._joint_currentPos = np.array(joint_pos_list, dtype=np.float)
 31 | 
 32 |         self._jointNameToId = jointNameToId
 33 |         self._kp = kp
 34 |         self._kd = kd
 35 |         self._torque = torque
 36 |         self._max_velocity = max_velocity
 37 |         self._timeStep = timeStep
 38 |         # print(self._joint_id)
 39 |         # print(self._joint_targetPos)
 40 |         # print(self._jointNameToId)
 41 | 
 42 |     def setRobot(self, robot, physicsClientId=None):
 43 |         self._robot = robot
 44 |         if physicsClientId != None:
 45 |             self._physicsClientId = physicsClientId
 46 |         jointNameToId = {}
 47 |         joint_id_list = []
 48 |         joint_pos_list = []
 49 |         self._joint_number = 0
 50 |         for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
 51 |             jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
 52 |             if jointInfo[2] == 0:
 53 |                 joint_id_list.append(jointInfo[0])
 54 |                 joint_pos_list.append(p.getJointState(self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
 55 |                 jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
 56 |                 self._joint_number += 1
 57 |         self._joint_id = np.array(joint_id_list, dtype=np.int32)
 58 |         self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
 59 |         self._joint_currentPos = np.array(joint_pos_list, dtype=np.float)
 60 | 
 61 |     def setMotorParameters(self, kp=None, kd=None, torque=None, maxVelocity=None, timeStep=None):
 62 |         if kp != None:
 63 |             self._kp = kp
 64 |         if kd != None:
 65 |             self._kd = kd
 66 |         if torque != None:
 67 |             self._torque = torque
 68 |         if maxVelocity != None:
 69 |             self._max_velocity = maxVelocity
 70 |         if timeStep != None:
 71 |             self._timeStep = timeStep
 72 | 
 73 |     def getRevoluteJoint_nameToId(self):
 74 |         return self._jointNameToId
 75 | 
 76 |     def getMotorAngle(self):
 77 |         for i in range(self._joint_number):
 78 |             self._joint_currentPos[i] = p.getJointState(self._robot, self._joint_id[i], physicsClientId=self._physicsClientId)[0]
 79 |         return self._joint_currentPos
 80 | 
 81 |     def setMotorAngle(self, motorTargetAngles):
 82 |         for i in range(self._joint_number):
 83 |             self._joint_targetPos[i] = motorTargetAngles[i]
 84 |             p.setJointMotorControl2(
 85 |                 bodyIndex=self._robot,
 86 |                 jointIndex=self._joint_id[i],
 87 |                 controlMode=p.POSITION_CONTROL,
 88 |                 targetPosition=self._joint_targetPos[i],
 89 |                 positionGain=self._kp,
 90 |                 velocityGain=self._kd,
 91 |                 force=self._torque,
 92 |                 maxVelocity=self._max_velocity,
 93 |                 physicsClientId=self._physicsClientId)
 94 | 
 95 |     def setMotorsAngleInRealTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
 96 |         if (motorTargetTime == 0):
 97 |             self._joint_targetPos = np.array(motorTargetAngles)
 98 |             for i in range(self._joint_number):
 99 |                 p.setJointMotorControl2(
100 |                     bodyIndex=self._robot,
101 |                     jointIndex=self._joint_id[i],
102 |                     controlMode=p.POSITION_CONTROL,
103 |                     targetPosition=self._joint_targetPos[i],
104 |                     positionGain=self._kp,
105 |                     velocityGain=self._kd,
106 |                     force=self._torque,
107 |                     maxVelocity=self._max_velocity,
108 |                     physicsClientId=self._physicsClientId)
109 |             time.sleep(delayTime)
110 |         else:
111 |             self._joint_currentPos = self._joint_targetPos
112 |             self._joint_targetPos = np.array(motorTargetAngles)
113 |             for i in range(self._joint_number):
114 |                 dydt = (self._joint_targetPos - self._joint_currentPos) / motorTargetTime
115 |             internalTime = 0.0
116 |             reft = time.time()
117 |             while internalTime < motorTargetTime:
118 |                 internalTime = time.time() - reft
119 |                 for i in range(self._joint_number):
120 |                     p.setJointMotorControl2(
121 |                         bodyIndex=self._robot,
122 |                         jointIndex=self._joint_id[i],
123 |                         controlMode=p.POSITION_CONTROL,
124 |                         targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
125 |                         positionGain=self._kp,
126 |                         velocityGain=self._kd,
127 |                         force=self._torque,
128 |                         maxVelocity=self._max_velocity,
129 |                         physicsClientId=self._physicsClientId)
130 | 
131 |     def setMotorsAngleInFixedTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
132 |         if (motorTargetTime == 0):
133 |             self._joint_targetPos = np.array(motorTargetAngles)
134 |             for i in range(self._joint_number):
135 |                 p.setJointMotorControl2(
136 |                     bodyIndex=self._robot,
137 |                     jointIndex=self._joint_id[i],
138 |                     controlMode=p.POSITION_CONTROL,
139 |                     targetPosition=self._joint_targetPos[i],
140 |                     positionGain=self._kp,
141 |                     velocityGain=self._kd,
142 |                     force=self._torque,
143 |                     maxVelocity=self._max_velocity,
144 |                     physicsClientId=self._physicsClientId)
145 |                 p.stepSimulation(physicsClientId=self._physicsClientId)
146 |         else:
147 |             self._joint_currentPos = self._joint_targetPos
148 |             self._joint_targetPos = np.array(motorTargetAngles)
149 |             for i in range(self._joint_number):
150 |                 dydt = (self._joint_targetPos - self._joint_currentPos) / motorTargetTime
151 |             internalTime = 0.0
152 |             while internalTime < motorTargetTime:
153 |                 internalTime += self._timeStep
154 |                 for i in range(self._joint_number):
155 |                     p.setJointMotorControl2(
156 |                         bodyIndex=self._robot,
157 |                         jointIndex=self._joint_id[i],
158 |                         controlMode=p.POSITION_CONTROL,
159 |                         targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
160 |                         positionGain=self._kp,
161 |                         velocityGain=self._kd,
162 |                         force=self._torque,
163 |                         maxVelocity=self._max_velocity,
164 |                         physicsClientId=self._physicsClientId)
165 |                 p.stepSimulation(physicsClientId=self._physicsClientId)
166 | 
167 |         if delayTime != 0:
168 |             for _ in range(int(delayTime / self._timeStep)):
169 |                 p.stepSimulation(physicsClientId=self._physicsClientId)
170 | 


--------------------------------------------------------------------------------
/play_various_walking.py:
--------------------------------------------------------------------------------
  1 | """
  2 | bipedal robot walking simulation
  3 | 
  4 | by Einsbon (Sunbin Kim)
  5 | - GitHub: https://github.com/Einsbon
  6 | - Youtube:  https://www.youtube.com/channel/UCt7FZ-8uzV_jHJiKp3NlHvg
  7 | - Blog: https://blog.naver.com/einsbon
  8 | """
  9 | 
 10 | import pybullet as p
 11 | import time
 12 | from time import sleep
 13 | import pybullet_data
 14 | import numpy as np
 15 | import math
 16 | import os
 17 | 
 18 | import motorController
 19 | import walkGenerator
 20 | 
 21 | # motor setting
 22 | motor_kp = 0.5
 23 | motor_kd = 0.5
 24 | motor_torque = 2
 25 | motor_max_velocity = 10.0
 26 | 
 27 | # physics parameter setting
 28 | fixedTimeStep = 1. / 1000
 29 | numSolverIterations = 200
 30 | 
 31 | physicsClient = p.connect(p.GUI)
 32 | p.setTimeStep(timeStep=fixedTimeStep, physicsClientId=physicsClient)
 33 | p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
 34 | p.setAdditionalSearchPath(pybullet_data.getDataPath())  # to load ground
 35 | 
 36 | p.setGravity(0, 0, 0)
 37 | p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=10, cameraPitch=-5, cameraTargetPosition=[0.3, 0.5, 0.1], physicsClientId=physicsClient)
 38 | 
 39 | planeId = p.loadSDF('stadium.sdf')  # or p.loadURDF('samurai.urdf')  # p.loadURDF('plane.urdf')
 40 | 
 41 | robot = p.loadURDF(os.path.abspath(os.path.dirname(__file__)) + '/humanoid_leg_12dof.8.urdf', [0, 0, 0.31],
 42 |                    p.getQuaternionFromEuler([0, 0, 0]),
 43 |                    useFixedBase=False)
 44 | 
 45 | controller = motorController.MotorController(robot, physicsClient, fixedTimeStep, motor_kp, motor_kd, motor_torque, motor_max_velocity)
 46 | 
 47 | walk = walkGenerator.WalkGenerator()
 48 | walk.setWalkParameter(bodyMovePoint=8,
 49 |                       legMovePoint=8,
 50 |                       height=50,
 51 |                       stride=90,
 52 |                       sit=40,
 53 |                       swayBody=30,
 54 |                       swayFoot=0,
 55 |                       bodyPositionForwardPlus=5,
 56 |                       swayShift=3,
 57 |                       liftPush=0.4,
 58 |                       landPull=0.6,
 59 |                       timeStep=0.06,
 60 |                       damping=0.0,
 61 |                       incline=0.0)
 62 | walk.generate()
 63 | walk.inverseKinematicsAll()
 64 | 
 65 | actionTime = walk._timeStep
 66 | p.setGravity(0, 0, -9.8)
 67 | p.setRealTimeSimulation(0)
 68 | controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 1, 0)
 69 | 
 70 | waitTime = 1
 71 | repeatTime = int(waitTime / fixedTimeStep)
 72 | for _ in range(repeatTime):
 73 |     p.stepSimulation()
 74 | 
 75 | p.setGravity(0, 0, -9.8)
 76 | 
 77 | # walk 8 steps
 78 | # start walking. right foot step
 79 | for i in range(np.size(walk.walkAnglesStartRight, 0)):
 80 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i], actionTime, 0)
 81 | for i in range(2):
 82 |     # left foot step
 83 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
 84 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i], actionTime, 0)
 85 |     # right foot step
 86 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
 87 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i], actionTime, 0)
 88 | # end walking. left
 89 | for i in range(np.size(walk.walkAnglesEndLeft, 0)):
 90 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i], actionTime, 0)
 91 | 
 92 | # rest 2 seconds
 93 | waitTime = 2
 94 | repeatTime = int(waitTime / fixedTimeStep)
 95 | for _ in range(repeatTime):
 96 |     p.stepSimulation()
 97 | 
 98 | ########################################################
 99 | p.resetBasePositionAndOrientation(robot, [0, 0, 0.31], p.getQuaternionFromEuler([0, 0, 0]))
100 | walk.setWalkParameter(bodyMovePoint=8,
101 |                       legMovePoint=8,
102 |                       height=50,
103 |                       stride=90,
104 |                       sit=70,
105 |                       swayBody=30,
106 |                       swayFoot=0,
107 |                       bodyPositionForwardPlus=5,
108 |                       swayShift=3,
109 |                       liftPush=0.4,
110 |                       landPull=0.6,
111 |                       timeStep=0.06,
112 |                       damping=0.0,
113 |                       incline=0.0)
114 | walk.generate()
115 | walk.inverseKinematicsAll()
116 | actionTime = walk._timeStep
117 | controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 2, 0)
118 | 
119 | waitTime = 1
120 | repeatTime = int(waitTime / fixedTimeStep)
121 | for _ in range(repeatTime):
122 |     p.stepSimulation()
123 | 
124 | p.setGravity(0, 0, -9.8)
125 | 
126 | for i in range(np.size(walk.walkAnglesStartRight, 0)):
127 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i], actionTime, 0)
128 | for i in range(2):
129 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
130 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i], actionTime, 0)
131 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
132 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i], actionTime, 0)
133 | for i in range(np.size(walk.walkAnglesEndLeft, 0)):
134 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i], actionTime, 0)
135 | 
136 | waitTime = 2
137 | repeatTime = int(waitTime / fixedTimeStep)
138 | for _ in range(repeatTime):
139 |     p.stepSimulation()
140 | 
141 | ########################################################
142 | p.resetBasePositionAndOrientation(robot, [0, 0, 0.31], p.getQuaternionFromEuler([0, 0, 0]))
143 | 
144 | walk.setWalkParameter(bodyMovePoint=8,
145 |                       legMovePoint=8,
146 |                       height=20,
147 |                       stride=40,
148 |                       sit=40,
149 |                       swayBody=15,
150 |                       swayFoot=0,
151 |                       bodyPositionForwardPlus=5,
152 |                       swayShift=3,
153 |                       liftPush=0.4,
154 |                       landPull=0.6,
155 |                       timeStep=0.03,
156 |                       damping=0.0,
157 |                       incline=0.0)
158 | walk.generate()
159 | walk.inverseKinematicsAll()
160 | actionTime = walk._timeStep
161 | controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 2, 0)
162 | 
163 | waitTime = 1
164 | repeatTime = int(waitTime / fixedTimeStep)
165 | for _ in range(repeatTime):
166 |     p.stepSimulation()
167 | 
168 | for i in range(np.size(walk.walkAnglesStartRight, 0)):
169 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i], actionTime, 0)
170 | for i in range(2):
171 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
172 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i], actionTime, 0)
173 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
174 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i], actionTime, 0)
175 | for i in range(np.size(walk.walkAnglesEndLeft, 0)):
176 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i], actionTime, 0)
177 | 
178 | waitTime = 2
179 | repeatTime = int(waitTime / fixedTimeStep)
180 | for _ in range(repeatTime):
181 |     p.stepSimulation()
182 | 
183 | ########################################################
184 | p.resetBasePositionAndOrientation(robot, [0, 0, 0.31], p.getQuaternionFromEuler([0, 0, 0]))
185 | 
186 | walk.setWalkParameter(bodyMovePoint=8,
187 |                       legMovePoint=8,
188 |                       height=50,
189 |                       stride=140,
190 |                       sit=40,
191 |                       swayBody=35,
192 |                       swayFoot=0,
193 |                       bodyPositionForwardPlus=-2,
194 |                       swayShift=3,
195 |                       liftPush=0.4,
196 |                       landPull=0.6,
197 |                       timeStep=0.12,
198 |                       damping=0.0,
199 |                       incline=0.0)
200 | walk.generate()
201 | walk.inverseKinematicsAll()
202 | actionTime = walk._timeStep
203 | controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 2, 0)
204 | 
205 | waitTime = 1
206 | repeatTime = int(waitTime / fixedTimeStep)
207 | for _ in range(repeatTime):
208 |     p.stepSimulation()
209 | 
210 | for i in range(np.size(walk.walkAnglesStartRight, 0)):
211 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i], actionTime, 0)
212 | for i in range(2):
213 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
214 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i], actionTime, 0)
215 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
216 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i], actionTime, 0)
217 | for i in range(np.size(walk.walkAnglesEndLeft, 0)):
218 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i], actionTime, 0)
219 | 
220 | waitTime = 2
221 | repeatTime = int(waitTime / fixedTimeStep)
222 | for _ in range(repeatTime):
223 |     p.stepSimulation()
224 | 
225 | ########################################################
226 | p.resetBasePositionAndOrientation(robot, [0, 0, 0.31], p.getQuaternionFromEuler([0, 0, 0]))
227 | 
228 | walk.setWalkParameter(bodyMovePoint=8,
229 |                       legMovePoint=8,
230 |                       height=40,
231 |                       stride=70,
232 |                       sit=40,
233 |                       swayBody=30,
234 |                       swayFoot=0,
235 |                       bodyPositionForwardPlus=-35,
236 |                       swayShift=3,
237 |                       liftPush=0.4,
238 |                       landPull=0.6,
239 |                       timeStep=0.06,
240 |                       damping=0.0,
241 |                       incline=0.0)
242 | walk.generate()
243 | walk.inverseKinematicsAll()
244 | actionTime = walk._timeStep
245 | controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 2, 0)
246 | 
247 | waitTime = 1
248 | repeatTime = int(waitTime / fixedTimeStep)
249 | for _ in range(repeatTime):
250 |     p.stepSimulation()
251 | 
252 | for i in range(np.size(walk.walkAnglesStartRight, 0)):
253 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i], actionTime, 0)
254 | for i in range(2):
255 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
256 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i], actionTime, 0)
257 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
258 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i], actionTime, 0)
259 | for i in range(np.size(walk.walkAnglesEndLeft, 0)):
260 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i], actionTime, 0)
261 | 
262 | waitTime = 2
263 | repeatTime = int(waitTime / fixedTimeStep)
264 | for _ in range(repeatTime):
265 |     p.stepSimulation()
266 | 
267 | ########################################################
268 | p.resetBasePositionAndOrientation(robot, [0, 0, 0.28], p.getQuaternionFromEuler([0, 0, 0]))
269 | 
270 | walk.setWalkParameter(bodyMovePoint=8,
271 |                       legMovePoint=8,
272 |                       height=50,
273 |                       stride=-90,
274 |                       sit=40,
275 |                       swayBody=30,
276 |                       swayFoot=0,
277 |                       bodyPositionForwardPlus=0,
278 |                       swayShift=3,
279 |                       liftPush=0.4,
280 |                       landPull=0.6,
281 |                       timeStep=0.06,
282 |                       damping=0.0,
283 |                       incline=0.0)
284 | walk.generate()
285 | walk.inverseKinematicsAll()
286 | actionTime = walk._timeStep
287 | controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 2, 0)
288 | 
289 | waitTime = 1
290 | repeatTime = int(waitTime / fixedTimeStep)
291 | for _ in range(repeatTime):
292 |     p.stepSimulation()
293 | 
294 | for i in range(np.size(walk.walkAnglesStartRight, 0)):
295 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i], actionTime, 0)
296 | for i in range(2):  # repeat twice
297 |     # left foot step
298 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
299 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i], actionTime, 0)
300 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
301 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i], actionTime, 0)
302 | for i in range(np.size(walk.walkAnglesEndLeft, 0)):
303 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i], actionTime, 0)
304 | 
305 | waitTime = 2
306 | repeatTime = int(waitTime / fixedTimeStep)
307 | for _ in range(repeatTime):
308 |     p.stepSimulation()
309 | 
310 | ########################################################
311 | p.resetBasePositionAndOrientation(robot, [0, 0, 0.31], p.getQuaternionFromEuler([0, 0, 0]))
312 | 
313 | walk.setWalkParameter(bodyMovePoint=8,
314 |                       legMovePoint=8,
315 |                       height=50,
316 |                       stride=0,
317 |                       sit=40,
318 |                       swayBody=30,
319 |                       swayFoot=0,
320 |                       bodyPositionForwardPlus=5,
321 |                       swayShift=3,
322 |                       liftPush=0.4,
323 |                       landPull=0.6,
324 |                       timeStep=0.06,
325 |                       damping=0.0,
326 |                       incline=0.0)
327 | walk.generate()
328 | walk.inverseKinematicsAll()
329 | actionTime = walk._timeStep
330 | controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 2, 0)
331 | 
332 | waitTime = 1
333 | repeatTime = int(waitTime / fixedTimeStep)
334 | for _ in range(repeatTime):
335 |     p.stepSimulation()
336 | 
337 | # Turn function is not accurate yet.
338 | for i in range(np.size(walk.walkAnglesStartRight, 0)):
339 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i] + walk.turnListUnfold[i] * 0.3, actionTime, 0)
340 | for i in range(3):
341 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
342 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i] + walk.turnListFold[i] * 0.3, actionTime, 0)
343 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
344 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i] + walk.turnListUnfold[i] * 0.3, actionTime, 0)
345 | for i in range(np.size(walk.walkAnglesEndLeft, 0)):
346 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i] + walk.turnListFold[i] * 0.3, actionTime, 0)
347 | 
348 | waitTime = 2
349 | repeatTime = int(waitTime / fixedTimeStep)
350 | for _ in range(repeatTime):
351 |     p.stepSimulation()
352 | 
353 | ########################################################
354 | p.resetBasePositionAndOrientation(robot, [0, 0, 0.31], p.getQuaternionFromEuler([0, 0, 0]))
355 | 
356 | walk.setWalkParameter(bodyMovePoint=8,
357 |                       legMovePoint=8,
358 |                       height=50,
359 |                       stride=90,
360 |                       sit=40,
361 |                       swayBody=30,
362 |                       swayFoot=0,
363 |                       bodyPositionForwardPlus=5,
364 |                       swayShift=3,
365 |                       liftPush=0.4,
366 |                       landPull=0.6,
367 |                       timeStep=0.06,
368 |                       damping=0.0,
369 |                       incline=0.0)
370 | walk.generate()
371 | walk.inverseKinematicsAll()
372 | actionTime = walk._timeStep
373 | controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 2, 0)
374 | 
375 | waitTime = 1
376 | repeatTime = int(waitTime / fixedTimeStep)
377 | for _ in range(repeatTime):
378 |     p.stepSimulation()
379 | 
380 | for i in range(np.size(walk.walkAnglesStartRight, 0)):
381 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i] + walk.turnListUnfold[i] * 0.3, actionTime, 0)
382 | for i in range(3):
383 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
384 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i] + walk.turnListFold[i] * 0.3, actionTime, 0)
385 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
386 |         controller.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i] + walk.turnListUnfold[i] * 0.3, actionTime, 0)
387 | for i in range(np.size(walk.walkAnglesEndLeft, 0)):
388 |     controller.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i] + walk.turnListFold[i] * 0.3, actionTime, 0)
389 | 
390 | ########################################################
391 | fixedTimeStep = 1 / 500
392 | p.setTimeStep(fixedTimeStep)
393 | 
394 | giantRobot = p.loadURDF(os.path.abspath(os.path.dirname(__file__)) + '/humanoid_leg_12dof.8.urdf', [-4.7, 0, 3.1],
395 |                         p.getQuaternionFromEuler([0, 0, 0]),
396 |                         useFixedBase=False,
397 |                         globalScaling=10)
398 | 
399 | for i in range(p.getNumJoints(giantRobot)):
400 |     p.changeDynamics(
401 |         giantRobot,
402 |         i,
403 |         lateralFriction=1,
404 |         spinningFriction=1,
405 |     )
406 | 
407 | waitTime = 2
408 | repeatTime = int(waitTime / fixedTimeStep)
409 | for _ in range(repeatTime):
410 |     p.stepSimulation()
411 | 
412 | controller2 = motorController.MotorController(giantRobot, physicsClient, fixedTimeStep, motor_kp, motor_kd, 50, motor_max_velocity)
413 | walk.setWalkParameter(bodyMovePoint=12,
414 |                       legMovePoint=12,
415 |                       height=30,
416 |                       stride=90,
417 |                       sit=35,
418 |                       swayBody=18,
419 |                       swayFoot=0,
420 |                       bodyPositionForwardPlus=5,
421 |                       swayShift=4,
422 |                       liftPush=0.7,
423 |                       landPull=0.8,
424 |                       timeStep=0.06,
425 |                       damping=0.0,
426 |                       incline=0.0)
427 | walk.generate()
428 | walk.inverseKinematicsAll()
429 | walk.showGaitPoint3D()
430 | actionTime = walk._timeStep
431 | controller2.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 4, 0)
432 | 
433 | waitTime = 1
434 | repeatTime = int(waitTime / fixedTimeStep)
435 | for _ in range(repeatTime):
436 |     p.stepSimulation()
437 | 
438 | for i in range(np.size(walk.walkAnglesStartRight, 0)):
439 |     controller2.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i], actionTime, 0)
440 | for i in range(2):
441 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
442 |         controller2.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i], actionTime, 0)
443 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
444 |         controller2.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i], actionTime, 0)
445 | for i in range(np.size(walk.walkAnglesEndLeft, 0)):
446 |     controller2.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i], actionTime, 0)
447 | 
448 | controller2.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([-10, -40, 30], [-10, -40, 30]), 1, 0.5)
449 | controller2.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([-80, -35, 50], [-10, -40, 30]), 1.5, 0.5)
450 | controller2.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([-10, -35, 45], [0, -40, 30]), 0.4, 0)
451 | controller2.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([60, -35, 45], [0, -40, 30]), 0.2, 3)
452 | controller2.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([0, -32, 45], [0, -32, 30]), 1, 0)
453 | controller2.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([0, 0, 10], [0, 0, 10]), 0.5, 50)
454 | 


--------------------------------------------------------------------------------
/screenshot1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Einsbon/bipedal-robot-walking-simulation/9a3348be92b016b17b71edbce20b5bcb60dc7d15/screenshot1.png


--------------------------------------------------------------------------------
/screenshot2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Einsbon/bipedal-robot-walking-simulation/9a3348be92b016b17b71edbce20b5bcb60dc7d15/screenshot2.png


--------------------------------------------------------------------------------
/walkGenerator.py:
--------------------------------------------------------------------------------
  1 | '''
  2 | by Einsbon (Sunbin Kim)
  3 | - GitHub: https://github.com/Einsbon
  4 | - Youtube:  https://www.youtube.com/channel/UCt7FZ-8uzV_jHJiKp3NlHvg
  5 | - Blog: https://blog.naver.com/einsbon
  6 | 
  7 | 
  8 | 
  9 | Robot's forward is direction X +
 10 |         left is Y+
 11 |         up is Z +
 12 | '''
 13 | 
 14 | import numpy as np
 15 | import matplotlib.pyplot as plt
 16 | import matplotlib as mpl
 17 | from mpl_toolkits.mplot3d import Axes3D
 18 | import math
 19 | 
 20 | 
 21 | class WalkGenerator():
 22 |     def __init__(self):
 23 |         #                                      0   1   2   3   4   5 
 24 |         self._motorDirectionRight = np.array([+1, +1, +1, +1, +1, +1])
 25 |         self._motorDirectionLeft = np.array([+1, +1, +1, +1, +1, +1])
 26 |         '''
 27 |         self._walkPoint0 = 0  # double support. point of the landed foot
 28 |         self._walkPoint1 = 0  # single support. point of the supporting foot
 29 |         self._walkPoint2 = 0  # double support. point of the foot to lift.
 30 |         self._walkPoint3 = 0  # single support. point of the swinging foot
 31 |         '''
 32 |         '''
 33 |         self.walkPointStartRightstepRightLeg = 0  # 오른쪽 발을 먼저 내밈. 그때의 오른쪽 발.
 34 |         self.walkPointStartRightstepLeftLeg = 0  # 오른쪽 발을 먼저 내밈. 그때의 왼쪽 발.
 35 | 
 36 |         self.walkPointStartLeftstepRightLeg = 0  # 왼쪽 발을 먼저 내밈. 그때의 오른쪽 발.
 37 |         self.walkPointStartLeftstepLeftLeg = 0  # 왼쪽 발을 먼저 내밈. 그때의 왼쪽 발.
 38 | 
 39 |         self.walkPointEndRightstepRightLeg = 0  # 오른쪽 발을 디디면서 끝남. 그때의 오른쪽 발.
 40 |         self.walkPointEndLeftstepRightLeg = 0  # 오른쪽 발을 디디면서 끝남. 그때의 왼쪽 발.
 41 |         self.walkPointEndLeftstepLeftLeg = 0
 42 |         self.walkPointEndRightstepLeftLeg = 0
 43 |         
 44 |         self.walkPointRightStepRightLeg = 0
 45 |         self.walkPointLeftStepRightLeg = 0
 46 | 
 47 |         self.walkPointRightStepLeftLeg = 0
 48 |         self.walkPointLeftStepLeftLeg = 0
 49 | 
 50 |         self.walkAnglesWalkingRight = 0
 51 |         self.walkAnglesWalkingLeft = 0
 52 |         self.walkAnglesStartRight = 0  # 왼쪽으로 sway 했다가 오른발을 먼저 내밈.
 53 |         self.walkAnglesStartLeft = 0  # 오른쪽으로 sway 했다가 왼발을 먼저 내밈.
 54 |         self.walkAnglesEndRight = 0  # 오른발을 디디면서 끝남.
 55 |         self.walkAnglesEndLeft = 0  # 왼발을 디디면서 끝남.
 56 | 
 57 |         self.turnListUnfold = 0
 58 |         self.turnListFold = 0
 59 |         '''
 60 |         # 로봇의 길이 설정. 길이 단위: mm
 61 |         self._pelvic_interval = 70.5
 62 |         self._legUp_length = 110
 63 |         self._legDown_length = 110
 64 |         self._footJoint_to_bottom = 45
 65 |         '''
 66 |         self._bodyMovePoint = 0
 67 |         self._legMovePoint = 0
 68 |         self._h = 0
 69 |         self._l = 0
 70 |         self._sit = 0
 71 |         self._swayBody = 0
 72 |         self._swayFoot = 0
 73 |         self._swayShift = 0
 74 |         self._liftPush = 0
 75 |         self._landPull = 0
 76 |         self._stepTime = 0
 77 |         self._bodyPositionXPlus = 0
 78 |         self._damping = 0
 79 |         self._incline = incline
 80 |         '''
 81 | 
 82 |     def setRobotParameter(self, pelvic_interval, leg_up_length, leg_down_length, foot_to_grount, foot_to_heel, foot_to_toe):
 83 |         pass
 84 | 
 85 |     def setWalkParameter(self, bodyMovePoint, legMovePoint, height, stride, sit, swayBody, swayFoot, bodyPositionForwardPlus, swayShift, liftPush=0.4, landPull=0.6, timeStep=0.1, damping=0, incline=0):
 86 |         self._bodyMovePoint = bodyMovePoint # the number of point when two feet are landed
 87 |         self._legMovePoint = legMovePoint   # the number of point when lift one foot
 88 |         self._h = height                    # foot lift height
 89 |         self._l = stride                    # stride length
 90 |         self._sit = sit                     # sit height. increase this will make leg more fold. too high or too low makes an error
 91 |         self._swayBody = swayBody           # body sway length
 92 |         self._swayFoot = swayFoot           # foot sway length. 0 -> feet move straight forward. plus this make floating leg spread.(increase gap between feet)
 93 |         self._swayShift = swayShift         # start point of sway
 94 |         self._liftPush = liftPush        # push the lifting foot backward when lifting the foot to gains momentum.
 95 |         self._landPull = landPull          # Before put the foot down, go forward more and pull back when landing.
 96 |         self._timeStep = timeStep           # simulation timeStep
 97 |         self._bodyPositionXPlus = bodyPositionForwardPlus  # plus this makes the body forward
 98 |         self._damping = damping             # damping at the start and end of foot lift.
 99 |         self._incline = incline             # tangent angle of incline
100 | 
101 |         self._stepPoint = bodyMovePoint + legMovePoint
102 | 
103 |     def generate(self):
104 |         walkPoint = self._bodyMovePoint * 2 + self._legMovePoint * 2
105 |         trajectoryLength = self._l * (2 * self._bodyMovePoint + self._legMovePoint) / (self._bodyMovePoint + self._legMovePoint)
106 | 
107 |         walkPoint0 = np.zeros((3, self._bodyMovePoint))
108 |         walkPoint1 = np.zeros((3, self._legMovePoint))
109 |         walkPoint2 = np.zeros((3, self._bodyMovePoint))
110 |         walkPoint3 = np.zeros((3, self._legMovePoint))
111 | 
112 |         self.walkPointStartRightstepRightLeg = np.zeros((3, self._bodyMovePoint + self._legMovePoint))
113 |         self.walkPointStartLeftstepRightLeg = np.zeros((3, self._bodyMovePoint + self._legMovePoint))
114 |         self.walkPointEndRightstepRightLeg = np.zeros((3, self._bodyMovePoint + self._legMovePoint))
115 |         self.walkPointEndLeftstepRightLeg = np.zeros((3, self._bodyMovePoint + self._legMovePoint))
116 | 
117 |         # walking motion
118 |         for i in range(self._bodyMovePoint):
119 |             t = (i + 1) / (walkPoint - self._legMovePoint)
120 |             walkPoint0[0][i] = -trajectoryLength * (t - 0.5)
121 |             walkPoint0[2][i] = self._sit
122 |             walkPoint0[1][i] = self._swayBody * math.sin(2 * math.pi * ((i + 1 - self._swayShift) / walkPoint))
123 | 
124 |         for i in range(self._legMovePoint):
125 |             t = (i + 1 + self._bodyMovePoint) / (walkPoint - self._legMovePoint)
126 |             walkPoint1[0][i] = -trajectoryLength * (t - 0.5)
127 |             walkPoint1[2][i] = self._sit
128 |             walkPoint1[1][i] = self._swayBody * math.sin(2 * math.pi * ((i + 1 + self._bodyMovePoint - self._swayShift) / walkPoint))
129 | 
130 |         for i in range(self._bodyMovePoint):
131 |             t = (i + 1 + self._bodyMovePoint + self._legMovePoint) / (walkPoint - self._legMovePoint)
132 |             walkPoint2[0][i] = -trajectoryLength * (t - 0.5)
133 |             walkPoint2[2][i] = self._sit
134 |             walkPoint2[1][i] = self._swayBody * math.sin(2 * math.pi * ((i + 1 + self._bodyMovePoint + self._legMovePoint - self._swayShift) / walkPoint))
135 | 
136 |         for i in range(self._legMovePoint):
137 |             t = (i + 1) / self._legMovePoint
138 |             sin_tpi = math.sin(t * math.pi)
139 | 
140 |             walkPoint3[0][i] = (2 * t - 1 + (1 - t) * self._liftPush * -sin_tpi + t * self._landPull * sin_tpi) * trajectoryLength / 2
141 |             walkPoint3[2][i] = math.sin(t * math.pi) * self._h + self._sit
142 |             walkPoint3[1][i] = math.sin(t * math.pi) * self._swayFoot + self._swayBody * math.sin(2 * math.pi * ((i + 1 + walkPoint - self._legMovePoint - self._swayShift) / walkPoint))
143 | 
144 |         # starting motion
145 |         for i in range(self._bodyMovePoint - self._swayShift):
146 |             t = (i + 1) / self._bodyMovePoint
147 |             self.walkPointStartRightstepRightLeg[0][i] = 0
148 |             self.walkPointStartRightstepRightLeg[2][i] = self._sit
149 | 
150 |             self.walkPointStartLeftstepRightLeg[0][i] = 0
151 |             self.walkPointStartLeftstepRightLeg[2][i] = self._sit
152 | 
153 |         for i in range(self._legMovePoint):
154 |             t = (i + 1) / self._legMovePoint
155 |             t2 = (i + 1) / (self._legMovePoint + self._swayShift)
156 |             sin_tpi = math.sin(t * math.pi)
157 | 
158 |             self.walkPointStartRightstepRightLeg[2][i + self._bodyMovePoint - self._swayShift] = math.sin(t * math.pi) * self._h + self._sit
159 |             self.walkPointStartRightstepRightLeg[0][i + self._bodyMovePoint - self._swayShift] = (2 * t + (1 - t) * self._liftPush * -sin_tpi + t * self._landPull * sin_tpi) * trajectoryLength / 4
160 |             self.walkPointStartLeftstepRightLeg[0][i + self._bodyMovePoint - self._swayShift] = (math.cos(t2 * math.pi / 2) - 1) * trajectoryLength * self._legMovePoint / (self._bodyMovePoint * 2 + self._legMovePoint) / 2
161 |             self.walkPointStartLeftstepRightLeg[0][i + self._bodyMovePoint - self._swayShift] = (math.cos(t2 * math.pi / 2) - 1) * trajectoryLength * ((self._swayShift + self._bodyMovePoint + self._legMovePoint) / (self._bodyMovePoint * 2 + self._legMovePoint) - 0.5)
162 | 
163 |             self.walkPointStartLeftstepRightLeg[2][i + self._bodyMovePoint - self._swayShift] = self._sit
164 | 
165 |         for i in range(self._swayShift):
166 |             t2 = (i + 1 + self._legMovePoint) / (self._legMovePoint + self._swayShift)
167 | 
168 |             self.walkPointStartRightstepRightLeg[0][i + self._legMovePoint + self._bodyMovePoint - self._swayShift] = -trajectoryLength * ((i + 1) / (walkPoint - self._legMovePoint) - 0.5)
169 |             self.walkPointStartRightstepRightLeg[2][i + self._legMovePoint + self._bodyMovePoint - self._swayShift] = self._sit
170 |             self.walkPointStartLeftstepRightLeg[0][i + self._legMovePoint + self._bodyMovePoint - self._swayShift] = -trajectoryLength * ((i + 1 + self._bodyMovePoint + self._legMovePoint) / (walkPoint - self._legMovePoint) - 0.5)
171 |             self.walkPointStartLeftstepRightLeg[0][i + self._legMovePoint + self._bodyMovePoint - self._swayShift] = (math.cos(t2 * math.pi / 2) - 1) * trajectoryLength * ((self._swayShift + self._bodyMovePoint + self._legMovePoint) / (self._bodyMovePoint * 2 + self._legMovePoint) - 0.5)
172 |             self.walkPointStartLeftstepRightLeg[2][i + self._legMovePoint + self._bodyMovePoint - self._swayShift] = self._sit
173 | 
174 |         for i in range(self._bodyMovePoint + self._legMovePoint):
175 |             t = (i + 1) / (self._bodyMovePoint + self._legMovePoint)
176 |             #self.walkPointStartRightstepRightLeg[1][i] = -self._swayBody * math.sin(t*math.pi) * math.sin(t*math.pi)
177 |             #self.walkPointStartLeftstepRightLeg[1][i] = self._swayBody * math.sin(t*math.pi) * math.sin(t*math.pi)
178 |             if t < 1 / 4:
179 |                 self.walkPointStartRightstepRightLeg[1][i] = -self._swayBody * (math.sin(t * math.pi) - (1 - math.sin(math.pi * 2 * t)) * (math.sin(4 * t * math.pi) / 4))
180 |                 self.walkPointStartLeftstepRightLeg[1][i] = self._swayBody * (math.sin(t * math.pi) - (1 - math.sin(math.pi * 2 * t)) * (math.sin(4 * t * math.pi) / 4))
181 |             else:
182 |                 self.walkPointStartRightstepRightLeg[1][i] = -self._swayBody * math.sin(t * math.pi)
183 |                 self.walkPointStartLeftstepRightLeg[1][i] = self._swayBody * math.sin(t * math.pi)
184 | 
185 |         # ending motion. 마무리 동작. 왼발이 뜸. 그러나 둘다 오른쪽다리 기준
186 |         for i in range(self._bodyMovePoint - self._swayShift):
187 |             self.walkPointEndLeftstepRightLeg[0][i] = -trajectoryLength * \
188 |                 ((i+1+self._swayShift)/(walkPoint-self._legMovePoint)-0.5)
189 |             self.walkPointEndLeftstepRightLeg[2][i] = self._sit
190 | 
191 |             self.walkPointEndRightstepRightLeg[0][i] = -trajectoryLength * \
192 |                 ((i + 1 + self._swayShift + self._bodyMovePoint+self._legMovePoint)/(walkPoint-self._legMovePoint)-0.5)
193 |             self.walkPointEndRightstepRightLeg[2][i] = self._sit
194 |         for i in range(self._legMovePoint):
195 |             t = (i + 1) / self._legMovePoint
196 |             sin_tpi = math.sin(t * math.pi)
197 | 
198 |             self.walkPointEndLeftstepRightLeg[0][i + self._bodyMovePoint - self._swayShift] = (math.sin(t * math.pi / 2) - 1) * trajectoryLength * ((self._bodyMovePoint) / (self._bodyMovePoint * 2 + self._legMovePoint) - 0.5)
199 |             self.walkPointEndLeftstepRightLeg[2][i + self._bodyMovePoint - self._swayShift] = self._sit
200 | 
201 |             self.walkPointEndRightstepRightLeg[0][i + self._bodyMovePoint - self._swayShift] = (2 * t - 2 + (1 - t) * self._liftPush * -sin_tpi + t * self._landPull * sin_tpi) * trajectoryLength / 4
202 |             self.walkPointEndRightstepRightLeg[2][i + self._bodyMovePoint - self._swayShift] = math.sin(t * math.pi) * self._h + self._sit
203 |         for i in range(self._swayShift):
204 |             self.walkPointEndLeftstepRightLeg[0][i + self._bodyMovePoint + self._legMovePoint - self._swayShift] = 0
205 |             self.walkPointEndLeftstepRightLeg[2][i + self._bodyMovePoint + self._legMovePoint - self._swayShift] = self._sit
206 | 
207 |             self.walkPointEndRightstepRightLeg[0][i + self._bodyMovePoint + self._legMovePoint - self._swayShift] = 0
208 |             self.walkPointEndRightstepRightLeg[2][i + self._bodyMovePoint + self._legMovePoint - self._swayShift] = self._sit
209 | 
210 |         # turn
211 | 
212 |         self.turnListUnfold = np.zeros((self._bodyMovePoint + self._legMovePoint, 12))
213 |         self.turnListFold = np.zeros((self._bodyMovePoint + self._legMovePoint, 12))
214 |         turnAngle = np.zeros(self._bodyMovePoint + self._legMovePoint)
215 |         for i in range(self._legMovePoint):
216 |             t = (i + 1) / self._legMovePoint
217 |             turnAngle[self._bodyMovePoint - self._swayShift + i] = (1 - math.cos(math.pi * t)) / 4
218 |         for i in range(self._swayShift):
219 |             turnAngle[self._bodyMovePoint + self._legMovePoint - self._swayShift + i] = 1 / 2
220 | 
221 |         for i in range(self._bodyMovePoint + self._legMovePoint):
222 |             self.turnListUnfold[i] = [-turnAngle[i], 0, 0, 0, 0, 0, +turnAngle[i], 0, 0, 0, 0, 0]
223 |             self.turnListFold[i] = [-0.5 + turnAngle[i], 0, 0, 0, 0, 0, +0.5 - turnAngle[i], 0, 0, 0, 0, 0]
224 | 
225 |         for i in range(self._bodyMovePoint + self._legMovePoint):
226 |             t = 1 - (i + 1) / (self._bodyMovePoint + self._legMovePoint)
227 | 
228 |             if t < 1 / 4:
229 |                 self.walkPointEndLeftstepRightLeg[1][i] = self._swayBody * (math.sin(t * math.pi) - (1 - math.sin(math.pi * 2 * t)) * (math.sin(4 * t * math.pi) / 4))
230 |                 self.walkPointEndRightstepRightLeg[1][i] = -self._swayBody * (math.sin(t * math.pi) - (1 - math.sin(math.pi * 2 * t)) * (math.sin(4 * t * math.pi) / 4))
231 |             else:
232 |                 self.walkPointEndLeftstepRightLeg[1][i] = self._swayBody * math.sin(t * math.pi)
233 |                 self.walkPointEndRightstepRightLeg[1][i] = -self._swayBody * math.sin(t * math.pi)
234 | 
235 |         # 추가 파라미터의 조정
236 | 
237 |         if self._incline != 0:  # 기울기. 계단 등에서 사용.
238 |             walkPoint0[2] = walkPoint0[2] + walkPoint0[0] * self._incline
239 |             walkPoint1[2] = walkPoint1[2] + walkPoint1[0] * self._incline
240 |             walkPoint2[2] = walkPoint2[2] + walkPoint2[0] * self._incline
241 |             walkPoint3[2] = walkPoint3[2] + walkPoint3[0] * self._incline
242 |             self.walkPointStartRightstepRightLeg[2] = self.walkPointStartRightstepRightLeg[2] + self.walkPointStartRightstepRightLeg[0] * self._incline
243 |             self.walkPointStartLeftstepRightLeg[2] = self.walkPointStartLeftstepRightLeg[2] + self.walkPointStartLeftstepRightLeg[0] * self._incline
244 |             self.walkPointEndLeftstepRightLeg[2] = self.walkPointEndLeftstepRightLeg[2] + self.walkPointEndLeftstepRightLeg[0] * self._incline
245 |             self.walkPointEndRightstepRightLeg[2] = self.walkPointEndRightstepRightLeg[2] + self.walkPointEndRightstepRightLeg[0] * self._incline
246 | 
247 |         if self._bodyPositionXPlus != 0:  # 허리 앞뒤 위치 조절
248 |             walkPoint0[0] = walkPoint0[0] - self._bodyPositionXPlus
249 |             walkPoint1[0] = walkPoint1[0] - self._bodyPositionXPlus
250 |             walkPoint2[0] = walkPoint2[0] - self._bodyPositionXPlus
251 |             walkPoint3[0] = walkPoint3[0] - self._bodyPositionXPlus
252 |             self.walkPointStartRightstepRightLeg[0] = self.walkPointStartRightstepRightLeg[0] - self._bodyPositionXPlus
253 |             self.walkPointStartLeftstepRightLeg[0] = self.walkPointStartLeftstepRightLeg[0] - self._bodyPositionXPlus
254 |             self.walkPointEndLeftstepRightLeg[0] = self.walkPointEndLeftstepRightLeg[0] - self._bodyPositionXPlus
255 |             self.walkPointEndRightstepRightLeg[0] = self.walkPointEndRightstepRightLeg[0] - self._bodyPositionXPlus
256 | 
257 |         if self._damping != 0:  # 댐핑 조절
258 |             dampHeight = (walkPoint3[2][-1] - walkPoint0[2][0]) / 2
259 |             walkPoint0[2][0] = walkPoint0[2][0] + dampHeight * self._damping
260 |             walkPoint2[2][0] = walkPoint2[2][0] - dampHeight * self._damping
261 | 
262 |         self._walkPoint0 = walkPoint0
263 |         self._walkPoint1 = walkPoint1
264 |         self._walkPoint2 = walkPoint2
265 |         self._walkPoint3 = walkPoint3
266 | 
267 |         self.walkPointLeftStepRightLeg = np.column_stack([walkPoint0[:, self._swayShift:], walkPoint1, walkPoint2[:, :self._swayShift]])
268 |         self.walkPointRightStepRightLeg = np.column_stack([walkPoint2[:, self._swayShift:], walkPoint3, walkPoint0[:, :self._swayShift]])
269 | 
270 |         self.walkPointLeftStepLeftLeg = self.walkPointRightStepRightLeg * np.array([[1], [-1], [1]])
271 |         self.walkPointRightStepLeftLeg = self.walkPointLeftStepRightLeg * np.array([[1], [-1], [1]])
272 | 
273 |         self.walkPointStartRightstepLeftLeg = self.walkPointStartLeftstepRightLeg * np.array([[1], [-1], [1]])
274 |         self.walkPointStartLeftstepLeftLeg = self.walkPointStartRightstepRightLeg * np.array([[1], [-1], [1]])
275 | 
276 |         self.walkPointEndLeftstepLeftLeg = self.walkPointEndRightstepRightLeg * np.array([[1], [-1], [1]])
277 |         self.walkPointEndRightstepLeftLeg = self.walkPointEndLeftstepRightLeg * np.array([[1], [-1], [1]])
278 | 
279 |     def inverseKinematicsPoint(self, pointRight, pointLeft):
280 |         # point list to angle list.
281 |         l3 = self._legUp_length
282 |         l4 = self._legDown_length
283 | 
284 |         fx = pointRight[0]
285 |         fy = pointRight[1]
286 |         fz = self._legUp_length + self._legDown_length - pointRight[2]
287 | 
288 |         a = math.sqrt(fx * fx + fy * fy + fz * fz)
289 | 
290 |         d1 = math.asin(fx / a)
291 |         d2 = math.acos((l3 * l3 + a * a - l4 * l4) / (2 * l3 * a))
292 |         #d3 = math.acos(fz/a)
293 |         d4 = math.acos((l4 * l4 + a * a - l3 * l3) / (2 * l4 * a))
294 |         d5 = math.pi - d2 - d4
295 | 
296 |         t1 = (math.atan2(fy, fz))
297 |         t2 = d1 + d2
298 |         t3 = math.pi - d5
299 |         t4 = -t2 + t3
300 |         t5 = -t1
301 | 
302 |         rightInverse = np.array([0, t1, -t2, t3, -t4, t5]) * self._motorDirectionRight
303 | 
304 |         fx = pointLeft[0]
305 |         fy = pointLeft[1]
306 |         fz = self._legUp_length + self._legDown_length - pointLeft[2]
307 | 
308 |         a = math.sqrt(fx * fx + fy * fy + fz * fz)
309 | 
310 |         d1 = math.asin(fx / a)
311 |         d2 = math.acos((l3 * l3 + a * a - l4 * l4) / (2 * l3 * a))
312 |         #d3 = math.acos(fz/a)
313 |         d4 = math.acos((l4 * l4 + a * a - l3 * l3) / (2 * l4 * a))
314 |         d5 = math.pi - d2 - d4
315 | 
316 |         t1 = (math.atan2(fy, fz))
317 |         t2 = d1 + d2
318 |         t3 = math.pi - d5
319 |         t4 = -t2 + t3
320 |         t5 = -t1
321 | 
322 |         leftInverse = np.array([0, t1, -t2, t3, -t4, t5]) * self._motorDirectionLeft
323 |         #print(np.hstack([rightInverse, leftInverse]))
324 |         return np.hstack([rightInverse, leftInverse])
325 | 
326 |     def inverseKinematicsList(self, point, isRightLeg):
327 |         inverseAngle = np.zeros((point[0].size, 6))
328 |         for i in range(point[0].size):
329 |             l3 = self._legUp_length
330 |             l4 = self._legDown_length
331 | 
332 |             fx = point[0][i]
333 |             fy = point[1][i]
334 |             fz = self._legUp_length + self._legDown_length - point[2][i]
335 | 
336 |             a = math.sqrt(fx * fx + fy * fy + fz * fz)
337 | 
338 |             d1 = math.asin(fx / a)
339 |             d2 = math.acos((l3 * l3 + a * a - l4 * l4) / (2 * l3 * a))
340 |             #d3 = math.acos(fz/a)
341 |             d4 = math.acos((l4 * l4 + a * a - l3 * l3) / (2 * l4 * a))
342 |             d5 = math.pi - d2 - d4
343 | 
344 |             t1 = (math.atan2(fy, fz))
345 |             t2 = d1 + d2
346 |             t3 = math.pi - d5
347 |             t4 = -t2 + t3
348 |             t5 = -t1
349 | 
350 |             if isRightLeg:
351 |                 inverseAngle[i] = np.array([0, t1, -t2, t3, -t4, t5]) * self._motorDirectionRight
352 |             else:
353 |                 inverseAngle[i] = np.array([0, t1, -t2, t3, -t4, t5]) * self._motorDirectionLeft
354 | 
355 |         return inverseAngle
356 | 
357 |     def showGaitPoint2D(self):
358 |         plt.plot(self._walkPoint0[0], self._walkPoint0[2], 'o-', c='red',  ms=7, lw=5)
359 |         plt.plot(self._walkPoint1[0], self._walkPoint1[2], 'o-', c='blue', ms=7, lw=5)
360 |         plt.plot(self._walkPoint2[0], self._walkPoint2[2], 'o-', c='red',  ms=7, lw=5)
361 |         plt.plot(self._walkPoint3[0], self._walkPoint3[2], 'o-', c='blue', ms=7, lw=5)
362 | 
363 |         plt.plot(self.walkPointStartRightstepRightLeg[0], self.walkPointStartRightstepRightLeg[2], '*-')
364 |         plt.plot(self.walkPointStartLeftstepRightLeg[0], self.walkPointStartLeftstepRightLeg[2],   '*-')
365 |         plt.plot(self.walkPointEndRightstepRightLeg[0], self.walkPointEndRightstepRightLeg[2],     '*-')
366 |         plt.plot(self.walkPointEndLeftstepRightLeg[0], self.walkPointEndLeftstepRightLeg[2],       '*-')
367 | 
368 |         plt.show()
369 | 
370 |     def showGaitPoint2DTop(self):
371 |         plt.plot(self._walkPoint0[0], self._walkPoint0[1], 'o-')
372 |         plt.plot(self._walkPoint1[0], self._walkPoint1[1], 'o-')
373 |         plt.plot(self._walkPoint2[0], self._walkPoint2[1], 'o-')
374 |         plt.plot(self._walkPoint3[0], self._walkPoint3[1], 'o-')
375 | 
376 |         plt.plot(self.walkPointStartRightstepRightLeg[0], self.walkPointStartRightstepRightLeg[1], '.-')
377 |         plt.plot(self.walkPointStartLeftstepRightLeg[0], self.walkPointStartLeftstepRightLeg[1], '.-')
378 | 
379 |         plt.plot(self.walkPointEndRightstepRightLeg[0], self.walkPointEndRightstepRightLeg[1], '+-')
380 |         plt.plot(self.walkPointEndLeftstepRightLeg[0], self.walkPointEndLeftstepRightLeg[1], '+-')
381 | 
382 |         plt.show()
383 | 
384 |     def showGaitPoint3D(self):
385 |         fig = plt.figure(1)
386 |         ax = fig.add_subplot(111, projection='3d')
387 | 
388 |         ax.plot(self.walkPointRightStepRightLeg[0], self.walkPointRightStepRightLeg[1],
389 |                 self.walkPointRightStepRightLeg[2], 'co-', lw=10, ms=6)
390 |         ax.plot(self.walkPointLeftStepRightLeg[0], self.walkPointLeftStepRightLeg[1],
391 |                 self.walkPointLeftStepRightLeg[2], 'mo-', lw=10, ms=6)
392 | 
393 |         ax.plot(self._walkPoint0[0], self._walkPoint0[1], self._walkPoint0[2], 'o')
394 |         ax.plot(self._walkPoint1[0], self._walkPoint1[1], self._walkPoint1[2], 'o')
395 |         ax.plot(self._walkPoint2[0], self._walkPoint2[1], self._walkPoint2[2], 'o')
396 |         ax.plot(self._walkPoint3[0], self._walkPoint3[1], self._walkPoint3[2], 'o')
397 | 
398 |         ax.plot(self.walkPointStartRightstepRightLeg[0], self.walkPointStartRightstepRightLeg[1],
399 |                 self.walkPointStartRightstepRightLeg[2], '*-')
400 |         ax.plot(self.walkPointStartLeftstepRightLeg[0], self.walkPointStartLeftstepRightLeg[1],
401 |                 self.walkPointStartLeftstepRightLeg[2], '*-')
402 | 
403 |         ax.plot(self.walkPointEndRightstepRightLeg[0], self.walkPointEndRightstepRightLeg[1],
404 |                 self.walkPointEndRightstepRightLeg[2], 'c-')
405 |         ax.plot(self.walkPointEndLeftstepRightLeg[0], self.walkPointEndLeftstepRightLeg[1],
406 |                 self.walkPointEndLeftstepRightLeg[2], '+-')
407 | 
408 |         plt.show()
409 | 
410 |     def inverseKinematicsAll(self):
411 |         self.walkAnglesStartRight = np.column_stack(
412 |             [self.inverseKinematicsList(self.walkPointStartRightstepRightLeg, True),
413 |              self.inverseKinematicsList(self.walkPointStartRightstepLeftLeg, False)])
414 |         self.walkAnglesStartLeft = np.column_stack(
415 |             [self.inverseKinematicsList(self.walkPointStartLeftstepRightLeg, True),
416 |              self.inverseKinematicsList(self.walkPointStartLeftstepLeftLeg, False)])
417 | 
418 |         self.walkAnglesEndLeft = np.column_stack(
419 |             [self.inverseKinematicsList(self.walkPointEndLeftstepRightLeg, True),
420 |              self.inverseKinematicsList(self.walkPointEndLeftstepLeftLeg, False)])
421 |         self.walkAnglesEndRight = np.column_stack(
422 |             [self.inverseKinematicsList(self.walkPointEndRightstepRightLeg, True),
423 |              self.inverseKinematicsList(self.walkPointEndRightstepLeftLeg, False)])
424 |              
425 |         self.walkAnglesWalkingRight = np.column_stack(
426 |             [self.inverseKinematicsList(self.walkPointRightStepRightLeg, True),
427 |              self.inverseKinematicsList(self.walkPointRightStepLeftLeg, False)])
428 |         self.walkAnglesWalkingLeft = np.column_stack(
429 |             [self.inverseKinematicsList(self.walkPointLeftStepRightLeg, True),
430 |              self.inverseKinematicsList(self.walkPointLeftStepLeftLeg, False)])
431 | 
432 | 
433 | def main():
434 |     walk = WalkGenerator()
435 |     walk.setWalkParameter(bodyMovePoint=8, legMovePoint=8, height=50, stride=80, sit=30, swayBody=60, swayFoot=0,
436 |                           bodyPositionForwardPlus=0, swayShift=5, liftPush=0.4, landPull=0.7, timeStep=0.06, damping=0.0, incline=0.0)
437 |     walk.generate()
438 |     walk.showGaitPoint2D()
439 |     walk.showGaitPoint2DTop()
440 |     walk.showGaitPoint3D()
441 | 
442 | 
443 | if __name__ == "__main__":
444 |     main()
445 | 


--------------------------------------------------------------------------------
/walking_simulation_example.py:
--------------------------------------------------------------------------------
  1 | """
  2 | bipedal robot walking simulation
  3 | 
  4 | by Einsbon (Sunbin Kim)
  5 | - GitHub: https://github.com/Einsbon
  6 | - Youtube:  https://www.youtube.com/channel/UCt7FZ-8uzV_jHJiKp3NlHvg
  7 | - Blog: https://blog.naver.com/einsbon
  8 | """
  9 | 
 10 | import pybullet as p
 11 | import time
 12 | from time import sleep
 13 | import pybullet_data
 14 | import numpy as np
 15 | import math
 16 | import os
 17 | 
 18 | import motorController
 19 | import walkGenerator
 20 | 
 21 | # motor parameters
 22 | motor_kp = 0.5
 23 | motor_kd = 0.5
 24 | motor_torque = 1.5
 25 | motor_max_velocity = 5.0
 26 | 
 27 | # physics parameters
 28 | fixedTimeStep = 1. / 2000
 29 | numSolverIterations = 200
 30 | 
 31 | physicsClient = p.connect(p.GUI)
 32 | p.setTimeStep(fixedTimeStep)
 33 | p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
 34 | p.setAdditionalSearchPath(pybullet_data.getDataPath())  # to load plane.urdf
 35 | 
 36 | p.setGravity(0, 0, 0)
 37 | 
 38 | p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=10, cameraPitch=-0, cameraTargetPosition=[0.4, 0, 0.1])
 39 | 
 40 | planeID = p.loadURDF("plane.urdf")
 41 | robotID = p.loadURDF(os.path.abspath(os.path.dirname(__file__)) + '/humanoid_leg_12dof.8.urdf', [0, 0, 0.31],
 42 |                      p.getQuaternionFromEuler([0, 0, 0]),
 43 |                      useFixedBase=False)
 44 | 
 45 | motorsController = motorController.MotorController(robotID, physicsClient, fixedTimeStep, motor_kp, motor_kd, motor_torque, motor_max_velocity)
 46 | print(motorsController.getRevoluteJoint_nameToId())
 47 | 
 48 | walk = walkGenerator.WalkGenerator()
 49 | walk.setWalkParameter(bodyMovePoint=8,
 50 |                       legMovePoint=8,
 51 |                       height=50,
 52 |                       stride=90,
 53 |                       sit=50,
 54 |                       swayBody=30,
 55 |                       swayFoot=0,
 56 |                       bodyPositionForwardPlus=5,
 57 |                       swayShift=3,
 58 |                       liftPush=0.5,
 59 |                       landPull=0.7,
 60 |                       timeStep=0.06,
 61 |                       damping=0.0,
 62 |                       incline=0.0)
 63 | walk.generate()
 64 | walk.inverseKinematicsAll()
 65 | walk.showGaitPoint3D()
 66 | 
 67 | actionTime = walk._timeStep
 68 | p.setGravity(0, 0, -9.8)
 69 | p.setRealTimeSimulation(0)
 70 | motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[0], 2, 0)
 71 | 
 72 | # rest 1 second in engine
 73 | waitTime = 1
 74 | repeatTime = int(waitTime / fixedTimeStep)
 75 | for _ in range(repeatTime):
 76 |     p.stepSimulation()
 77 |     # time.sleep(fixedTimeStep)
 78 | 
 79 | p.setGravity(0, 0, -9.8)
 80 | rightStep = True
 81 | walkPointNum = walk._bodyMovePoint + walk._legMovePoint
 82 | 
 83 | for i in range(walkPointNum):
 84 |     motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesStartLeft[i], actionTime, 0)
 85 | for _ in range(4):
 86 |     for i in range(np.size(walk.walkAnglesWalkingRight, 0)):
 87 |         motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i], actionTime, 0)
 88 |     for i in range(np.size(walk.walkAnglesWalkingLeft, 0)):
 89 |         motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i], actionTime, 0)
 90 | for i in range(walkPointNum):
 91 |     motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesEndRight[i], actionTime, 0)
 92 | '''
 93 | Implemented in a more complex way.
 94 | 
 95 | if rightStep == True:
 96 |     for i in range(walkPointNum):
 97 |         motorsController.setMotorsAngleInFixedTimestep(
 98 |             walk.inverseKinematicsPoint(walk.walkPointStartRightstepRightLeg[:, i], walk.walkPointStartRightstepLeftLeg[:, i]), actionTime, 0)
 99 |     rightStep = False
100 | else:
101 |     for i in range(walkPointNum):
102 |         motorsController.setMotorsAngleInFixedTimestep(
103 |             walk.inverseKinematicsPoint(walk.walkPointStartLeftstepRightLeg[:, i], walk.walkPointStartLeftstepLeftLeg[:, i]), actionTime, 0)
104 |     rightStep = True
105 | for i in range(4):
106 |     if(rightStep):
107 |         # right foot step
108 |         for i in range(walkPointNum):
109 |             motorsController.setMotorsAngleInFixedTimestep(
110 |                 walk.inverseKinematicsPoint(walk.walkPointRightStepRightLeg[:, i], walk.walkPointRightStepLeftLeg[:, i]), actionTime, 0)
111 |         rightStep = False
112 |     else:
113 |         # left foot step
114 |         for i in range(walkPointNum):
115 |             motorsController.setMotorsAngleInFixedTimestep(
116 |                 walk.inverseKinematicsPoint(walk.walkPointLeftStepRightLeg[:, i], walk.walkPointLeftStepLeftLeg[:, i]), actionTime, 0)
117 |         rightStep = True
118 | if rightStep == True:
119 |     # end walking. left
120 |     for i in range(walkPointNum):
121 |         motorsController.setMotorsAngleInFixedTimestep(
122 |             walk.inverseKinematicsPoint(walk.walkPointEndRightstepRightLeg[:, i], walk.walkPointEndRightstepLeftLeg[:, i]), actionTime, 0)
123 |     rightStep = False
124 | else:
125 |     # end walking. left
126 |     for i in range(walkPointNum):
127 |         motorsController.setMotorsAngleInFixedTimestep(
128 |             walk.inverseKinematicsPoint(walk.walkPointEndLeftstepRightLeg[:, i], walk.walkPointEndLeftstepLeftLeg[:, i]), actionTime, 0)
129 |     rightStep = True
130 | '''
131 | 
132 | # rest 2 seconds in engine
133 | waitTime = 1
134 | repeatTime = int(waitTime / fixedTimeStep)
135 | for _ in range(repeatTime):
136 |     p.stepSimulation()
137 | 
138 | # robot control using (x,y,z) point.
139 | motorsController.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([0, 0, 0], [0, 0, 0]), 0.5, 0.5)
140 | motorsController.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([0, 0, 40], [0, 0, 40]), 0.5, 0.5)
141 | motorsController.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([0, 0, 0], [0, 0, 0]), 0.5, 0.5)
142 | motorsController.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([0, 0, 40], [0, 0, 40]), 0.5, 0.5)
143 | motorsController.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([0, 30, 40], [0, 30, 40]), 0.5, 0.5)
144 | motorsController.setMotorsAngleInFixedTimestep(walk.inverseKinematicsPoint([0, 0, 40], [0, 0, 40]), 0.5, 0.5)
145 | 
146 | # More applied version. Press Enter to start or stop walking.
147 | walking = False
148 | rightStep = True
149 | while (1):
150 |     keys = p.getKeyboardEvents()
151 |     for k, v in keys.items():
152 |         if (k == 65309) and (v == 3 or v == 6):  # if enter key is pressed
153 |             walking = True
154 |             keys = {}
155 | 
156 |     if walking == True:
157 |         if rightStep == True:
158 |             for i in range(walkPointNum):
159 |                 motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesStartRight[i], actionTime, 0)
160 |             rightStep = False
161 |         else:
162 |             for i in range(walkPointNum):
163 |                 motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesStartLeft[i], actionTime, 0)
164 |             rightStep = True
165 | 
166 |         keys = p.getKeyboardEvents()
167 |         for k, v in keys.items():
168 |             if (k == 65309) and (v == 3 or v == 6):  # if enter key is pressed
169 |                 walking = False
170 |                 keys = {}
171 | 
172 |         while (walking):
173 |             if (rightStep):
174 |                 for i in range(walkPointNum):
175 |                     motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingRight[i], actionTime, 0)
176 |                 rightStep = False
177 |             else:
178 |                 for i in range(walkPointNum):
179 |                     motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesWalkingLeft[i], actionTime, 0)
180 |                 rightStep = True
181 | 
182 |             keys = p.getKeyboardEvents()
183 |             for k, v in keys.items():
184 |                 if (k == 65309) and (v == 3 or v == 6):  # if enter key is pressed
185 |                     walking = False
186 |                     keys = {}
187 | 
188 |         if rightStep == True:
189 |             for i in range(walkPointNum):
190 |                 motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesEndRight[i], actionTime, 0)
191 |             rightStep = False
192 |         else:
193 |             for i in range(walkPointNum):
194 |                 motorsController.setMotorsAngleInFixedTimestep(walk.walkAnglesEndLeft[i], actionTime, 0)
195 |             rightStep = True
196 |     else:
197 |         p.stepSimulation()
198 | 


--------------------------------------------------------------------------------