Add dof="2d" option to flexcomp for in-plane deformations.
PiperOrigin-RevId: 902620719 Change-Id: Ib06d3f7b9439e1d90a8373a4289ade0c327e72e4
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
cad734ae70
commit
2d12dee025
@@ -3593,7 +3593,7 @@ saving the XML:
|
||||
|
||||
.. _body-flexcomp-dof:
|
||||
|
||||
:at:`dof`: :at-val:`[full, radial, trilinear, quadratic], "full"`
|
||||
:at:`dof`: :at-val:`[full, radial, trilinear, quadratic, 2d], "full"`
|
||||
The parametrization of the flex's degrees of freedom (dofs). See the video on the right illustrating the
|
||||
different parametrizations with deformable spheres. The three models in the video are respectively
|
||||
`sphere_full <https://github.com/google-deepmind/mujoco/blob/main/model/flex/sphere_full.xml>`__,
|
||||
@@ -3608,6 +3608,10 @@ saving the XML:
|
||||
requires a free joint at the flex's parent in order for free body motion to be possible. This type of
|
||||
parametrization is appropriate for shapes that are relatively spherical.
|
||||
|
||||
**2d**
|
||||
Two orthogonal translational dofs (X and Y) per vertex. This restricts the motion of the vertices to planes
|
||||
parallel to the parent body's X-Y plane.
|
||||
|
||||
**trilinear**
|
||||
Three translational dofs at each corner of the bounding box of the flex, for a total of 24 dofs for the entire
|
||||
flex, independent of the number of vertices. The positions of the vertices are updated using trilinear
|
||||
|
||||
@@ -0,0 +1,429 @@
|
||||
<!-- Copyright 2026 DeepMind Technologies Limited
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<mujoco model="Gripper 2D Fin Ray">
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<option cone="elliptic" impratio="10" integrator="implicitfast"/>
|
||||
|
||||
<worldbody>
|
||||
<body name="hand" pos="0 0 .37">
|
||||
<joint name="lift" type="slide" range="-.13 .05"/>
|
||||
<geom type="box" size=".12 .06 .03" rgba=".2 .2 .2 1"/>
|
||||
|
||||
<body name="right_arm">
|
||||
<joint name="right" type="slide" axis="-1 0 0"/>
|
||||
<geom type="box" size=".015 .06 .06" pos=".10 0 -.09"
|
||||
rgba=".2 .2 .2 1"/>
|
||||
<body name="right_finger" pos=".1 0 -.15" euler="90 0 0">
|
||||
<!--
|
||||
Fin Ray skeleton mesh (right finger).
|
||||
|
||||
The mesh is the OUTLINE of the fin ray structure:
|
||||
two tapered vertical walls connected by horizontal ribs.
|
||||
Each strut is a thin strip of 2 triangles; the interior
|
||||
between ribs is empty. The radius property inflates the
|
||||
visual geometry to give the skeleton thickness.
|
||||
|
||||
Vertex layout (XY plane, 7 rib levels, 4 verts per level):
|
||||
Level i: L_front, L_back, R_front, R_back
|
||||
Each pair offset by h=0.0015 along Y to create in-plane thickness.
|
||||
Out-of-plane thickness set with radius parameter.
|
||||
|
||||
Side view of skeleton:
|
||||
|===================| level 0 (base, pinned)
|
||||
| |
|
||||
|=================| level 1
|
||||
| |
|
||||
|===============| level 2
|
||||
| |
|
||||
|============| level 3
|
||||
| |
|
||||
|==========| level 4
|
||||
| |
|
||||
|========| level 5
|
||||
| |
|
||||
|=====| level 6
|
||||
-->
|
||||
<flexcomp name="right_fin" type="direct" dim="2"
|
||||
radius=".004" rgba="0 .7 .7 1" mass=".3" dof="2d"
|
||||
point="
|
||||
-0.0210 0.0015 0.0000
|
||||
-0.0170 0.0015 0.0000
|
||||
-0.020538 -0.0015 0.0000
|
||||
-0.016538 -0.0015 0.0000
|
||||
0.0170 0.0015 0.0000
|
||||
0.0210 0.0015 0.0000
|
||||
0.016538 -0.0015 0.0000
|
||||
0.020538 -0.0015 0.0000
|
||||
-0.0190 -0.0115 0.0000
|
||||
-0.0150 -0.0115 0.0000
|
||||
-0.018538 -0.0145 0.0000
|
||||
-0.014538 -0.0145 0.0000
|
||||
0.0150 -0.0115 0.0000
|
||||
0.0190 -0.0115 0.0000
|
||||
0.014538 -0.0145 0.0000
|
||||
0.018538 -0.0145 0.0000
|
||||
-0.0170 -0.0245 0.0000
|
||||
-0.0130 -0.0245 0.0000
|
||||
-0.016308 -0.0275 0.0000
|
||||
-0.012308 -0.0275 0.0000
|
||||
0.0130 -0.0245 0.0000
|
||||
0.0170 -0.0245 0.0000
|
||||
0.012308 -0.0275 0.0000
|
||||
0.016308 -0.0275 0.0000
|
||||
-0.0140 -0.0375 0.0000
|
||||
-0.0100 -0.0375 0.0000
|
||||
-0.013308 -0.0405 0.0000
|
||||
-0.009308 -0.0405 0.0000
|
||||
0.0100 -0.0375 0.0000
|
||||
0.0140 -0.0375 0.0000
|
||||
0.009308 -0.0405 0.0000
|
||||
0.013308 -0.0405 0.0000
|
||||
-0.0110 -0.0505 0.0000
|
||||
-0.0070 -0.0505 0.0000
|
||||
-0.010308 -0.0535 0.0000
|
||||
-0.006308 -0.0535 0.0000
|
||||
0.0070 -0.0505 0.0000
|
||||
0.0110 -0.0505 0.0000
|
||||
0.006308 -0.0535 0.0000
|
||||
0.010308 -0.0535 0.0000
|
||||
-0.0080 -0.0635 0.0000
|
||||
-0.0040 -0.0635 0.0000
|
||||
-0.007200 -0.0665 0.0000
|
||||
-0.003200 -0.0665 0.0000
|
||||
0.0040 -0.0635 0.0000
|
||||
0.0080 -0.0635 0.0000
|
||||
0.003200 -0.0665 0.0000
|
||||
0.007200 -0.0665 0.0000
|
||||
-0.0040 -0.0785 0.0000
|
||||
-0.0001 -0.0785 0.0000
|
||||
-0.003200 -0.0815 0.0000
|
||||
0.000700 -0.0815 0.0000
|
||||
0.0001 -0.0785 0.0000
|
||||
0.0040 -0.0785 0.0000
|
||||
-0.000700 -0.0815 0.0000
|
||||
0.003200 -0.0815 0.0000
|
||||
"
|
||||
element="
|
||||
0 2 3
|
||||
0 3 1
|
||||
|
||||
4 6 7
|
||||
4 7 5
|
||||
|
||||
1 3 6
|
||||
1 6 4
|
||||
|
||||
8 10 11
|
||||
8 11 9
|
||||
|
||||
12 14 15
|
||||
12 15 13
|
||||
|
||||
9 11 14
|
||||
9 14 12
|
||||
|
||||
16 18 19
|
||||
16 19 17
|
||||
|
||||
20 22 23
|
||||
20 23 21
|
||||
|
||||
17 19 22
|
||||
17 22 20
|
||||
|
||||
24 26 27
|
||||
24 27 25
|
||||
|
||||
28 30 31
|
||||
28 31 29
|
||||
|
||||
25 27 30
|
||||
25 30 28
|
||||
|
||||
32 34 35
|
||||
32 35 33
|
||||
|
||||
36 38 39
|
||||
36 39 37
|
||||
|
||||
33 35 38
|
||||
33 38 36
|
||||
|
||||
40 42 43
|
||||
40 43 41
|
||||
|
||||
44 46 47
|
||||
44 47 45
|
||||
|
||||
41 43 46
|
||||
41 46 44
|
||||
|
||||
48 50 51
|
||||
48 51 49
|
||||
|
||||
52 54 55
|
||||
52 55 53
|
||||
|
||||
49 51 54
|
||||
49 54 52
|
||||
|
||||
2 8 9
|
||||
2 9 3
|
||||
|
||||
6 12 13
|
||||
6 13 7
|
||||
|
||||
10 16 17
|
||||
10 17 11
|
||||
|
||||
14 20 21
|
||||
14 21 15
|
||||
|
||||
18 24 25
|
||||
18 25 19
|
||||
|
||||
22 28 29
|
||||
22 29 23
|
||||
|
||||
26 32 33
|
||||
26 33 27
|
||||
|
||||
30 36 37
|
||||
30 37 31
|
||||
|
||||
34 40 41
|
||||
34 41 35
|
||||
|
||||
38 44 45
|
||||
38 45 39
|
||||
|
||||
42 48 49
|
||||
42 49 43
|
||||
|
||||
46 52 53
|
||||
46 53 47
|
||||
">
|
||||
<edge equality="true" damping="0.02"
|
||||
solimp="0.99 0.999 0.0001 0.5 2" solref="0.001 1"/>
|
||||
<contact selfcollide="none" internal="false"
|
||||
contype="2" conaffinity="2"/>
|
||||
<pin id="0 1 2 3 4 5 6 7"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<body name="left_arm">
|
||||
<joint name="left" type="slide" axis="1 0 0"/>
|
||||
<geom type="box" size=".015 .06 .06" pos="-.10 0 -.09"
|
||||
rgba=".2 .2 .2 1"/>
|
||||
<body name="left_finger" pos="-.1 0 -.15" euler="90 0 0">
|
||||
<flexcomp name="left_fin" type="direct" dim="2"
|
||||
radius=".004" rgba="0 .7 .7 1" mass=".3" dof="2d"
|
||||
point="
|
||||
-0.0210 0.0015 0.0000
|
||||
-0.0170 0.0015 0.0000
|
||||
-0.020538 -0.0015 0.0000
|
||||
-0.016538 -0.0015 0.0000
|
||||
0.0170 0.0015 0.0000
|
||||
0.0210 0.0015 0.0000
|
||||
0.016538 -0.0015 0.0000
|
||||
0.020538 -0.0015 0.0000
|
||||
-0.0190 -0.0115 0.0000
|
||||
-0.0150 -0.0115 0.0000
|
||||
-0.018538 -0.0145 0.0000
|
||||
-0.014538 -0.0145 0.0000
|
||||
0.0150 -0.0115 0.0000
|
||||
0.0190 -0.0115 0.0000
|
||||
0.014538 -0.0145 0.0000
|
||||
0.018538 -0.0145 0.0000
|
||||
-0.0170 -0.0245 0.0000
|
||||
-0.0130 -0.0245 0.0000
|
||||
-0.016308 -0.0275 0.0000
|
||||
-0.012308 -0.0275 0.0000
|
||||
0.0130 -0.0245 0.0000
|
||||
0.0170 -0.0245 0.0000
|
||||
0.012308 -0.0275 0.0000
|
||||
0.016308 -0.0275 0.0000
|
||||
-0.0140 -0.0375 0.0000
|
||||
-0.0100 -0.0375 0.0000
|
||||
-0.013308 -0.0405 0.0000
|
||||
-0.009308 -0.0405 0.0000
|
||||
0.0100 -0.0375 0.0000
|
||||
0.0140 -0.0375 0.0000
|
||||
0.009308 -0.0405 0.0000
|
||||
0.013308 -0.0405 0.0000
|
||||
-0.0110 -0.0505 0.0000
|
||||
-0.0070 -0.0505 0.0000
|
||||
-0.010308 -0.0535 0.0000
|
||||
-0.006308 -0.0535 0.0000
|
||||
0.0070 -0.0505 0.0000
|
||||
0.0110 -0.0505 0.0000
|
||||
0.006308 -0.0535 0.0000
|
||||
0.010308 -0.0535 0.0000
|
||||
-0.0080 -0.0635 0.0000
|
||||
-0.0040 -0.0635 0.0000
|
||||
-0.007200 -0.0665 0.0000
|
||||
-0.003200 -0.0665 0.0000
|
||||
0.0040 -0.0635 0.0000
|
||||
0.0080 -0.0635 0.0000
|
||||
0.003200 -0.0665 0.0000
|
||||
0.007200 -0.0665 0.0000
|
||||
-0.0040 -0.0785 0.0000
|
||||
-0.0001 -0.0785 0.0000
|
||||
-0.003200 -0.0815 0.0000
|
||||
0.000700 -0.0815 0.0000
|
||||
0.0001 -0.0785 0.0000
|
||||
0.0040 -0.0785 0.0000
|
||||
-0.000700 -0.0815 0.0000
|
||||
0.003200 -0.0815 0.0000
|
||||
"
|
||||
element="
|
||||
0 2 3
|
||||
0 3 1
|
||||
|
||||
4 6 7
|
||||
4 7 5
|
||||
|
||||
1 3 6
|
||||
1 6 4
|
||||
|
||||
8 10 11
|
||||
8 11 9
|
||||
|
||||
12 14 15
|
||||
12 15 13
|
||||
|
||||
9 11 14
|
||||
9 14 12
|
||||
|
||||
16 18 19
|
||||
16 19 17
|
||||
|
||||
20 22 23
|
||||
20 23 21
|
||||
|
||||
17 19 22
|
||||
17 22 20
|
||||
|
||||
24 26 27
|
||||
24 27 25
|
||||
|
||||
28 30 31
|
||||
28 31 29
|
||||
|
||||
25 27 30
|
||||
25 30 28
|
||||
|
||||
32 34 35
|
||||
32 35 33
|
||||
|
||||
36 38 39
|
||||
36 39 37
|
||||
|
||||
33 35 38
|
||||
33 38 36
|
||||
|
||||
40 42 43
|
||||
40 43 41
|
||||
|
||||
44 46 47
|
||||
44 47 45
|
||||
|
||||
41 43 46
|
||||
41 46 44
|
||||
|
||||
48 50 51
|
||||
48 51 49
|
||||
|
||||
52 54 55
|
||||
52 55 53
|
||||
|
||||
49 51 54
|
||||
49 54 52
|
||||
|
||||
2 8 9
|
||||
2 9 3
|
||||
|
||||
6 12 13
|
||||
6 13 7
|
||||
|
||||
10 16 17
|
||||
10 17 11
|
||||
|
||||
14 20 21
|
||||
14 21 15
|
||||
|
||||
18 24 25
|
||||
18 25 19
|
||||
|
||||
22 28 29
|
||||
22 29 23
|
||||
|
||||
26 32 33
|
||||
26 33 27
|
||||
|
||||
30 36 37
|
||||
30 37 31
|
||||
|
||||
34 40 41
|
||||
34 41 35
|
||||
|
||||
38 44 45
|
||||
38 45 39
|
||||
|
||||
42 48 49
|
||||
42 49 43
|
||||
|
||||
46 52 53
|
||||
46 53 47
|
||||
">
|
||||
<edge equality="true" damping="0.02"
|
||||
solimp="0.99 0.999 0.0001 0.5 2" solref="0.001 1"/>
|
||||
<contact selfcollide="none" internal="false"
|
||||
contype="2" conaffinity="2"/>
|
||||
<pin id="0 1 2 3 4 5 6 7"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<body>
|
||||
<freejoint/>
|
||||
<geom type="cylinder" size=".025 .05" pos="0 0 .1" rgba=".5 .5 0 1" euler="90 0 0" mass=".01"
|
||||
priority="1" contype="2" condim="6" friction="2" solref="0.001 1" solimp="0.99 0.999 0.0001 0.5 2"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<equality>
|
||||
<joint joint1="right" joint2="left"/>
|
||||
</equality>
|
||||
|
||||
<tendon>
|
||||
<fixed name="grasp">
|
||||
<joint joint="right" coef="1"/>
|
||||
<joint joint="left" coef="1"/>
|
||||
</fixed>
|
||||
</tendon>
|
||||
|
||||
<actuator>
|
||||
<position name="lift" joint="lift" kp="600" dampratio="1"
|
||||
ctrlrange="-1 1"/>
|
||||
<position name="grasp" tendon="grasp" kp="200" dampratio="1"
|
||||
ctrlrange="0 1"/>
|
||||
</actuator>
|
||||
|
||||
</mujoco>
|
||||
@@ -613,6 +613,17 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
|
||||
}
|
||||
}
|
||||
|
||||
// add two orthogonal sliders (x and y only)
|
||||
else if (doftype == mjFCOMPDOF_2D) {
|
||||
for (int j=0; j < 2; j++) {
|
||||
mjsJoint* jnt = mjs_addJoint(pb, 0);
|
||||
jnt->type = mjJNT_SLIDE;
|
||||
mjuu_setvec(jnt->pos, 0, 0, 0);
|
||||
mjuu_setvec(jnt->axis, 0, 0, 0);
|
||||
jnt->axis[j] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// construct body name, add to vertbody
|
||||
char txt[100];
|
||||
mju::sprintf_arr(txt, "%s_%d", name.c_str(), i);
|
||||
|
||||
@@ -45,6 +45,7 @@ typedef enum _mjtDof {
|
||||
mjFCOMPDOF_RADIAL,
|
||||
mjFCOMPDOF_TRILINEAR,
|
||||
mjFCOMPDOF_QUADRATIC,
|
||||
mjFCOMPDOF_2D,
|
||||
|
||||
mjNFCOMPDOFS
|
||||
} mjtDof;
|
||||
|
||||
@@ -932,7 +932,8 @@ const mjMap fdof_map[mjNFCOMPDOFS] = {
|
||||
{"full", mjFCOMPDOF_FULL},
|
||||
{"radial", mjFCOMPDOF_RADIAL},
|
||||
{"trilinear", mjFCOMPDOF_TRILINEAR},
|
||||
{"quadratic", mjFCOMPDOF_QUADRATIC}
|
||||
{"quadratic", mjFCOMPDOF_QUADRATIC},
|
||||
{"2d", mjFCOMPDOF_2D}
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -1227,6 +1227,71 @@ TEST_F(UserFlexTest, TotalMassQuadratic) {
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
TEST_F(UserFlexTest, Dof2d) {
|
||||
// 3x3 grid with dof="2d": 9 vertices, 2 DOFs each -> nv = 18
|
||||
static constexpr char xml_2d[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<flexcomp name="test" type="grid" count="3 3 1" spacing=".1 .1 .1"
|
||||
dim="2" radius=".01" dof="2d">
|
||||
<edge equality="true"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
// same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv = 27
|
||||
static constexpr char xml_full[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<flexcomp name="test" type="grid" count="3 3 1" spacing=".1 .1 .1"
|
||||
dim="2" radius=".01">
|
||||
<edge equality="true"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
std::array<char, 1024> error;
|
||||
|
||||
// load 2d model
|
||||
mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size());
|
||||
ASSERT_THAT(m_2d, NotNull()) << error.data();
|
||||
mjData* d_2d = mj_makeData(m_2d);
|
||||
|
||||
// load full model
|
||||
mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size());
|
||||
ASSERT_THAT(m_full, NotNull()) << error.data();
|
||||
mjData* d_full = mj_makeData(m_full);
|
||||
|
||||
// verify DOF counts
|
||||
EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs
|
||||
EXPECT_EQ(m_full->nv, 27); // 9 vertices * 3 DOFs
|
||||
|
||||
// same number of vertices and elements
|
||||
EXPECT_EQ(m_2d->nflexvert, m_full->nflexvert);
|
||||
EXPECT_EQ(m_2d->nflexelem, m_full->nflexelem);
|
||||
|
||||
// each body has 2 DOFs in 2d mode, 3 in full mode
|
||||
for (int i = 1; i < m_2d->nbody; i++) {
|
||||
EXPECT_EQ(m_2d->body_dofnum[i], 2) << "body " << i;
|
||||
}
|
||||
for (int i = 1; i < m_full->nbody; i++) {
|
||||
EXPECT_EQ(m_full->body_dofnum[i], 3) << "body " << i;
|
||||
}
|
||||
|
||||
// simulate a few steps to make sure nothing crashes
|
||||
for (int i = 0; i < 10; i++) {
|
||||
mj_step(m_2d, d_2d);
|
||||
mj_step(m_full, d_full);
|
||||
}
|
||||
|
||||
mj_deleteModel(m_2d);
|
||||
mj_deleteModel(m_full);
|
||||
mj_deleteData(d_2d);
|
||||
mj_deleteData(d_full);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
|
||||
Reference in New Issue
Block a user