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:
|
.. _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
|
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
|
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>`__,
|
`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
|
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.
|
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**
|
**trilinear**
|
||||||
Three translational dofs at each corner of the bounding box of the flex, for a total of 24 dofs for the entire
|
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
|
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
|
// construct body name, add to vertbody
|
||||||
char txt[100];
|
char txt[100];
|
||||||
mju::sprintf_arr(txt, "%s_%d", name.c_str(), i);
|
mju::sprintf_arr(txt, "%s_%d", name.c_str(), i);
|
||||||
|
|||||||
@@ -45,6 +45,7 @@ typedef enum _mjtDof {
|
|||||||
mjFCOMPDOF_RADIAL,
|
mjFCOMPDOF_RADIAL,
|
||||||
mjFCOMPDOF_TRILINEAR,
|
mjFCOMPDOF_TRILINEAR,
|
||||||
mjFCOMPDOF_QUADRATIC,
|
mjFCOMPDOF_QUADRATIC,
|
||||||
|
mjFCOMPDOF_2D,
|
||||||
|
|
||||||
mjNFCOMPDOFS
|
mjNFCOMPDOFS
|
||||||
} mjtDof;
|
} mjtDof;
|
||||||
|
|||||||
@@ -932,7 +932,8 @@ const mjMap fdof_map[mjNFCOMPDOFS] = {
|
|||||||
{"full", mjFCOMPDOF_FULL},
|
{"full", mjFCOMPDOF_FULL},
|
||||||
{"radial", mjFCOMPDOF_RADIAL},
|
{"radial", mjFCOMPDOF_RADIAL},
|
||||||
{"trilinear", mjFCOMPDOF_TRILINEAR},
|
{"trilinear", mjFCOMPDOF_TRILINEAR},
|
||||||
{"quadratic", mjFCOMPDOF_QUADRATIC}
|
{"quadratic", mjFCOMPDOF_QUADRATIC},
|
||||||
|
{"2d", mjFCOMPDOF_2D}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -1227,6 +1227,71 @@ TEST_F(UserFlexTest, TotalMassQuadratic) {
|
|||||||
mj_deleteModel(m);
|
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
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user