|
3 | 3 |
|
4 | 4 | <option integrator="implicitfast"/> |
5 | 5 |
|
| 6 | + <default> |
| 7 | + <default class="omy"> |
| 8 | + <joint frictionloss="0.1" armature="0.1"/> |
| 9 | + <position kp="800" dampratio="1"/> |
| 10 | + <!-- DY-80 motor: Joint 1, 2 --> |
| 11 | + <default class="DY-80"> |
| 12 | + <position forcerange="-61.4 61.4"/> |
| 13 | + <default class="joint1"> |
| 14 | + <joint axis="0 0 1" range="-6.283185 6.283185"/> |
| 15 | + </default> |
| 16 | + <default class="joint2"> |
| 17 | + <joint axis="0 1 0" range="-6.283185 6.283185"/> |
| 18 | + </default> |
| 19 | + </default> |
| 20 | + <!-- DY-70 motor: Joint 3, 4, 5, 6 --> |
| 21 | + <default class="DY-70"> |
| 22 | + <position forcerange="-31.7 31.7"/> |
| 23 | + <default class="joint3"> |
| 24 | + <joint axis="0 1 0" range="-2.617994 2.617994"/> |
| 25 | + </default> |
| 26 | + <default class="joint4"> |
| 27 | + <joint axis="0 1 0" range="-6.283185 6.283185"/> |
| 28 | + </default> |
| 29 | + <default class="joint5"> |
| 30 | + <joint axis="0 0 1" range="-6.283185 6.283185"/> |
| 31 | + </default> |
| 32 | + <default class="joint6"> |
| 33 | + <joint axis="0 1 0" range="-6.283185 6.283185"/> |
| 34 | + </default> |
| 35 | + </default> |
| 36 | + <!-- Visual / Collision --> |
| 37 | + <default class="visual"> |
| 38 | + <geom type="mesh" contype="0" conaffinity="0" density="0" group="2"/> |
| 39 | + </default> |
| 40 | + <default class="collision"> |
| 41 | + <geom type="mesh" group="3"/> |
| 42 | + </default> |
| 43 | + </default> |
| 44 | + </default> |
| 45 | + |
6 | 46 | <asset> |
7 | 47 | <material name="black" rgba="0.1 0.1 0.1 1"/> |
8 | | - |
9 | 48 | <mesh name="base_unit" file="base_unit.stl" scale="0.001 0.001 0.001"/> |
10 | 49 | <mesh name="link1" file="link1.stl" scale="0.001 0.001 0.001"/> |
11 | 50 | <mesh name="link2" file="link2.stl" scale="0.001 0.001 0.001"/> |
|
15 | 54 | <mesh name="link6" file="link6.stl" scale="0.001 0.001 0.001"/> |
16 | 55 | </asset> |
17 | 56 |
|
18 | | - <default> |
19 | | - <!-- DY-80 motor: Joint 1, 2 --> |
20 | | - <default class="DY_80"> |
21 | | - <joint frictionloss="0.1" armature="0.1"/> |
22 | | - <position kp="1500.0" dampratio="5.0" forcerange="-61.4 61.4"/> |
23 | | - <default class="Joint1"> |
24 | | - <joint axis="0 0 1" range="-6.283185 6.283185"/> |
25 | | - </default> |
26 | | - <default class="Joint2"> |
27 | | - <joint axis="0 1 0" range="-6.283185 6.283185"/> |
28 | | - </default> |
29 | | - </default> |
30 | | - <!-- DY-70 motor: Joint 3, 4, 5, 6 --> |
31 | | - <default class="DY_70"> |
32 | | - <joint frictionloss="0.1" armature="0.1"/> |
33 | | - <position kp="1500.0" dampratio="5.0" forcerange="-31.7 31.7"/> |
34 | | - <default class="Joint3"> |
35 | | - <joint axis="0 1 0" range="-2.617994 2.617994"/> |
36 | | - </default> |
37 | | - <default class="Joint4"> |
38 | | - <joint axis="0 1 0" range="-6.283185 6.283185"/> |
39 | | - </default> |
40 | | - <default class="Joint5"> |
41 | | - <joint axis="0 0 1" range="-6.283185 6.283185"/> |
42 | | - </default> |
43 | | - <default class="Joint6"> |
44 | | - <joint axis="0 1 0" range="-6.283185 6.283185"/> |
45 | | - </default> |
46 | | - </default> |
47 | | - <!-- Visual / Collision --> |
48 | | - <default class="visual"> |
49 | | - <geom type="mesh" contype="0" conaffinity="0" density="0" group="2" material="black"/> |
50 | | - </default> |
51 | | - <default class="collision"> |
52 | | - <geom group="3" type="mesh" material="black"/> |
53 | | - </default> |
54 | | - </default> |
55 | | - |
56 | 57 | <worldbody> |
57 | | - <body name="base_unit"> |
| 58 | + <body name="base_unit" childclass="omy"> |
58 | 59 | <inertial mass="1.5287486" pos="-0.0040215824 0.0013083991 0.055073470" |
59 | 60 | fullinertia="0.0046428203 0.0049736955 0.0064078321 0.000044405532 -0.00015049296 0.000020756047"/> |
60 | | - <geom mesh="base_unit" class="visual"/> |
| 61 | + <geom mesh="base_unit" material="black" class="visual"/> |
61 | 62 | <geom mesh="base_unit" class="collision"/> |
62 | 63 | <body name="link1" pos="0 0 0.1715"> |
63 | 64 | <inertial mass="2.0648832" pos="-0.00011063615 -0.0054711270 -0.015897733" |
64 | 65 | fullinertia="0.0030332190 0.0027276724 0.0022826576 0.0000051792838 0.0000033357961 0.00017934953"/> |
65 | | - <joint name="Joint1" class="Joint1"/> |
66 | | - <geom mesh="link1" class="visual"/> |
| 66 | + <joint name="joint1" class="joint1"/> |
| 67 | + <geom mesh="link1" material="black" class="visual"/> |
67 | 68 | <geom mesh="link1" class="collision"/> |
68 | 69 | <body name="link2" pos="0 -0.1215 0"> |
69 | 70 | <inertial mass="3.6795395" pos="0.000011405379 0.016184244 0.10360634" |
70 | 71 | fullinertia="0.051731918 0.050353401 0.0045014360 0.0000023958819 0.000057893960 -0.00043825716"/> |
71 | | - <joint name="Joint2" class="Joint2"/> |
72 | | - <geom mesh="link2" class="visual"/> |
| 72 | + <joint name="joint2" class="joint2"/> |
| 73 | + <geom mesh="link2" material="black" class="visual"/> |
73 | 74 | <geom mesh="link2" class="collision"/> |
74 | 75 | <body name="link3" pos="0 0 0.247"> |
75 | 76 | <inertial mass="2.3865916" pos="0.000078148689 0.10718481 0.14117267" |
76 | 77 | fullinertia="0.022694399 0.021920087 0.0027761559 -0.00000094142428 -0.000018551757 0.00019179387"/> |
77 | | - <joint name="Joint3" class="Joint3"/> |
78 | | - <geom mesh="link3" class="visual"/> |
| 78 | + <joint name="joint3" class="joint3"/> |
| 79 | + <geom mesh="link3" material="black" class="visual"/> |
79 | 80 | <geom mesh="link3" class="collision"/> |
80 | 81 | <body name="link4" pos="0 0.1215 0.2195"> |
81 | 82 | <inertial mass="1.4002347" pos="-0.00013318256 -0.10746667 0.019090688" |
82 | 83 | fullinertia="0.0017545679 0.0015652147 0.0012159839 -0.0000045863379 -0.0000017759142 0.00015339913"/> |
83 | | - <joint name="Joint4" class="Joint4"/> |
84 | | - <geom mesh="link4" class="visual"/> |
| 84 | + <joint name="joint4" class="joint4"/> |
| 85 | + <geom mesh="link4" material="black" class="visual"/> |
85 | 86 | <geom mesh="link4" class="collision"/> |
86 | 87 | <body name="link5" pos="0 -0.113 0"> |
87 | 88 | <inertial mass="1.4002347" pos="0.00013318256 -0.019090688 0.10996667" |
88 | 89 | fullinertia="0.0017545679 0.0012159839 0.0015652147 -0.0000017759143 -0.0000045863376 0.00015339913"/> |
89 | | - <joint name="Joint5" class="Joint5"/> |
90 | | - <geom mesh="link5" class="visual"/> |
| 90 | + <joint name="joint5" class="joint5"/> |
| 91 | + <geom mesh="link5" material="black" class="visual"/> |
91 | 92 | <geom mesh="link5" class="collision"/> |
92 | 93 | <body name="link6" pos="0 0 0.1155"> |
93 | 94 | <inertial mass="0.32442692" pos="0.000011080146 -0.080594056 0.0093029337" |
94 | 95 | fullinertia="0.00019398849 0.00024913070 0.00016536751 0.00000021420132 -0.0000012342019 -0.0000038937348"/> |
95 | | - <joint name="Joint6" class="Joint6"/> |
96 | | - <geom mesh="link6" class="visual"/> |
| 96 | + <joint name="joint6" class="joint6"/> |
| 97 | + <geom mesh="link6" material="black" class="visual"/> |
97 | 98 | <geom mesh="link6" class="collision"/> |
98 | 99 | </body> |
99 | 100 | </body> |
|
105 | 106 | </worldbody> |
106 | 107 |
|
107 | 108 | <actuator> |
108 | | - <position class="Joint1" name="Joint1" joint="Joint1" inheritrange="1"/> |
109 | | - <position class="Joint2" name="Joint2" joint="Joint2" inheritrange="1"/> |
110 | | - <position class="Joint3" name="Joint3" joint="Joint3" inheritrange="1"/> |
111 | | - <position class="Joint4" name="Joint4" joint="Joint4" inheritrange="1"/> |
112 | | - <position class="Joint5" name="Joint5" joint="Joint5" inheritrange="1"/> |
113 | | - <position class="Joint6" name="Joint6" joint="Joint6" inheritrange="1"/> |
| 109 | + <position class="joint1" name="joint1" joint="joint1" inheritrange="1"/> |
| 110 | + <position class="joint2" name="joint2" joint="joint2" inheritrange="1"/> |
| 111 | + <position class="joint3" name="joint3" joint="joint3" inheritrange="1"/> |
| 112 | + <position class="joint4" name="joint4" joint="joint4" inheritrange="1"/> |
| 113 | + <position class="joint5" name="joint5" joint="joint5" inheritrange="1"/> |
| 114 | + <position class="joint6" name="joint6" joint="joint6" inheritrange="1"/> |
114 | 115 | </actuator> |
115 | 116 |
|
116 | | - <contact> |
117 | | - <exclude body1="base_unit" body2="link1"/> |
118 | | - </contact> |
119 | | - |
120 | 117 | <keyframe> |
121 | 118 | <key name="home" qpos="0 0 0 0 0 0" ctrl="0 0 0 0 0 0"/> |
122 | 119 | </keyframe> |
|
0 commit comments