Skip to content

Commit b323bd6

Browse files
juelgjuelg
authored
Merge pull request #205 from utn-mi/juelg/ik-pin-mjcf
fix(ik): pinocchio ik with mjcf
2 parents a07eef8 + c247142 commit b323bd6

File tree

8 files changed

+270
-365
lines changed

8 files changed

+270
-365
lines changed

‎assets/fr3/mjcf/fr3_0.xml‎

Lines changed: 198 additions & 0 deletions
Large diffs are not rendered by default.

‎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"/>

‎assets/fr3/mjcf/fr3_unnamed.xml‎

Lines changed: 53 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,50 @@
11
<mujoco>
2+
<!-- custom tcp offset -->
3+
<custom>
4+
<numeric name="tcp_offset_translation" data="0.0 0.0 0.1034"/>
5+
<!-- rotation matrix is row major -->
6+
<numeric name="tcp_offset_rotation_matrix" data="0.707 0.707 0 -0.707 0.707 0 0 0 1"/>
7+
</custom>
8+
<default>
9+
<default class="fr3">
10+
<joint armature="0.1" damping="1"/>
11+
<position inheritrange="1"/>
12+
<default class="finger">
13+
<joint axis="0 1 0" type="slide" range="0 0.04"/>
14+
</default>
15+
<default class="visual">
16+
<geom type="mesh" group="2" contype="0" conaffinity="0"/>
17+
</default>
18+
<default class="collision">
19+
<geom type="mesh" group="3" mass="0" density="0"/>
20+
<default class="fingertip_pad_collision_1">
21+
<geom type="box" size="0.0085 0.004 0.0085" pos="0 0.0055 0.0445"/>
22+
</default>
23+
<default class="fingertip_pad_collision_2">
24+
<geom type="box" size="0.003 0.002 0.003" pos="0.0055 0.002 0.05"/>
25+
</default>
26+
<default class="fingertip_pad_collision_3">
27+
<geom type="box" size="0.003 0.002 0.003" pos="-0.0055 0.002 0.05"/>
28+
</default>
29+
<default class="fingertip_pad_collision_4">
30+
<geom type="box" size="0.003 0.002 0.0035" pos="0.0055 0.002 0.0395"/>
31+
</default>
32+
<default class="fingertip_pad_collision_5">
33+
<geom type="box" size="0.003 0.002 0.0035" pos="-0.0055 0.002 0.0395"/>
34+
</default>
35+
</default>
36+
<site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
37+
</default>
38+
<default class="d435i">
39+
<material specular="0" shininess="0.25"/>
40+
<default class="visual_d435i">
41+
<geom group="2" type="mesh" contype="0" conaffinity="0" mass="0"/>
42+
</default>
43+
<default class="collision_d435i">
44+
<geom group="3" type="mesh" mass="0"/>
45+
</default>
46+
</default>
47+
</default>
248
<worldbody>
349
<body name="base" childclass="fr3" gravcomp="1">
450
<body name="fr3_link0" gravcomp="1">
@@ -13,41 +59,41 @@
1359
<body name="fr3_link1" pos="0 0 0.333" gravcomp="1">
1460
<inertial pos="4.128e-07 -0.0181251 -0.0386036" quat="0.999901 0.0021545 0.00429041 0.0132325" mass="2.92747"
1561
diaginertia="0.0186043 0.0181194 0.00538716"/>
16-
<joint name="fr3_joint1" axis="0 0 1" range="-2.7437 2.7437" actuatorfrcrange="-87 87"/>
62+
<joint name="fr3_joint1" axis="0 0 1" range="-2.7437 2.7437" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
1763
<geom name="fr3_link1_collision" class="collision" mesh="fr3_link1_coll"/>
1864
<geom material="white" mesh="fr3_link1" class="visual"/>
1965
<body name="fr3_link2" quat="1 -1 0 0" gravcomp="1">
2066
<inertial pos="0.00318289 -0.0743222 0.00881461" quat="0.356855 0.680818 -0.434812 0.469126" mass="2.93554"
2167
diaginertia="0.0299291 0.0299291 0.0299291"/>
22-
<joint name="fr3_joint2" axis="0 0 1" range="-1.7837 1.7837" actuatorfrcrange="-87 87"/>
68+
<joint name="fr3_joint2" axis="0 0 1" range="-1.7837 1.7837" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
2369
<geom material="white" mesh="fr3_link2" class="visual"/>
2470
<geom name="fr3_link2_collision" class="collision" mesh="fr3_link2_coll"/>
2571
<body name="fr3_link3" pos="0 -0.316 0" quat="1 1 0 0" gravcomp="1">
2672
<inertial pos="0.0407016 -0.00482006 -0.0289731" quat="0.950032 -0.148357 0.229935 0.150201" mass="2.2449"
2773
diaginertia="0.0140109 0.0140109 0.0140109"/>
28-
<joint name="fr3_joint3" axis="0 0 1" range="-2.9007 2.9007" actuatorfrcrange="-87 87"/>
74+
<joint name="fr3_joint3" axis="0 0 1" range="-2.9007 2.9007" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
2975
<geom mesh="fr3_link3_0" material="white" class="visual"/>
3076
<geom mesh="fr3_link3_1" material="black" class="visual"/>
3177
<geom name="fr3_link3_collision" class="collision" mesh="fr3_link3_coll"/>
3278
<body name="fr3_link4" pos="0.0825 0 0" quat="1 1 0 0" gravcomp="1">
3379
<inertial pos="-0.0459101 0.0630493 -0.00851879" quat="0.23761 0.892458 -0.00078702 0.383484"
3480
mass="2.6156" diaginertia="0.0206104 0.0206104 0.0206104"/>
35-
<joint name="fr3_joint4" axis="0 0 1" range="-3.0421 -0.1518" actuatorfrcrange="-87 87"/>
81+
<joint name="fr3_joint4" axis="0 0 1" range="-3.0421 -0.1518" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
3682
<geom mesh="fr3_link4_0" material="white" class="visual"/>
3783
<geom mesh="fr3_link4_1" material="black" class="visual"/>
3884
<geom name="fr3_link4_collision" class="collision" mesh="fr3_link4_coll"/>
3985
<body name="fr3_link5" pos="-0.0825 0.384 0" quat="1 -1 0 0" gravcomp="1">
4086
<inertial pos="-0.00160396 0.0292536 -0.0972966" quat="0.922285 0.098826 0.0982562 -0.360514"
4187
mass="2.32712" diaginertia="0.0182879 0.0182879 0.0182879"/>
42-
<joint name="fr3_joint5" axis="0 0 1" range="-2.8065 2.8065" actuatorfrcrange="-12 12"/>
88+
<joint name="fr3_joint5" axis="0 0 1" range="-2.8065 2.8065" actuatorfrcrange="-12 12" actuatorgravcomp="true"/>
4389
<geom mesh="fr3_link5_0" material="white" class="visual"/>
4490
<geom mesh="fr3_link5_1" material="white" class="visual"/>
4591
<geom mesh="fr3_link5_2" material="black" class="visual"/>
4692
<geom name="fr3_link5_collision" class="collision" mesh="fr3_link5_coll"/>
4793
<body name="fr3_link6" quat="1 1 0 0" gravcomp="1">
4894
<inertial pos="0.0597131 -0.0410295 -0.0101693" quat="0.593933 0.525442 0.520644 0.316361"
4995
mass="1.81704" diaginertia="0.00483538 0.00483538 0.00483538"/>
50-
<joint name="fr3_joint6" axis="0 0 1" range="0.5445 4.5169" actuatorfrcrange="-12 12"/>
96+
<joint name="fr3_joint6" axis="0 0 1" range="0.5445 4.5169" actuatorfrcrange="-12 12" actuatorgravcomp="true"/>
5197
<geom mesh="fr3_link6_0" material="button_green" class="visual"/>
5298
<geom mesh="fr3_link6_1" material="white" class="visual"/>
5399
<geom mesh="fr3_link6_2" material="white" class="visual"/>
@@ -60,7 +106,7 @@
60106
<body name="fr3_link7" pos="0.088 0 0" quat="1 1 0 0" gravcomp="1">
61107
<inertial pos="0.00452258 0.00862619 -0.0161633" quat="0.120255 0.394761 -0.799132 0.437139"
62108
mass="0.627143" diaginertia="3.076e-07 3.076e-07 3.076e-07"/>
63-
<joint name="fr3_joint7" axis="0 0 1" range="-3.0159 3.0159" actuatorfrcrange="-12 12"/>
109+
<joint name="fr3_joint7" axis="0 0 1" range="-3.0159 3.0159" actuatorfrcrange="-12 12" actuatorgravcomp="true"/>
64110
<geom mesh="fr3_link7_0" material="black" class="visual"/>
65111
<geom mesh="fr3_link7_1" material="white" class="visual"/>
66112
<geom mesh="fr3_link7_2" material="white" class="visual"/>

0 commit comments

Comments
 (0)