Skip to content

Commit 58cd587

Browse files
juelgjuelg
committed
fix(ik): pinocchio ik with mjcf
- mjcf of pin ik must be the plain robot xml - it has to have the default attribute which needs to be at the beginning
1 parent a07eef8 commit 58cd587

8 files changed

Lines changed: 253 additions & 362 deletions

File tree

‎assets/fr3/mjcf/fr3_0.xml‎

Lines changed: 192 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,192 @@
1+
<mujoco>
2+
<default>
3+
<default class="fr3">
4+
<joint armature="0.1" damping="1"/>
5+
<position inheritrange="1"/>
6+
<default class="finger">
7+
<joint axis="0 1 0" type="slide" range="0 0.04"/>
8+
</default>
9+
<default class="visual">
10+
<geom type="mesh" group="2" contype="0" conaffinity="0"/>
11+
</default>
12+
<default class="collision">
13+
<geom type="mesh" group="3" mass="0" density="0"/>
14+
<default class="fingertip_pad_collision_1">
15+
<geom type="box" size="0.0085 0.004 0.0085" pos="0 0.0055 0.0445"/>
16+
</default>
17+
<default class="fingertip_pad_collision_2">
18+
<geom type="box" size="0.003 0.002 0.003" pos="0.0055 0.002 0.05"/>
19+
</default>
20+
<default class="fingertip_pad_collision_3">
21+
<geom type="box" size="0.003 0.002 0.003" pos="-0.0055 0.002 0.05"/>
22+
</default>
23+
<default class="fingertip_pad_collision_4">
24+
<geom type="box" size="0.003 0.002 0.0035" pos="0.0055 0.002 0.0395"/>
25+
</default>
26+
<default class="fingertip_pad_collision_5">
27+
<geom type="box" size="0.003 0.002 0.0035" pos="-0.0055 0.002 0.0395"/>
28+
</default>
29+
</default>
30+
<site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
31+
</default>
32+
<default class="d435i">
33+
<material specular="0" shininess="0.25"/>
34+
<default class="visual_d435i">
35+
<geom group="2" type="mesh" contype="0" conaffinity="0" mass="0"/>
36+
</default>
37+
<default class="collision_d435i">
38+
<geom group="3" type="mesh" mass="0"/>
39+
</default>
40+
</default>
41+
</default>
42+
<worldbody>
43+
<body name="base_0" childclass="fr3" gravcomp="1">
44+
<body name="fr3_link0_0" gravcomp="1">
45+
<geom mesh="fr3_link0_0" material="black" class="visual"/>
46+
<geom mesh="fr3_link0_1" material="white" class="visual"/>
47+
<geom mesh="fr3_link0_2" material="white" class="visual"/>
48+
<geom mesh="fr3_link0_3" material="white" class="visual"/>
49+
<geom mesh="fr3_link0_4" material="white" class="visual"/>
50+
<geom mesh="fr3_link0_5" material="red" class="visual"/>
51+
<geom mesh="fr3_link0_6" material="black" class="visual"/>
52+
<geom name="fr3_link0_collision_0" mesh="fr3_link0_coll" class="collision"/>
53+
<body name="fr3_link1_0" pos="0 0 0.333" gravcomp="1">
54+
<inertial pos="4.128e-07 -0.0181251 -0.0386036" quat="0.999901 0.0021545 0.00429041 0.0132325" mass="2.92747"
55+
diaginertia="0.0186043 0.0181194 0.00538716"/>
56+
<joint name="fr3_joint1_0" axis="0 0 1" range="-2.7437 2.7437" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
57+
<geom name="fr3_link1_collision_0" class="collision" mesh="fr3_link1_coll"/>
58+
<geom material="white" mesh="fr3_link1" class="visual"/>
59+
<body name="fr3_link2_0" quat="1 -1 0 0" gravcomp="1">
60+
<inertial pos="0.00318289 -0.0743222 0.00881461" quat="0.356855 0.680818 -0.434812 0.469126" mass="2.93554"
61+
diaginertia="0.0299291 0.0299291 0.0299291"/>
62+
<joint name="fr3_joint2_0" axis="0 0 1" range="-1.7837 1.7837" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
63+
<geom material="white" mesh="fr3_link2" class="visual"/>
64+
<geom name="fr3_link2_collision_0" class="collision" mesh="fr3_link2_coll"/>
65+
<body name="fr3_link3_0" pos="0 -0.316 0" quat="1 1 0 0" gravcomp="1">
66+
<inertial pos="0.0407016 -0.00482006 -0.0289731" quat="0.950032 -0.148357 0.229935 0.150201" mass="2.2449"
67+
diaginertia="0.0140109 0.0140109 0.0140109"/>
68+
<joint name="fr3_joint3_0" axis="0 0 1" range="-2.9007 2.9007" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
69+
<geom mesh="fr3_link3_0" material="white" class="visual"/>
70+
<geom mesh="fr3_link3_1" material="black" class="visual"/>
71+
<geom name="fr3_link3_collision_0" class="collision" mesh="fr3_link3_coll"/>
72+
<body name="fr3_link4_0" pos="0.0825 0 0" quat="1 1 0 0" gravcomp="1">
73+
<inertial pos="-0.0459101 0.0630493 -0.00851879" quat="0.23761 0.892458 -0.00078702 0.383484"
74+
mass="2.6156" diaginertia="0.0206104 0.0206104 0.0206104"/>
75+
<joint name="fr3_joint4_0" axis="0 0 1" range="-3.0421 -0.1518" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
76+
<geom mesh="fr3_link4_0" material="white" class="visual"/>
77+
<geom mesh="fr3_link4_1" material="black" class="visual"/>
78+
<geom name="fr3_link4_collision_0" class="collision" mesh="fr3_link4_coll"/>
79+
<body name="fr3_link5_0" pos="-0.0825 0.384 0" quat="1 -1 0 0" gravcomp="1">
80+
<inertial pos="-0.00160396 0.0292536 -0.0972966" quat="0.922285 0.098826 0.0982562 -0.360514"
81+
mass="2.32712" diaginertia="0.0182879 0.0182879 0.0182879"/>
82+
<joint name="fr3_joint5_0" axis="0 0 1" range="-2.8065 2.8065" actuatorfrcrange="-12 12" actuatorgravcomp="true"/>
83+
<geom mesh="fr3_link5_0" material="white" class="visual"/>
84+
<geom mesh="fr3_link5_1" material="white" class="visual"/>
85+
<geom mesh="fr3_link5_2" material="black" class="visual"/>
86+
<geom name="fr3_link5_collision_0" class="collision" mesh="fr3_link5_coll"/>
87+
<body name="fr3_link6_0" quat="1 1 0 0" gravcomp="1">
88+
<inertial pos="0.0597131 -0.0410295 -0.0101693" quat="0.593933 0.525442 0.520644 0.316361"
89+
mass="1.81704" diaginertia="0.00483538 0.00483538 0.00483538"/>
90+
<joint name="fr3_joint6_0" axis="0 0 1" range="0.5445 4.5169" actuatorfrcrange="-12 12" actuatorgravcomp="true"/>
91+
<geom mesh="fr3_link6_0" material="button_green" class="visual"/>
92+
<geom mesh="fr3_link6_1" material="white" class="visual"/>
93+
<geom mesh="fr3_link6_2" material="white" class="visual"/>
94+
<geom mesh="fr3_link6_3" material="gray" class="visual"/>
95+
<geom mesh="fr3_link6_4" material="button_red" class="visual"/>
96+
<geom mesh="fr3_link6_5" material="white" class="visual"/>
97+
<geom mesh="fr3_link6_6" material="black" class="visual"/>
98+
<geom mesh="fr3_link6_7" material="button_blue" class="visual"/>
99+
<geom name="fr3_link6_collision_0" class="collision" mesh="fr3_link6_coll"/>
100+
<body name="fr3_link7_0" pos="0.088 0 0" quat="1 1 0 0" gravcomp="1">
101+
<inertial pos="0.00452258 0.00862619 -0.0161633" quat="0.120255 0.394761 -0.799132 0.437139"
102+
mass="0.627143" diaginertia="3.076e-07 3.076e-07 3.076e-07"/>
103+
<joint name="fr3_joint7_0" axis="0 0 1" range="-3.0159 3.0159" actuatorfrcrange="-12 12" actuatorgravcomp="true"/>
104+
<geom mesh="fr3_link7_0" material="black" class="visual"/>
105+
<geom mesh="fr3_link7_1" material="white" class="visual"/>
106+
<geom mesh="fr3_link7_2" material="white" class="visual"/>
107+
<geom mesh="fr3_link7_3" material="black" class="visual"/>
108+
<geom name="fr3_link7_collision_0" class="collision" mesh="fr3_link7_coll"/>
109+
<site name="attachment_site_0" pos="0 0 0.107"/>
110+
<body name="hand_0" pos="0 0 0.107" quat="0.9238795 0 0 -0.3826834" gravcomp="1">
111+
<geom mesh="Panda_RealSenseD435_Camera_Mount" class="visual" pos="0.065 0.03 0" euler="1.57 3.142 0"/>
112+
<geom mesh="Panda_RealSenseD435_Camera_Mount" class="collision"/>
113+
<body name="d435i_0" childclass="d435i" pos="0.051 0 0.065" euler="0 0 -1.5707" gravcomp="1">
114+
<geom mesh="d435i_0" material="IR_Lens" class="visual_d435i"/>
115+
<geom mesh="d435i_1" material="IR_Emitter_Lens" class="visual_d435i"/>
116+
<geom mesh="d435i_2" material="IR_Rim" class="visual_d435i"/>
117+
<geom mesh="d435i_3" material="IR_Lens" class="visual_d435i"/>
118+
<geom mesh="d435i_4" material="Cameras_Gray" class="visual_d435i"/>
119+
<geom mesh="d435i_5" material="Black_Acrylic" class="visual_d435i"/>
120+
<geom mesh="d435i_6" material="Black_Acrylic" class="visual_d435i"/>
121+
<geom mesh="d435i_7" material="RGB_Pupil" class="visual_d435i"/>
122+
<geom mesh="d435i_8" mass="0.072" material="Metal_Casing" class="visual_d435i"/>
123+
<geom class="collision_d435i" type="capsule" mesh="d435i_8" name="d435i_collision_0"/>
124+
<camera name="wrist_0" mode="fixed" pos="0.1 0 0" quat="0 1 0 0" resolution="1920 1080"/>
125+
</body>
126+
<inertial mass="0.73" pos="-0.01 0 0.03" diaginertia="0.001 0.0025 0.0017"/>
127+
<geom mesh="franka_hand_0" material="white" class="visual"/>
128+
<geom mesh="franka_hand_1" material="black" class="visual"/>
129+
<geom mesh="franka_hand_2" material="black" class="visual"/>
130+
<geom mesh="franka_hand_3" material="white" class="visual"/>
131+
<geom mesh="franka_hand_4" material="white" class="visual"/>
132+
<geom mesh="franka_hand_coll" class="collision" name="hand_c_0"/>
133+
<site name="tcp_0" pos="0 0 0.1034" rgba="0 0 0 0.25"/>
134+
<body name="left_finger_0" pos="0 0 0.0584" gravcomp="1">
135+
<inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
136+
<joint name="finger_joint1_0" class="finger"/>
137+
<geom mesh="finger_0" material="white" class="visual"/>
138+
<geom mesh="finger_1" material="black" class="visual"/>
139+
<geom mesh="finger_0" class="collision" name="finger_0_left_0"/>
140+
<geom class="fingertip_pad_collision_1" name="fingertip_pad_collision_1_left_0" friction="2 0.05 0.0001"/>
141+
<geom class="fingertip_pad_collision_2" name="fingertip_pad_collision_2_left_0" friction="2 0.05 0.0001"/>
142+
<geom class="fingertip_pad_collision_3" name="fingertip_pad_collision_3_left_0" friction="2 0.05 0.0001"/>
143+
<geom class="fingertip_pad_collision_4" name="fingertip_pad_collision_4_left_0" friction="2 0.05 0.0001"/>
144+
<geom class="fingertip_pad_collision_5" name="fingertip_pad_collision_5_left_0" friction="2 0.05 0.0001"/>
145+
</body>
146+
<body name="right_finger_0" pos="0 0 0.0584" quat="0 0 0 1" gravcomp="1">
147+
<inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
148+
<joint name="finger_joint2_0" class="finger"/>
149+
<geom mesh="finger_0" material="white" class="visual"/>
150+
<geom mesh="finger_1" material="black" class="visual"/>
151+
<geom mesh="finger_0" class="collision" name="finger_0_right_0"/>
152+
<geom class="fingertip_pad_collision_1" name="fingertip_pad_collision_1_right_0" friction="2 0.05 0.0001"/>
153+
<geom class="fingertip_pad_collision_2" name="fingertip_pad_collision_2_right_0" friction="2 0.05 0.0001"/>
154+
<geom class="fingertip_pad_collision_3" name="fingertip_pad_collision_3_right_0" friction="2 0.05 0.0001"/>
155+
<geom class="fingertip_pad_collision_4" name="fingertip_pad_collision_4_right_0" friction="2 0.05 0.0001"/>
156+
<geom class="fingertip_pad_collision_5" name="fingertip_pad_collision_5_right_0" friction="2 0.05 0.0001"/>
157+
</body>
158+
</body>
159+
</body>
160+
</body>
161+
</body>
162+
</body>
163+
</body>
164+
</body>
165+
</body>
166+
</body>
167+
</body>
168+
</worldbody>
169+
170+
<tendon>
171+
<fixed name="split_0">
172+
<joint joint="finger_joint1_0" coef="0.5"/>
173+
<joint joint="finger_joint2_0" coef="0.5"/>
174+
</fixed>
175+
</tendon>
176+
177+
<equality>
178+
<joint joint1="finger_joint1_0" joint2="finger_joint2_0" solimp="0.95 0.99 0.001" solref="0.005 1"/>
179+
</equality>
180+
<actuator>
181+
<position class="fr3" name="fr3_joint1_0" joint="fr3_joint1_0" kp="4500" kv="450"/>
182+
<position class="fr3" name="fr3_joint2_0" joint="fr3_joint2_0" kp="4500" kv="450"/>
183+
<position class="fr3" name="fr3_joint3_0" joint="fr3_joint3_0" kp="3500" kv="350"/>
184+
<position class="fr3" name="fr3_joint4_0" joint="fr3_joint4_0" kp="3500" kv="350"/>
185+
<position class="fr3" name="fr3_joint5_0" joint="fr3_joint5_0" kp="2000" kv="200"/>
186+
<position class="fr3" name="fr3_joint6_0" joint="fr3_joint6_0" kp="2000" kv="200"/>
187+
<position class="fr3" name="fr3_joint7_0" joint="fr3_joint7_0" kp="2000" kv="200"/>
188+
<!-- Remap original ctrlrange (0, 0.04) to (0, 255): 0.04 * 100 / 255 = 0.01568627451 -->
189+
<general class="fr3" name="actuator8_0" tendon="split_0" forcerange="-100 100" ctrlrange="0 255"
190+
gainprm="0.01568627451 0 0" biasprm="0 -100 -10"/>
191+
</actuator>
192+
</mujoco>

‎assets/fr3/mjcf/fr3_common.xml‎

Lines changed: 0 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -1,49 +1,7 @@
11
<mujoco model="fr3">
22
<compiler angle="radian" meshdir="assets"/>
3-
43
<option integrator="implicitfast" impratio="20" noslip_iterations="5" cone="elliptic"/>
54

6-
<default>
7-
<default class="fr3">
8-
<joint armature="0.1" damping="1"/>
9-
<position inheritrange="1"/>
10-
<default class="finger">
11-
<joint axis="0 1 0" type="slide" range="0 0.04"/>
12-
</default>
13-
<default class="visual">
14-
<geom type="mesh" group="2" contype="0" conaffinity="0"/>
15-
</default>
16-
<default class="collision">
17-
<geom type="mesh" group="3" mass="0" density="0"/>
18-
<default class="fingertip_pad_collision_1">
19-
<geom type="box" size="0.0085 0.004 0.0085" pos="0 0.0055 0.0445"/>
20-
</default>
21-
<default class="fingertip_pad_collision_2">
22-
<geom type="box" size="0.003 0.002 0.003" pos="0.0055 0.002 0.05"/>
23-
</default>
24-
<default class="fingertip_pad_collision_3">
25-
<geom type="box" size="0.003 0.002 0.003" pos="-0.0055 0.002 0.05"/>
26-
</default>
27-
<default class="fingertip_pad_collision_4">
28-
<geom type="box" size="0.003 0.002 0.0035" pos="0.0055 0.002 0.0395"/>
29-
</default>
30-
<default class="fingertip_pad_collision_5">
31-
<geom type="box" size="0.003 0.002 0.0035" pos="-0.0055 0.002 0.0395"/>
32-
</default>
33-
</default>
34-
<site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
35-
</default>
36-
<default class="d435i">
37-
<material specular="0" shininess="0.25"/>
38-
<default class="visual_d435i">
39-
<geom group="2" type="mesh" contype="0" conaffinity="0" mass="0"/>
40-
</default>
41-
<default class="collision_d435i">
42-
<geom group="3" type="mesh" mass="0"/>
43-
</default>
44-
</default>
45-
</default>
46-
475
<asset>
486
<material name="black" rgba=".2 .2 .2 1"/>
497
<material name="white" rgba="1 1 1 1"/>

0 commit comments

Comments
 (0)