- Added passive forces plugins

- Added new `cable` composite type:
  * The `initial` parameter specifies the joint at the starting boundary: `free`, `ball`, or `none`.
  * The boundary bodies are exposed with the names:`B_left` and `B_right`.
  * The vertex initial positions can be specified directly in the XML with the parameter `vertex`.
  * The orientation of the body frame **is** the orientation of the material frame of the curve.

- Added new `cable` passive force plugin:
  * Twist and bending stiffness can be set separately with the parameters `twist` and `bend`.
  * The stress-free configuration can be set to be the initial one or flat with the flag `flat`.
  * New cable example showing the formation of plectoneme.
  * New coil example.
  * New belt example showing interaction between twist and anisotropy.
  * Added test using cantilever exact solution.

PiperOrigin-RevId: 480033694
Change-Id: I491271bce8fccb185961477e903e5a72d172c8a3
This commit is contained in:
Alessio Quaglino
2022-10-10 02:45:59 -07:00
committed by Copybara-Service
parent 794ef0b771
commit e250ff0d5a
34 changed files with 1564 additions and 72 deletions
+1
View File
@@ -80,6 +80,7 @@ target_include_directories(
PRIVATE src
)
add_subdirectory(plugin/elasticity)
add_subdirectory(src/engine)
add_subdirectory(src/user)
add_subdirectory(src/xml)
+18
View File
@@ -1403,6 +1403,8 @@ mjModel
int ntupledata; // number of objects in all tuple fields
int nkey; // number of keyframes
int nmocap; // number of mocap bodies
int nplugin; // number of plugin instances
int npluginattr; // number of chars in all plugin config attributes
int nuser_body; // number of mjtNums in body_user
int nuser_jnt; // number of mjtNums in jnt_user
int nuser_geom; // number of mjtNums in geom_user
@@ -1415,12 +1417,14 @@ mjModel
// sizes set after mjModel construction (only affect mjData)
int nM; // number of non-zeros in sparse inertia matrix
int nD; // number of non-zeros in sparse derivative matrix
int nemax; // number of potential equality-constraint rows
int njmax; // number of available rows in constraint Jacobian
int nconmax; // number of potential contacts in contact list
int nstack; // number of fields in mjData stack
int nuserdata; // number of extra fields in mjData
int nsensordata; // number of fields in sensor data vector
int npluginstate; // number of fields in the plugin state vector
int nbuffer; // number of bytes in buffer
@@ -1461,6 +1465,7 @@ mjModel
mjtNum* body_inertia; // diagonal inertia in ipos/iquat frame (nbody x 3)
mjtNum* body_invweight0; // mean inv inert in qpos0 (trn, rot) (nbody x 2)
mjtNum* body_user; // user data (nbody x nuser_body)
int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1)
// joints
int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
@@ -1576,6 +1581,7 @@ mjModel
// skins
int* skin_matid; // skin material id; -1: none (nskin x 1)
int* skin_group; // group for visibility (nskin x 1)
float* skin_rgba; // skin rgba (nskin x 4)
float* skin_inflate; // inflate skin in normal direction (nskin x 1)
int* skin_vertadr; // first vertex address (nskin x 1)
@@ -1679,17 +1685,20 @@ mjModel
int* actuator_group; // group for visibility (nu x 1)
mjtByte* actuator_ctrllimited; // is control limited (nu x 1)
mjtByte* actuator_forcelimited;// is force limited (nu x 1)
mjtByte* actuator_actlimited; // is activation limited (nu x 1)
mjtNum* actuator_dynprm; // dynamics parameters (nu x mjNDYN)
mjtNum* actuator_gainprm; // gain parameters (nu x mjNGAIN)
mjtNum* actuator_biasprm; // bias parameters (nu x mjNBIAS)
mjtNum* actuator_ctrlrange; // range of controls (nu x 2)
mjtNum* actuator_forcerange; // range of forces (nu x 2)
mjtNum* actuator_actrange; // range of activations (nu x 2)
mjtNum* actuator_gear; // scale length and transmitted force (nu x 6)
mjtNum* actuator_cranklength; // crank length for slider-crank (nu x 1)
mjtNum* actuator_acc0; // acceleration from unit force in qpos0 (nu x 1)
mjtNum* actuator_length0; // actuator length in qpos0 (nu x 1)
mjtNum* actuator_lengthrange; // feasible actuator length range (nu x 2)
mjtNum* actuator_user; // user data (nu x nuser_actuator)
int* actuator_plugin; // plugin instance id; -1: not a plugin (nu x 1)
// sensors
int* sensor_type; // sensor type (mjtSensor) (nsensor x 1)
@@ -1704,6 +1713,14 @@ mjModel
mjtNum* sensor_cutoff; // cutoff for real and positive; 0: ignore (nsensor x 1)
mjtNum* sensor_noise; // noise standard deviation (nsensor x 1)
mjtNum* sensor_user; // user data (nsensor x nuser_sensor)
int* sensor_plugin; // plugin instance id; -1: not a plugin (nsensor x 1)
// plugin instances
int* plugin; // globally registered plugin slot number (nplugin x 1)
int* plugin_stateadr; // address in the plugin state array (nplugin x 1)
int* plugin_statenum; // number of states in the plugin instance (nplugin x 1)
char* plugin_attr; // config attributes of plugin instances (npluginattr x 1)
int* plugin_attradr; // address to each instance's config attrib (nplugin x 1)
// custom numeric fields
int* numeric_adr; // address of field in numeric_data (nnumeric x 1)
@@ -1753,6 +1770,7 @@ mjModel
int* name_textadr; // text name pointers (ntext x 1)
int* name_tupleadr; // tuple name pointers (ntuple x 1)
int* name_keyadr; // keyframe name pointers (nkey x 1)
int* name_pluginadr; // plugin instance name pointers (nplugin x 1)
char* names; // names of all objects, 0-terminated (nnames x 1)
};
typedef struct _mjModel mjModel;
+17 -1
View File
@@ -2286,7 +2286,7 @@ coordinates results in compiler error. See :ref:`CComposite` in the modeling gui
All automatically generated model elements have names indicating the element type and index. For example, the body at
coordinates (2, 0) in a 2D grid is named "B2_0" by default. If prefix="C" is specified, the same body is named
"CB2_0". The prefix is needed when multiple composite objects are used in the same model, to avoid name conflicts.
:at:`type`: :at-val:`[particle, grid, rope, loop, cloth, box, cylinder, ellipsoid], required`
:at:`type`: :at-val:`[particle, grid, cable, rope, loop, cloth, box, cylinder, ellipsoid], required`
This attribute determines the type of composite object. The remaining attributes and sub-elements are then
interpreted according to the type. Default settings are also adjusted depending on the type.
@@ -2316,6 +2316,12 @@ coordinates results in compiler error. See :ref:`CComposite` in the modeling gui
elements are equality-constrained to remain connected (using the "connect" constraint type). The softness of this
equality constraint is adjusted with the attributes solrefsmooth and solimpsmooth.
The **cable** type creates a 1D chain of bodies connected with ball joints, each having a geom with user-defined type
(cylinder, capsule or box). The geometry can either be defined with an array of 3D vertex coordinates :at:`vertex`
or with prescribed functions with the option :at:`curve`. Currently, only linear and trigonometric functions are
supported. For example, an helix can be obtained with curve="cos(s) sin(s) s". The size is set with the option
:at:`size`, resulting in :math:`f(s)=(size[1]*\cos(2*\pi*size[2]), size[1]*\sin(2*\pi*size[2]), size[0]*s)`.
The **cloth** type is a different way to model cloth, beyond type="grid". Here the elements are connected with
universal joints and form a kinematic spanning tree. The root of the tree is the parent body, and its coordinates in
the grid are inferred from its name - similar to rope but here the naming format is "CB2_0". Neighboring bodies that
@@ -2370,6 +2376,16 @@ coordinates results in compiler error. See :ref:`CComposite` in the modeling gui
smoothness-preserving equality constraint for box, cylinder and ellipsoid types. For all other types they have no
effect. They obey the same rules as all other solref and solimp attributes in MJCF, except their defaults here are
adjusted depending on the composite type. See :ref:`CSolver`.
:at:`vertex`: :at-val:`real(3*nvert), optional`
Vertex 3D positions in global coordinates (cable only).
:at:`initial`: :at-val:`[free, ball, none], "0"`
Behavior of the first point (cable only). Free: free joint. Ball: ball joint. None: no dof.
:at:`curve`: :at-val:`string(3), optional`
Functions specifying the vertex positions (cable only). Available functions are `s`, `cos(s)`, and `sin(s)`, where
`s` is the arc length parameter.
:at:`size`: :at-val:`int(3), optional`
Scaling of the curve functions (cable only). `size[0]` is the scaling of `s`, `size[1]` is the radius of `\cos(s)`
and `\sin(s)`, and `size[2]` is the speed of the argument (i.e. `\cos(2*\pi*size[2]*s)`).
.. _composite-joint:
+18
View File
@@ -60,6 +60,24 @@ General
- Added :at:`realtime` option to :ref:`visual` for starting a simulation at a slower speed.
- Added new :at:`cable` composite type:
* Cable elements are connected with ball joints.
* The `initial` parameter specifies the joint at the starting boundary: :at:`free`, :at:`ball`, or :at:`none`.
* The boundary bodies are exposed with the names :at:`B_left` and :at:`B_right`.
* The vertex initial positions can be specified directly in the XML with the parameter :at:`vertex`.
* The orientation of the body frame **is** the orientation of the material frame of the curve.
- Added new :at:`cable` passive force plugin:
* Twist and bending stiffness can be set separately with the parameters :at:`twist` and :at:`bend`.
* The stress-free configuration can be set to be the initial one or flat with the flag :at:`flat`.
* New `cable.xml <https://github.com/deepmind/mujoco/tree/main/model/plugin/cable.xml>`_ example
showing the formation of plectoneme.
* New `coil.xml <https://github.com/deepmind/mujoco/tree/main/model/plugin/coil.xml>`_ example
showing a curved equilibrium configuration.
* New `belt.xml <https://github.com/deepmind/mujoco/tree/main/model/plugin/belt.xml>`_ example
showing interaction between twist and anisotropy.
* Added test using cantilever exact solution.
Python bindings
^^^^^^^^^^^^^^^
+4 -2
View File
@@ -632,6 +632,7 @@ struct mjModel_ {
mjtNum* body_inertia; // diagonal inertia in ipos/iquat frame (nbody x 3)
mjtNum* body_invweight0; // mean inv inert in qpos0 (trn, rot) (nbody x 2)
mjtNum* body_user; // user data (nbody x nuser_body)
int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1)
// joints
int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
@@ -864,7 +865,7 @@ struct mjModel_ {
mjtNum* actuator_length0; // actuator length in qpos0 (nu x 1)
mjtNum* actuator_lengthrange; // feasible actuator length range (nu x 2)
mjtNum* actuator_user; // user data (nu x nuser_actuator)
int* actuator_plugin; // plugin instance id; -1: not a plugin actuator (nu x 1)
int* actuator_plugin; // plugin instance id; -1: not a plugin (nu x 1)
// sensors
int* sensor_type; // sensor type (mjtSensor) (nsensor x 1)
@@ -879,11 +880,12 @@ struct mjModel_ {
mjtNum* sensor_cutoff; // cutoff for real and positive; 0: ignore (nsensor x 1)
mjtNum* sensor_noise; // noise standard deviation (nsensor x 1)
mjtNum* sensor_user; // user data (nsensor x nuser_sensor)
int* sensor_plugin; // plugin instance id; -1: not a plugin sensor (nsensor x 1)
int* sensor_plugin; // plugin instance id; -1: not a plugin (nsensor x 1)
// plugin instances
int* plugin; // globally registered plugin slot number (nplugin x 1)
int* plugin_stateadr; // address in the plugin state array (nplugin x 1)
int* plugin_statenum; // number of states in the plugin instance (nplugin x 1)
char* plugin_attr; // config attributes of plugin instances (npluginattr x 1)
int* plugin_attradr; // address to each instance's config attrib (nplugin x 1)
+4 -3
View File
@@ -20,7 +20,8 @@
typedef enum mjtPluginTypeBit_ {
mjPLUGIN_ACTUATOR = 1<<0,
mjPLUGIN_SENSOR = 1<<1
mjPLUGIN_SENSOR = 1<<1,
mjPLUGIN_PASSIVE = 1<<2,
} mjtPluginTypeBit;
struct mjpPlugin_ {
@@ -38,8 +39,8 @@ struct mjpPlugin_ {
// dimension of the specified sensor's output (required only for sensor plugins)
int (*nsensordata)(const mjModel* m, int instance, int sensor_id);
// called when a new mjData is being created (required)
void (*init)(const mjModel* m, mjData* d, int instance);
// called when a new mjData is being created (required), returns 0 on success or -1 on failure
int (*init)(const mjModel* m, mjData* d, int instance);
// called when an mjData is being freed (optional)
void (*destroy)(mjData* d, int instance);
+2
View File
@@ -171,6 +171,7 @@
X( mjtNum, body_inertia, nbody, 3 ) \
X( mjtNum, body_invweight0, nbody, 2 ) \
X( mjtNum, body_user, nbody, MJ_M(nuser_body) ) \
X( int, body_plugin, nbody, 1 ) \
X( int, jnt_type, njnt, 1 ) \
X( int, jnt_qposadr, njnt, 1 ) \
X( int, jnt_dofadr, njnt, 1 ) \
@@ -385,6 +386,7 @@
X( int, sensor_plugin, nsensor, 1 ) \
X( int, plugin, nplugin, 1 ) \
X( int, plugin_stateadr, nplugin, 1 ) \
X( int, plugin_statenum, nplugin, 1 ) \
X( char, plugin_attr, npluginattr, 1 ) \
X( int, plugin_attradr, nplugin, 1 ) \
X( int, numeric_adr, nnumeric, 1 ) \
+1
View File
@@ -415,6 +415,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
values=dict([
('mjPLUGIN_ACTUATOR', 1),
('mjPLUGIN_SENSOR', 2),
('mjPLUGIN_PASSIVE', 4),
]),
)),
('mjtGridPos',
+76
View File
@@ -0,0 +1,76 @@
<!-- Copyright 2021 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="Belt">
<include file="scene.xml"/>
<extension>
<required plugin="mujoco.elasticity.cable"/>
</extension>
<statistic center="0 0 .2" extent="1"/>
<visual>
<global elevation="-30"/>
</visual>
<option timestep="0.001">
<flag gravity="disable"/>
</option>
<compiler autolimits="true"/>
<worldbody>
<composite type="cable" offset="-.5 0 .05" initial="none"
vertex="
0. 0. 0.
0.05 0. 0.05
0.1 0. 0.1
0.15 0. 0.15
0.2 0. 0.2
0.25 0. 0.25
0.3 0. 0.3
0.35 0. 0.35
0.4 0. 0.4
0.45 0. 0.45
0.5 0. 0.5
0.55 0. 0.45
0.6 0. 0.4
0.65 0. 0.35
0.7 0. 0.3
0.75 0. 0.25
0.8 0. 0.2
0.85 0. 0.15
0.9 0. 0.1
0.95 0. 0.05
1.0 0. 0. ">
<plugin plugin="mujoco.elasticity.cable">
<!--Units are in Pa (SI)-->
<config key="twist" value="1e8"/>
<config key="bend" value="1e7"/>
</plugin>
<joint kind="main" damping="0.1"/>
<geom type="box" size="0.03535533906 0.02 0.0015" rgba=".8 .2 .1 1" group="3"/>
<skin subgrid="3"/>
</composite>
</worldbody>
<equality>
<connect name="right_boundary" body1="B_last" anchor="0.07 0 0"/>
</equality>
<actuator>
<motor site="S_last" gear="0 0 0 1 0 0" ctrlrange="-.2 .2"/>
</actuator>
</mujoco>
+56
View File
@@ -0,0 +1,56 @@
<!-- Copyright 2021 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="Cable">
<include file="scene.xml"/>
<extension>
<required plugin="mujoco.elasticity.cable"/>
</extension>
<statistic center="0 0 .3" extent="1"/>
<visual>
<global elevation="-30"/>
</visual>
<compiler autolimits="true"/>
<size nconmax="100" njmax="300" nstack="500000"/>
<worldbody>
<composite type="cable" curve="s" count="41 1 1" size="1" offset="-.3 0 .6" initial="none">
<plugin plugin="mujoco.elasticity.cable">
<!--Units are in Pa (SI)-->
<config key="twist" value="1e7"/>
<config key="bend" value="4e6"/>
</plugin>
<joint kind="main" damping=".015"/>
<geom type="capsule" size=".005" rgba=".8 .2 .1 1" condim="1"/>
</composite>
<body name="slider" pos=".7 0 .6">
<joint type="slide" axis="1 0 0" damping=".1"/>
<geom size=".01"/>
</body>
</worldbody>
<equality>
<connect name="right_boundary" body1="B_last" body2="slider" anchor=".025 0 0"/>
</equality>
<contact>
<exclude body1="B_last" body2="slider"/>
</contact>
<actuator>
<motor site="S_last" gear="0 0 0 1 0 0" ctrlrange="-.03 .03"/>
</actuator>
</mujoco>
+104
View File
@@ -0,0 +1,104 @@
<!-- Copyright 2021 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="Coil">
<include file="scene.xml"/>
<extension>
<required plugin="mujoco.elasticity.cable"/>
</extension>
<statistic center="0 0 .2" extent="1"/>
<compiler autolimits="true"/>
<visual>
<global elevation="-30"/>
</visual>
<default>
<geom solref="0.01 1" solimp=".95 .99 .0001"/>
</default>
<worldbody>
<composite prefix="free" type="cable" offset="-.25 0 .3" initial="free"
vertex="
0.1 0. 0.
0.0809017 0.05877853 0.001256637
0.0309017 0.09510565 0.002513274
-0.0309017 0.09510565 0.003769911
-0.0809017 0.05877853 0.005026548
-0.1 0. 0.006283185
-0.0809017 -0.05877853 0.007539822
-0.0309017 -0.09510565 0.008796459
0.0309017 -0.09510565 0.010053096
0.0809017 -0.05877853 0.011309734
0.1 -0. 0.012566371
0.0809017 0.05877853 0.01256637
0.0309017 0.09510565 0.02513274
-0.0309017 0.09510565 0.03769911
-0.0809017 0.05877853 0.05026548
-0.1 0. 0.06283185
-0.0809017 -0.05877853 0.07539822
-0.0309017 -0.09510565 0.08796459
0.0309017 -0.09510565 0.10053096
0.0809017 -0.05877853 0.11309734
0.1 -0. 0.12566371
0.0809017 0.05877853 0.13823008
0.0309017 0.09510565 0.15079645
-0.0309017 0.09510565 0.16336282
-0.0809017 0.05877853 0.17592919
-0.1 0. 0.18849556
-0.0809017 -0.05877853 0.20106193
-0.0309017 -0.09510565 0.2136283
0.0309017 -0.09510565 0.22619467
0.0809017 -0.05877853 0.23876104
0.1 -0. 0.25132741">
<plugin plugin="mujoco.elasticity.cable">
<!--Units are in Pa (SI)-->
<config key="twist" value="5e8"/>
<config key="bend" value="15e8"/>
</plugin>
<joint kind="main" damping="0.12" armature="0.01"/>
<geom type="capsule" size=".005" rgba=".8 .2 .1 1"/>
</composite>
<composite prefix="actuated" type="cable" curve="cos(s) sin(s) s" count="41 1 1"
size="0.251327412 .1 4" offset="0.25 0 .05" initial="fixed">
<plugin plugin="mujoco.elasticity.cable">
<!--Units are in Pa (SI)-->
<config key="twist" value="5e8"/>
<config key="bend" value="15e8"/>
</plugin>
<joint kind="main" damping="0.1" armature="0.01"/>
<geom type="capsule" size=".005" rgba=".8 .2 .1 1"/>
</composite>
<body name="slider" pos="0.35 0 0.301327412">
<joint name="slide" type="slide" axis="0 0 1" damping="5"/>
<geom size=".01"/>
</body>
</worldbody>
<equality>
<connect name="right_boundary" body1="actuatedB_last" body2="slider" anchor="0.03 0 0" solref="0.0002 1"/>
</equality>
<contact>
<exclude body1="actuatedB_last" body2="slider"/>
</contact>
<actuator>
<position joint="slide" name="extend" gear="1 0 0 0 0 0" ctrlrange="-.25 1" kp="100"/>
</actuator>
</mujoco>
+41
View File
@@ -0,0 +1,41 @@
<!-- Copyright 2021 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>
<compiler meshdir="asset" texturedir="asset"/>
<statistic meansize=".05"/>
<visual>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<map stiffness="700" shadowscale="0.5" fogstart="1" fogend="15" zfar="40" haze="1"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="512"/>
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="10 10" texuniform="true"/>
</asset>
<worldbody>
<light diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 2.0" dir="0 0 -1" castshadow="false"/>
<light directional="true" diffuse=".8 .8 .8" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
<geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0" material="matplane" condim="1"/>
</worldbody>
</mujoco>
+29
View File
@@ -0,0 +1,29 @@
# Copyright 2022 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
#
# https://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.
set(MUJOCO_ELASTICITY_INCLUDE
${CMAKE_CURRENT_SOURCE_DIR}/../..
${CMAKE_CURRENT_SOURCE_DIR}/../../src
)
set(MUJOCO_ELASTICITY_SRCS
cable.cc
cable.h
)
add_library(cable SHARED)
target_sources(cable PRIVATE ${MUJOCO_ELASTICITY_SRCS})
target_include_directories(cable PRIVATE ${MUJOCO_ELASTICITY_INCLUDE})
target_link_libraries(cable PRIVATE mujoco)
+257
View File
@@ -0,0 +1,257 @@
// Copyright 2022 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.
#include <algorithm>
#include <cstddef>
#include <sstream>
#include <optional>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "cable.h"
namespace mujoco::plugin::elasticity {
namespace {
// compute quaternion difference between two frames in joint coordinates
void QuatDiff(mjtNum* quat, const mjtNum body_quat[4],
const mjtNum joint_quat[4], bool pullback) {
if (pullback == 0) {
// contribution in local coordinates
mju_mulQuat(quat, body_quat, joint_quat);
} else {
// contribution pulled-back in local coordinates of the other body
mjtNum invquat[4];
mju_mulQuat(invquat, body_quat, joint_quat);
mju_negQuat(quat, invquat);
}
}
// compute local force given material properties, orientation,
// and reference curvature
// inputs:
// stiffness - material parameters
// quat - orientation of the body in local coordinates
// omega0 - initial curvature
// xquat - cartesian orientation of the body (optional)
// scl - scaling of the force
// outputs:
// qfrc - local torque contribution
void LocalForce(mjtNum qfrc[3], const mjtNum stiffness[4],
const mjtNum quat[4], const mjtNum omega0[3],
const mjtNum xquat[4], mjtNum scl) {
mjtNum omega[3], lfrc[3];
// compute curvature
mju_quat2Vel(omega, quat, scl);
// subtract omega0 in reference configuration
mjtNum tmp[] = {
- stiffness[0]*(omega[0] - omega0[0]) / stiffness[3],
- stiffness[1]*(omega[1] - omega0[1]) / stiffness[3],
- stiffness[2]*(omega[2] - omega0[2]) / stiffness[3],
};
// rotate into global frame
if (xquat) {
mju_rotVecQuat(lfrc, tmp, xquat);
} else {
mju_copy3(lfrc, tmp);
}
// add to total qfrc
mju_addToScl3(qfrc, lfrc, scl);
}
// reads numeric attributes
bool CheckAttr(const char* name, const mjModel* m, int instance) {
char *end;
std::string value = mj_getPluginConfig(m, instance, name);
value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end());
strtod(value.c_str(), &end);
return end == value.data() + value.size();
}
} // namespace
// factory function
std::optional<Elasticity> Elasticity::Create(
const mjModel* m, mjData* d, int instance) {
if (CheckAttr("twist", m, instance) && CheckAttr("bend", m, instance)) {
return Elasticity(m, d, instance);
} else {
mju_warning("Invalid parameter specification in cable plugin");
return std::nullopt;
}
}
// plugin constructor
Elasticity::Elasticity(const mjModel* m, mjData* d, int instance) {
// parameters were validated by the factor function
std::string flat = mj_getPluginConfig(m, instance, "flat");
mjtNum G = strtod(mj_getPluginConfig(m, instance, "twist"), nullptr);
mjtNum E = strtod(mj_getPluginConfig(m, instance, "bend"), nullptr);
// count plugin bodies
n = 0;
for (int i = 1; i < m->nbody; i++) {
if (m->body_plugin[i] == instance) {
if (!n++) {
i0 = i;
}
}
}
// allocate arrays
prev.assign(n, 0); // index of previous body
next.assign(n, 0); // index of next body
omega0.assign(3*n, 0); // reference curvature
stiffness.assign(4*n, 0); // material parameters
// run forward kinematics to populate xquat (mjData not yet initialized)
mju_zero(d->mocap_quat, 4*m->nmocap);
mju_copy(d->qpos, m->qpos0, m->nq);
mj_kinematics(m, d);
// compute initial curvature
for (int b = 0; b < n; b++) {
int i = i0 + b;
if (m->body_plugin[i] != instance) {
mju_error("This body does not have the requested plugin instance");
}
bool first = (b == 0), last = (b == n-1);
prev[b] = first ? 0 : -1;
next[b] = last ? 0 : +1;
// compute omega0: curvature at equilibrium
if (prev[b] && flat != "true") {
int qadr = m->jnt_qposadr[m->body_jntadr[i]] + m->body_dofnum[i]-3;
mju_subQuat(omega0.data()+3*b, m->body_quat+4*i, d->qpos+qadr);
} else {
mju_zero3(omega0.data()+3*b);
}
// compute physical parameters
int geom_i = m->body_geomadr[i];
mjtNum J = 0, Iy = 0, Iz = 0;
if (m->geom_type[geom_i] == mjGEOM_CYLINDER ||
m->geom_type[geom_i] == mjGEOM_CAPSULE) {
// https://en.wikipedia.org/wiki/Torsion_constant#Circle
// https://en.wikipedia.org/wiki/List_of_second_moments_of_area
J = mjPI * pow(m->geom_size[3*geom_i+0], 4) / 2;
Iy = Iz = mjPI * pow(m->geom_size[3*geom_i+0], 4) / 4.;
} else if (m->geom_type[geom_i] == mjGEOM_BOX) {
// https://en.wikipedia.org/wiki/Torsion_constant#Rectangle
// https://en.wikipedia.org/wiki/List_of_second_moments_of_area
mjtNum h = m->geom_size[3*geom_i+1];
mjtNum w = m->geom_size[3*geom_i+2];
mjtNum a = std::max(h, w);
mjtNum b = std::min(h, w);
J = a*pow(b, 3)*(16./3.-3.36*b/a*(1-pow(b, 4)/pow(a, 4)/12));
Iy = pow(2 * w, 3) * 2 * h / 12.;
Iz = pow(2 * h, 3) * 2 * w / 12.;
}
stiffness[4*b+0] = J * G;
stiffness[4*b+1] = Iy * E;
stiffness[4*b+2] = Iz * E;
stiffness[4*b+3] =
prev[b] ? mju_dist3(d->xpos+3*i, d->xpos+3*(i+prev[b])) : 0;
}
}
void Elasticity::Compute(const mjModel* m, mjData* d, int instance) {
for (int b = 0; b < n; b++) {
// index into body array
int i = i0 + b;
if (m->body_plugin[i] != instance) {
mju_error(
"This body is not associated with the requested plugin instance");
}
// if no stiffness, skip body
if (!stiffness[b*4+0] && !stiffness[b*4+1] && !stiffness[b*4+2]) {
continue;
}
// elastic forces
mjtNum quat[4] = {0};
mjtNum xfrc[3] = {0};
// local orientation
if (prev[b]) {
int qadr = m->jnt_qposadr[m->body_jntadr[i]] + m->body_dofnum[i]-3;
QuatDiff(quat, m->body_quat+4*i, d->qpos+qadr, 0);
// contribution of orientation i-1 to xfrc i
LocalForce(xfrc, stiffness.data()+4*b, quat, omega0.data()+3*b,
d->xquat+4*(i+prev[b]), 1);
}
if (next[b]) {
int bn = b + next[b];
int in = i + next[b];
// local orientation
int qadr = m->jnt_qposadr[m->body_jntadr[in]] + m->body_dofnum[in]-3;
QuatDiff(quat, m->body_quat+4*in, d->qpos+qadr, 1);
// contribution of orientation i+1 to xfrc i
LocalForce(xfrc, stiffness.data()+4*bn, quat, omega0.data()+3*bn,
d->xquat+4*i, -1);
}
// convert from global coordinates and apply torque to com
mj_applyFT(m, d, 0, xfrc, d->xpos+3*i, i, d->qfrc_passive);
}
}
mjPLUGIN_DYNAMIC_LIBRARY_INIT {
mjpPlugin plugin;
mjp_defaultPlugin(&plugin);
plugin.name = "mujoco.elasticity.cable";
plugin.type |= mjPLUGIN_PASSIVE;
const char* attributes[] = {"twist", "bend", "flat"};
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
plugin.attributes = attributes;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
auto elasticity_or_null = Elasticity::Create(m, d, instance);
if (!elasticity_or_null.has_value()) {
return -1;
}
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
new Elasticity(std::move(*elasticity_or_null)));
return 0;
};
plugin.destroy = +[](mjData* d, int instance) {
delete reinterpret_cast<Elasticity*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) {
auto* elasticity = reinterpret_cast<Elasticity*>(d->plugin_data[instance]);
elasticity->Compute(m, d, instance);
};
mjp_registerPlugin(&plugin);
}
} // namespace mujoco::plugin::elasticity
+52
View File
@@ -0,0 +1,52 @@
// Copyright 2022 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.
#ifndef MUJOCO_SRC_PLUGIN_ELASTICITY_CABLE_H_
#define MUJOCO_SRC_PLUGIN_ELASTICITY_CABLE_H_
#include <optional>
#include <vector>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
namespace mujoco::plugin::elasticity {
class Elasticity {
public:
// Creates a new Elasticity instance (allocated with `new`) or
// returns null on failure.
static std::optional<Elasticity> Create(const mjModel* m, mjData* d,
int instance);
Elasticity(Elasticity&&) = default;
~Elasticity() = default;
void Compute(const mjModel* m, mjData* d, int instance);
int i0; // index of first body
int n; // number of bodies in the cable
std::vector<int> prev; // indices of previous bodies (n x 1)
std::vector<int> next; // indices of next bodies (n x 1)
std::vector<mjtNum> stiffness; // stiffness parameters (n x 4)
std::vector<mjtNum> omega0; // reference curvature (n x 3)
private:
Elasticity(const mjModel* m, mjData* d, int instance);
};
} // namespace mujoco::plugin::elasticity
#endif // MUJOCO_SRC_PLUGIN_ELASTICITY_CABLE_H_
+1 -1
View File
@@ -65,7 +65,7 @@ PYBIND11_MODULE(_functions, pymodule) {
DEF_WITH_OMITTED_PY_ARGS(traits::mj_printSchema,
"filename", "buffer", "buffer_sz")(
pymodule, [](bool flg_html, bool flg_pad) {
constexpr int kBufferSize = 30000;
constexpr int kBufferSize = 40000;
auto buffer = std::unique_ptr<char[]>(new char[kBufferSize]);
const int out_length = InterceptMjErrors(::mj_printSchema)(
nullptr, buffer.get(), kBufferSize, flg_html, flg_pad);
+20
View File
@@ -23,6 +23,7 @@
#include "engine/engine_core_constraint.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
@@ -1422,6 +1423,25 @@ void mj_passive(const mjModel* m, mjData* d) {
if (mjcb_passive) {
mjcb_passive(m, d);
}
// plugin
if (m->nplugin) {
const int nslot = mjp_pluginCount();
// iterate over plugins, call compute if type is mjPLUGIN_PASSIVE
for (int i=0; i<m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error_i("invalid plugin slot: %d", slot);
}
if (plugin->type & mjPLUGIN_PASSIVE) {
if (!plugin->compute) {
mju_error_i("`compute` is a null function pointer for plugin at slot %d", slot);
}
plugin->compute(m, d, i, mjPLUGIN_PASSIVE);
}
}
}
}
+11 -7
View File
@@ -875,10 +875,14 @@ static mjData* _makeData(const mjModel* m) {
for (int i = 0; i < m->nplugin; ++i) {
d->plugin[i] = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
if (!plugin->init) {
mju_error_i("`init` is a null function pointer for plugin at slot %d", m->plugin[i]);
if (plugin->init) {
if (plugin->init(m, d, i) < 0) {
mju_free(d->buffer);
mju_free(d->arena);
mju_free(d);
mju_error_i("plugin->init failed for plugin id %d", i);
}
}
plugin->init(m, d, i);
}
return d;
@@ -1142,10 +1146,9 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
for (int i = 0; i < m->nplugin; ++i) {
d->plugin[i] = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
if (!plugin->reset) {
mju_error_i("`reset` is a null function pointer for plugin at slot %d", m->plugin[i]);
if (plugin->reset) {
plugin->reset(m, d, i);
}
plugin->reset(m, d, i);
}
}
@@ -1333,6 +1336,7 @@ const char* mj_validateReferences(const mjModel* m) {
X(body_jntadr, nbody, njnt , m->body_jntnum ) \
X(body_dofadr, nbody, nv , m->body_dofnum ) \
X(body_geomadr, nbody, ngeom , m->body_geomnum ) \
X(body_plugin, nbody, nplugin , 0 ) \
X(jnt_qposadr, njnt, nq , 0 ) \
X(jnt_dofadr, njnt, nv , 0 ) \
X(jnt_bodyid, njnt, nbody , 0 ) \
@@ -1364,7 +1368,7 @@ const char* mj_validateReferences(const mjModel* m) {
X(pair_geom2, npair, ngeom , 0 ) \
X(actuator_plugin, nu, nplugin , 0 ) \
X(sensor_plugin, nsensor, nplugin , 0 ) \
X(plugin_stateadr, nplugin, npluginstate , 0 ) \
X(plugin_stateadr, nplugin, npluginstate , m->plugin_statenum ) \
X(plugin_attradr, nplugin, npluginattr , 0 ) \
X(tendon_adr, ntendon, nwrap , m->tendon_num ) \
X(tendon_matid, ntendon, nmat , 0 ) \
+1 -1
View File
@@ -91,7 +91,7 @@ MJAPI void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl);
// res = vec1 + vec2*scl
MJAPI void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl);
// normalize vector, return length before normalization
// normalize vector, return length before normalization, set to [1, 0, 0] if norm is tiny
MJAPI mjtNum mju_normalize3(mjtNum vec[3]);
// compute vector length (without normalizing)
+373 -36
View File
@@ -60,6 +60,11 @@ mjCComposite::mjCComposite(void) {
flatinertia = 0;
mj_defaultSolRefImp(solrefsmooth, solimpsmooth);
// cable
curve[0] = curve[1] = curve[2] = mjCOMPSHAPE_ZERO;
mjuu_setvec(size, 1, 0, 0);
initial = "ball";
// skin
skin = false;
skintexcoord = false;
@@ -122,6 +127,7 @@ void mjCComposite::SetDefault(void) {
type==mjCOMPTYPE_PARTICLE ||
type==mjCOMPTYPE_ROPE ||
type==mjCOMPTYPE_LOOP ||
type==mjCOMPTYPE_CABLE ||
(type==mjCOMPTYPE_GRID && tmpdim==1)) {
for (i=0; i<mjNCOMPKINDS; i++) {
def[i].geom.group = 0;
@@ -146,6 +152,7 @@ void mjCComposite::SetDefault(void) {
break;
case mjCOMPTYPE_CABLE: // cable
case mjCOMPTYPE_ROPE: // rope
break;
@@ -194,9 +201,10 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
}
// check geom type
if (def[0].geom.type!=mjGEOM_SPHERE &&
def[0].geom.type!=mjGEOM_CAPSULE &&
def[0].geom.type!=mjGEOM_ELLIPSOID) {
if ((def[0].geom.type!=mjGEOM_SPHERE &&
def[0].geom.type!=mjGEOM_CAPSULE &&
def[0].geom.type!=mjGEOM_ELLIPSOID) &&
type!=mjCOMPTYPE_CABLE) {
return comperr(error, "Composite geom type must be sphere, capsule or ellipsoid", error_sz);
}
@@ -213,8 +221,22 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
}
// check spacing
if (spacing<mjMINVAL) {
return comperr(error, "Positive spacing expected in composite", error_sz);
if (mjuu_dot3(size, size)<mjMINVAL && uservert.empty()) {
return comperr(error, "Positive spacing or length expected in composite", error_sz);
}
// check either spacing or length
if (spacing && type==mjCOMPTYPE_CABLE) {
return comperr(error, "Spacing is not supported by cable composite", error_sz);
}
// check either uservert or count but not both
if (!uservert.empty()) {
if (count[0]>1) {
return comperr(error, "Either vertex or count can be specified, not both", error_sz);
}
count[0] = uservert.size()/3;
count[1] = 1;
}
// determine dimensionality, check singleton order
@@ -231,7 +253,7 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
}
// require 3x3 for subgrid
if (skin && skinsubgrid>0) {
if (skin && skinsubgrid>0 && type!=mjCOMPTYPE_CABLE) {
if (count[0]<3 || count[1]<3) {
return comperr(error, "At least 3x3 required for skin subgrid", error_sz);
}
@@ -249,6 +271,9 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
case mjCOMPTYPE_LOOP:
return MakeRope(model, body, error, error_sz);
case mjCOMPTYPE_CABLE:
return MakeCable(model, body, error, error_sz);
case mjCOMPTYPE_CLOTH:
return MakeCloth(model, body, error, error_sz);
@@ -407,9 +432,9 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err
// skin
if (skin) {
if (skinsubgrid>0) {
MakeSkin2Subgrid(model);
MakeSkin2Subgrid(model, skininflate);
} else {
MakeSkin2(model);
MakeSkin2(model, skininflate);
}
}
@@ -418,6 +443,203 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err
bool mjCComposite::MakeCable(mjCModel* model, mjCBody* body, char* error, int error_sz) {
// check dim
if (dim!=1) {
return comperr(error, "Cable must be one-dimensional", error_sz);
}
// check geom type
if (def[0].geom.type!=mjGEOM_CYLINDER &&
def[0].geom.type!=mjGEOM_CAPSULE &&
def[0].geom.type!=mjGEOM_BOX) {
return comperr(error, "Cable geom type must be sphere, capsule or box", error_sz);
}
// add name to model
mjCText* pte = model->AddText();
pte->name = "composite_" + prefix;
pte->data = "rope_" + prefix;
// populate uservert if not specified
if (uservert.empty()) {
for (int ix=0; ix<count[0]; ix++) {
for (int k=0; k<3; k++) {
switch (curve[k]) {
case mjCOMPSHAPE_LINE:
uservert.push_back(ix*size[0]/(count[0]-1));
break;
case mjCOMPSHAPE_COS:
uservert.push_back(size[1]*cos(mjPI*ix*size[2]/(count[0]-1)));
break;
case mjCOMPSHAPE_SIN:
uservert.push_back(size[1]*sin(mjPI*ix*size[2]/(count[0]-1)));
break;
case mjCOMPSHAPE_ZERO:
uservert.push_back(0);
break;
default:
// SHOULD NOT OCCUR
mju_error_i("Invalid composite shape: %d", curve[k]);
break;
}
}
}
}
// create frame
mjtNum normal[3], prev_quat[4];
mjuu_setvec(normal, 0, 1, 0);
mjuu_setvec(prev_quat, 1, 0, 0, 0);
// add one body after the other
for (int ix=0; ix<count[0]-1; ix++) {
body = AddCableBody(model, body, ix, normal, prev_quat);
}
// add skin
if (def[0].geom.type==mjGEOM_BOX) {
if (skinsubgrid>0) {
count[1]+=2;
MakeSkin2Subgrid(model, 2*def[0].geom.size[2]);
count[1]-=2;
} else {
count[1]++;
MakeSkin2(model, 2*def[0].geom.size[2]);
count[1]--;
}
}
return true;
}
mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]) {
char txt_geom[100], txt_site[100], txt_slide[100];
char this_body[100], next_body[100], this_joint[100];
mjtNum dquat[4], this_quat[4];
// set flags
int lastidx = count[0]-2;
bool first = ix==0;
bool last = ix==lastidx;
bool secondlast = ix==lastidx-1;
// compute edge and tangent vectors
mjtNum edge[3], tprev[3], tnext[3];
mjuu_setvec(edge, uservert[3*(ix+1)+0]-uservert[3*ix+0],
uservert[3*(ix+1)+1]-uservert[3*ix+1],
uservert[3*(ix+1)+2]-uservert[3*ix+2]);
if (!first) {
mjuu_setvec(tprev, uservert[3*ix+0]-uservert[3*(ix-1)+0],
uservert[3*ix+1]-uservert[3*(ix-1)+1],
uservert[3*ix+2]-uservert[3*(ix-1)+2]);
mjuu_normvec(tprev, 3);
}
if (!last) {
mjuu_setvec(tnext, uservert[3*(ix+2)+0]-uservert[3*(ix+1)+0],
uservert[3*(ix+2)+1]-uservert[3*(ix+1)+1],
uservert[3*(ix+2)+2]-uservert[3*(ix+1)+2]);
mjuu_normvec(tnext, 3);
}
// update moving frame
mjtNum length = mju_updateFrame(this_quat, normal, edge, tprev, tnext, first);
// create body, joint, and geom names
if (first) {
mju::sprintf_arr(this_body, "%sB_first", prefix.c_str());
mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix+1);
mju::sprintf_arr(this_joint, "%sJ_first", prefix.c_str());
mju::sprintf_arr(txt_site, "%sS_first", prefix.c_str());
} else if (last) {
mju::sprintf_arr(this_body, "%sB_last", prefix.c_str());
mju::sprintf_arr(next_body, "%sB_first", prefix.c_str());
mju::sprintf_arr(this_joint, "%sJ_last", prefix.c_str());
mju::sprintf_arr(txt_site, "%sS_last", prefix.c_str());
} else if (secondlast){
mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
mju::sprintf_arr(next_body, "%sB_last", prefix.c_str());
mju::sprintf_arr(this_joint, "%sJ_%d", prefix.c_str(), ix);
} else {
mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix+1);
mju::sprintf_arr(this_joint, "%sJ_%d", prefix.c_str(), ix);
}
mju::sprintf_arr(txt_geom, "%sG%d", prefix.c_str(), ix);
mju::sprintf_arr(txt_slide, "%sJs%d", prefix.c_str(), ix);
// add body
body = body->AddBody();
body->name = this_body;
if (first) {
mjuu_setvec(body->pos, offset[0]+uservert[3*ix],
offset[1]+uservert[3*ix+1],
offset[2]+uservert[3*ix+2]);
mjuu_copyvec(body->quat, this_quat, 4);
} else {
mjuu_setvec(body->pos, length, 0, 0);
mjtNum negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]};
mjuu_mulquat(dquat, negquat, this_quat);
mjuu_copyvec(body->quat, dquat, 4);
}
// add geom
mjCGeom* geom = body->AddGeom(def);
geom->def = body->def;
geom->name = txt_geom;
if (def[0].geom.type==mjGEOM_CYLINDER ||
def[0].geom.type==mjGEOM_CAPSULE) {
mjuu_zerovec(geom->fromto, 6);
geom->fromto[3] = length;
} else if (def[0].geom.type==mjGEOM_BOX) {
mjuu_zerovec(geom->pos, 3);
geom->pos[0] = length/2;
geom->size[0] = length/2;
}
// add plugin
if (plugin_instance) {
body->is_plugin = true;
body->plugin_name = plugin_name;
body->plugin_instance = plugin_instance;
body->plugin_instance_name = plugin_instance_name;
}
// update orientation
mjuu_copyvec(prev_quat, this_quat, 4);
// add curvature joint
if (!first || strcmp(initial.c_str(), "none")) {
mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_JOINT);
jnt->def = body->def;
jnt->type = (first && strcmp(initial.c_str(), "free")==0) ? mjJNT_FREE : mjJNT_BALL;
jnt->damping = jnt->type==mjJNT_FREE ? 0 : jnt->damping;
jnt->armature = jnt->type==mjJNT_FREE ? 0 : jnt->armature;
jnt->frictionloss = jnt->type==mjJNT_FREE ? 0 : jnt->frictionloss;
jnt->name = this_joint;
}
// exclude contact pair
if (!last) {
mjCBodyPair* exclude = model->AddExclude();
exclude->bodyname1 = this_body;
exclude->bodyname2 = next_body;
}
// add site at the boundary
if (last || first) {
mjCSite* site = body->AddSite(def);
site->def = body->def;
site->name = txt_site;
mjuu_setvec(site->pos, last ? length : 0, 0, 0);
mjuu_setvec(site->quat, 1, 0, 0, 0);
}
return body;
}
// make rope
bool mjCComposite::MakeRope(mjCModel* model, mjCBody* body, char* error, int error_sz) {
// check dim
@@ -709,9 +931,9 @@ bool mjCComposite::MakeCloth(mjCModel* model, mjCBody* body, char* error, int er
// skin
if (skin) {
if (skinsubgrid>0) {
MakeSkin2Subgrid(model);
MakeSkin2Subgrid(model, skininflate);
} else {
MakeSkin2(model);
MakeSkin2(model, skininflate);
}
}
@@ -1043,7 +1265,7 @@ void mjCComposite::MakeShear(mjCModel* model) {
// add skin to 2D
void mjCComposite::MakeSkin2(mjCModel* model) {
void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
char txt[100];
int N = count[0]*count[1];
@@ -1053,7 +1275,7 @@ void mjCComposite::MakeSkin2(mjCModel* model) {
skin->name = txt;
skin->material = skinmaterial;
mjuu_copyvec(skin->rgba, skinrgba, 4);
skin->inflate = skininflate;
skin->inflate = inflate;
skin->group = skingroup;
// populate mesh: two sides
@@ -1129,6 +1351,21 @@ void mjCComposite::MakeSkin2(mjCModel* model) {
skin->face.push_back(N + iy+1 + (count[0]-1)*count[1]);
}
// couple with bones
if (type==mjCOMPTYPE_CLOTH || type==mjCOMPTYPE_GRID) {
MakeClothBones(model, skin);
} else if (type==mjCOMPTYPE_CABLE) {
MakeCableBones(model, skin);
}
}
// add bones in 2D
void mjCComposite::MakeClothBones(mjCModel* model, mjCSkin* skin) {
char txt[100];
int N = count[0]*count[1];
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
@@ -1159,6 +1396,124 @@ void mjCComposite::MakeSkin2(mjCModel* model) {
}
void mjCComposite::MakeClothBonesSubgrid(mjCModel* model, mjCSkin* skin) {
char txt[100];
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
// body name
mju::sprintf_arr(txt, "%sB%d_%d", prefix.c_str(), ix, iy);
// bind pose
skin->bodyname.push_back(txt);
skin->bindpos.push_back(ix*spacing);
skin->bindpos.push_back(iy*spacing);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
// empty vertid and vertweight
vector<int> vertid;
vector<float> vertweight;
skin->vertid.push_back(vertid);
skin->vertweight.push_back(vertweight);
}
}
}
// add bones to 1D
void mjCComposite::MakeCableBones(mjCModel* model, mjCSkin* skin) {
char this_body[100];
int N = count[0]*count[1];
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
// body name
if (ix==0) {
mju::sprintf_arr(this_body, "%sB_first", prefix.c_str());
} else if (ix>=count[0]-2) {
mju::sprintf_arr(this_body, "%sB_last", prefix.c_str());
} else {
mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
}
// bind pose
if (iy==0) {
skin->bodyname.push_back(this_body);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(-def[0].geom.size[1]);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1); skin->bindquat.push_back(0);
skin->bindquat.push_back(0); skin->bindquat.push_back(0);
} else {
skin->bodyname.push_back(this_body);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(def[0].geom.size[1]);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1); skin->bindquat.push_back(0);
skin->bindquat.push_back(0); skin->bindquat.push_back(0);
}
// create vertid and vertweight
skin->vertid.push_back({ix*count[1]+iy, N + ix*count[1]+iy});
skin->vertweight.push_back({1, 1});
}
}
}
void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjCSkin* skin) {
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
char txt[100];
// body name
if (ix==0) {
mju::sprintf_arr(txt, "%sB_first", prefix.c_str());
} else if (ix>=count[0]-2) {
mju::sprintf_arr(txt, "%sB_last", prefix.c_str());
} else {
mju::sprintf_arr(txt, "%sB_%d", prefix.c_str(), ix);
}
// bind pose
if (iy==0) {
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(-def[0].geom.size[1]);
skin->bindpos.push_back(0);
} else if (iy==2) {
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(def[0].geom.size[1]);
skin->bindpos.push_back(0);
} else {
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(0);
skin->bindpos.push_back(0);
}
skin->bodyname.push_back(txt);
skin->bindquat.push_back(1);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
// empty vertid and vertweight
skin->vertid.push_back({});
skin->vertweight.push_back({});
}
}
}
//------------------------------------- subgrid matrices
// C = W * [f; f_x; f_y; f_xy]
@@ -1390,7 +1745,7 @@ static const mjtNum subD22[] = {
// add skin to 2D, with subgrid
void mjCComposite::MakeSkin2Subgrid(mjCModel* model) {
void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
// assemble pointers to Dxx matrices
const mjtNum* Dp[3][3] = {
{subD00, subD01, subD02},
@@ -1474,7 +1829,7 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model) {
skin->name = txt;
skin->material = skinmaterial;
mjuu_copyvec(skin->rgba, skinrgba, 4);
skin->inflate = skininflate;
skin->inflate = inflate;
skin->group = skingroup;
// populate mesh: two sides
@@ -1554,28 +1909,10 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model) {
skin->face.push_back(NN + iy+1 + (C0-1)*C1);
}
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
// body name
mju::sprintf_arr(txt, "%sB%d_%d", prefix.c_str(), ix, iy);
// bind pose
skin->bodyname.push_back(txt);
skin->bindpos.push_back(ix*spacing);
skin->bindpos.push_back(iy*spacing);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
// empty vertid and vertweight
vector<int> vertid;
vector<float> vertweight;
skin->vertid.push_back(vertid);
skin->vertweight.push_back(vertweight);
}
if (type==mjCOMPTYPE_CLOTH || type==mjCOMPTYPE_GRID) {
MakeClothBonesSubgrid(model, skin);
} else if (type==mjCOMPTYPE_CABLE) {
MakeCableBonesSubgrid(model, skin);
}
// bind vertices to bones: one big square at a time
+36 -5
View File
@@ -26,6 +26,7 @@
typedef enum _mjtCompType {
mjCOMPTYPE_PARTICLE = 0,
mjCOMPTYPE_GRID,
mjCOMPTYPE_CABLE,
mjCOMPTYPE_ROPE,
mjCOMPTYPE_LOOP,
mjCOMPTYPE_CLOTH,
@@ -48,6 +49,16 @@ typedef enum _mjtCompKind {
} mjtCompKind;
typedef enum _mjtCompShape {
mjCOMPSHAPE_LINE = 0,
mjCOMPSHAPE_COS,
mjCOMPSHAPE_SIN,
mjCOMPSHAPE_ZERO,
mjNCOMPSHAPES
} mjtCompShape;
class mjCComposite {
public:
mjCComposite(void);
@@ -60,19 +71,22 @@ class mjCComposite {
bool MakeParticle(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeGrid(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeRope(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeCable(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeCloth(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeBox(mjCModel* model, mjCBody* body, char* error, int error_sz);
void MakeShear(mjCModel* model);
void MakeSkin2(mjCModel* model);
void MakeSkin2Subgrid(mjCModel* model);
void MakeSkin2(mjCModel* model, mjtNum inflate);
void MakeSkin2Subgrid(mjCModel* model, mjtNum inflate);
void MakeClothBones(mjCModel* model, mjCSkin* skin);
void MakeClothBonesSubgrid(mjCModel* model, mjCSkin* skin);
void MakeCableBones(mjCModel* model, mjCSkin* skin);
void MakeCableBonesSubgrid(mjCModel* model, mjCSkin* skin);
void MakeSkin3(mjCModel* model);
void MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side, int& vcnt, const char* format);
void MakeSkin3Smooth(mjCSkin* skin, int c0, int c1, int side,
const std::map<std::string, int>& vmap, const char* format);
mjCBody* AddClothBody(mjCModel* model, mjCBody* body, int ix, int iy, int ix1, int iy1);
mjCBody* AddRopeBody(mjCModel* model, mjCBody* body, int ix, int ix1);
void BoxProject(double* pos);
@@ -87,6 +101,18 @@ class mjCComposite {
mjtNum solrefsmooth[mjNREF]; // solref for smoothing equality
mjtNum solimpsmooth[mjNIMP]; // solimp for smoothing equality
// currently used only for cable
std::string initial; // root boundary type
std::vector<float> uservert; // user-specified vertex positions
mjtNum size[3]; // rope size (meaning depends on the shape)
mjtCompShape curve[3]; // geometric shape
// plugin support
bool is_plugin;
std::string plugin_name;
std::string plugin_instance_name;
mjCPlugin* plugin_instance;
// skin
bool skin; // generate skin
bool skintexcoord; // generate texture coordinates
@@ -102,6 +128,11 @@ class mjCComposite {
// computed internally
int dim; // dimensionality
private:
mjCBody* AddRopeBody(mjCModel* model, mjCBody* body, int ix, int ix1);
mjCBody* AddClothBody(mjCModel* model, mjCBody* body, int ix, int iy, int ix1, int iy1);
mjCBody* AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]);
};
#endif // MUJOCO_SRC_USER_USER_COMPOSITE_H_
+9 -1
View File
@@ -2601,7 +2601,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
}
// set sensor_plugin to the plugin instance ID
for (int i = 0; i < nbody; ++i) {
if (bodies[i]->is_plugin) {
m->body_plugin[i] = bodies[i]->plugin_instance->id;
} else {
m->body_plugin[i] = -1;
}
}
std::vector<std::vector<int>> plugin_to_sensors(nplugin);
for (int i = 0; i < nsensor; ++i) {
if (sensors[i]->type == mjSENS_PLUGIN) {
@@ -2623,6 +2630,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
int nstate = plugin->nstate(m, i);
m->plugin_stateadr[i] = stateadr;
m->plugin_statenum[i] = nstate;
stateadr += nstate;
if (plugin->type & mjPLUGIN_SENSOR) {
for (int sensor_id : plugin_to_sensors[i]) {
+21
View File
@@ -328,6 +328,12 @@ mjCBody::mjCBody(mjCModel* _model) {
lastdof = -1;
userdata.clear();
// plugin variables
is_plugin = false;
plugin_instance = nullptr;
plugin_name = "";
plugin_instance_name = "";
// clear object lists
bodies.clear();
geoms.clear();
@@ -799,6 +805,21 @@ void mjCBody::Compile(void) {
// compile all lights
for (i=0; i<lights.size(); i++) lights[i]->Compile();
// plugin
if (is_plugin) {
if (plugin_name.empty() && plugin_instance_name.empty()) {
throw mjCError(
this, "neither 'plugin' nor 'instance' is specified for body '%s', (id = %d)",
name.c_str(), id);
}
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
if (!(plugin->type & mjPLUGIN_PASSIVE)) {
throw mjCError(this, "plugin '%s' does not support passive forces", plugin->name);
}
}
}
+8 -1
View File
@@ -118,6 +118,7 @@ class mjCAlternative {
//------------------------- class mjCBase ----------------------------------------------------------
// Generic functionality for all derived classes
class mjCPlugin;
class mjCBase {
friend class mjCDef;
@@ -127,10 +128,16 @@ class mjCBase {
int id; // object id
int xmlpos[2]; // row and column in xml file
mjCDef* def; // defaults class used to init this object
mjCModel* model; // pointer to model that created object
// plugin support
bool is_plugin;
std::string plugin_name;
std::string plugin_instance_name;
mjCPlugin* plugin_instance;
protected:
mjCBase(); // constructor
mjCModel* model; // pointer to model that created object
};
+50
View File
@@ -23,6 +23,7 @@
#include <mujoco/mjtnum.h>
#include "engine/engine_macro.h"
#include "engine/engine_util_spatial.h"
using std::isnan;
using std::string;
@@ -461,6 +462,55 @@ void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double
}
// update moving frame along a curve or initialize it, returns edge length
// inputs:
// normal - normal vector computed by a previous call to the function
// edge - edge vector (non-unit tangent vector)
// tprv - unit tangent vector of previous body
// tnxt - unit tangent vector of next body
// first - 1 if the frame requires initialization
// outputs:
// quat - frame orientation
// normal - unit normal vector
mjtNum mju_updateFrame(mjtNum quat[4], mjtNum normal[3], const mjtNum edge[3],
const mjtNum tprv[3], const mjtNum tnxt[3], int first) {
mjtNum tangent[3], binormal[3];
// normalize tangent
mjuu_copyvec(tangent, edge, 3);
mjuu_normvec(tangent, 3);
// compute moving frame
if (first) {
// use the first vertex binormal for the first edge
mjuu_crossvec(binormal, tangent, tnxt);
mjuu_normvec(binormal, 3);
// compute edge normal given tangent and binormal
mjuu_crossvec(normal, binormal, tangent);
mjuu_normvec(normal, 3);
} else {
mjtNum darboux[4];
// rotate edge normal about the vertex binormal
mjuu_crossvec(binormal, tprv, tangent);
mjtNum angle = atan2(mjuu_normvec(binormal, 3), mjuu_dot3(tprv, tangent));
mju_axisAngle2Quat(darboux, binormal, angle);
mju_rotVecQuat(normal, normal, darboux);
mjuu_normvec(normal, 3);
// compute edge binormal given tangent and normal
mjuu_crossvec(binormal, tangent, normal);
mjuu_normvec(binormal, 3);
}
// global orientation of the frame
mjuu_frame2quat(quat, tangent, normal, binormal);
// return edge length
return sqrt(mjuu_dot3(edge, edge));
}
// strip directory from filename
string mjuu_strippath(string filename) {
// find last pathsymbol
+13
View File
@@ -124,6 +124,19 @@ void mjuu_offcenter(double* res, const double mass, const double* vec);
// compute viscosity coefficients from mass and inertia
void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double scl=1);
// update moving frame along a discrete curve or initialize it, returns edge length
// inputs:
// normal - normal vector computed by a previous call to the function
// edge - edge vector (non-unit tangent vector)
// tprv - unit tangent vector of previous body
// tnxt - unit tangent vector of next body
// first - 1 if the frame requires initialization
// outputs:
// quat - frame orientation
// normal - unit normal vector
double mju_updateFrame(double quat[4], double normal[3], const double edge[3],
const double tprv[3], const double tnxt[3], int first);
// strip path from filename
std::string mjuu_strippath(std::string filename);
+72 -5
View File
@@ -79,7 +79,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 183;
static const int nMJCF = 191;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -230,6 +230,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"body", "R", "10", "name", "childclass", "pos", "quat", "mocap",
"axisangle", "xyaxes", "zaxis", "euler", "user"},
{"<"},
{"plugin", "*", "3", "name", "plugin", "instance"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{"inertial", "?", "9", "pos", "quat", "mass", "diaginertia",
"axisangle", "xyaxes", "zaxis", "euler", "fullinertia"},
{"joint", "*", "21", "name", "class", "type", "group", "pos", "axis",
@@ -251,9 +255,14 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"light", "*", "15", "name", "class", "directional", "castshadow", "active",
"pos", "dir", "attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular",
"mode", "target"},
{"composite", "*", "8", "prefix", "type", "count", "spacing", "offset",
"flatinertia", "solrefsmooth", "solimpsmooth"},
{"composite", "*", "13", "prefix", "type", "count", "spacing", "offset",
"flatinertia", "solrefsmooth", "solimpsmooth", "vertex",
"flat", "initial", "curve", "size"},
{"<"},
{"plugin", "*", "3", "name", "plugin", "instance"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{"joint", "*", "15", "kind", "group", "stiffness", "damping", "armature",
"solreffix", "solimpfix",
"limited", "range", "margin", "solreflimit", "solimplimit",
@@ -652,6 +661,7 @@ const mjMap comp_map[mjNCOMPTYPES] = {
{"grid", mjCOMPTYPE_GRID},
{"rope", mjCOMPTYPE_ROPE},
{"loop", mjCOMPTYPE_LOOP},
{"cable", mjCOMPTYPE_CABLE},
{"cloth", mjCOMPTYPE_CLOTH},
{"box", mjCOMPTYPE_BOX},
{"cylinder", mjCOMPTYPE_CYLINDER},
@@ -667,6 +677,15 @@ const mjMap jkind_map[3] = {
};
// composite rope shape
const mjMap shape_map[mjNCOMPSHAPES] = {
{"s", mjCOMPSHAPE_LINE},
{"cos(s)", mjCOMPSHAPE_COS},
{"sin(s)", mjCOMPSHAPE_SIN},
{"0", mjCOMPSHAPE_ZERO}
};
// composite tendon kind
const mjMap tkind_map[2] = {
{"main", mjCOMPKIND_TENDON},
@@ -1859,11 +1878,46 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
if (MapValue(elem, "type", &n, comp_map, mjNCOMPTYPES, true)) {
comp.type = (mjtCompType)n;
}
ReadAttr(elem, "count", 3, comp.count, text, true, false);
ReadAttr(elem, "spacing", 1, &comp.spacing, text, true);
ReadAttr(elem, "count", 3, comp.count, text, false, false);
ReadAttr(elem, "spacing", 1, &comp.spacing, text, false);
ReadAttr(elem, "offset", 3, comp.offset, text);
ReadAttr(elem, "flatinertia", 1, &comp.flatinertia, text);
// plugin
XMLElement* eplugin = elem->FirstChildElement("plugin");
if (eplugin) {
ReadAttrTxt(eplugin, "plugin", comp.plugin_name);
ReadAttrTxt(eplugin, "instance", comp.plugin_instance_name);
if (comp.plugin_instance_name.empty()) {
comp.plugin_instance = model->AddPlugin();
comp.plugin_instance->name = "composite"+comp.prefix;
comp.plugin_instance_name = comp.plugin_instance->name;
} else {
model->hasImplicitPluginElem = true;
}
ReadPluginConfigs(eplugin, comp.plugin_instance);
}
// cable
std::string curves;
ReadAttrTxt(elem, "curve", curves);
ReadAttrTxt(elem, "initial", comp.initial);
ReadAttr(elem, "size", 3, comp.size, text, false, false);
if (ReadAttrTxt(elem, "vertex", text)){
String2Vector(text, comp.uservert);
}
// process curve string
std::istringstream iss(curves);
int i = 0;
while (iss) {
iss >> text;
comp.curve[i++] = (mjtCompShape)FindKey(shape_map, mjNCOMPSHAPES, text);
if (iss.eof()){
break;
}
};
// skin
XMLElement* eskin = elem->FirstChildElement("skin");
if (eskin) {
@@ -2619,6 +2673,19 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
OneLight(elem, plight);
}
// plugin sub-element
else if (name == "plugin") {
pbody->is_plugin = true;
ReadAttrTxt(elem, "plugin", pbody->plugin_name);
ReadAttrTxt(elem, "instance", pbody->plugin_instance_name);
if (pbody->plugin_instance_name.empty()) {
pbody->plugin_instance = model->AddPlugin();
} else {
model->hasImplicitPluginElem = true;
}
ReadPluginConfigs(elem, pbody->plugin_instance);
}
// composite sub-element
else if (name=="composite") {
// create composite and parse
+22
View File
@@ -1373,6 +1373,28 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
OneLight(InsertEnd(elem, "light"), body->lights[i], body->lights[i]->def);
}
// write plugin
if (body->is_plugin) {
XMLElement *child = InsertEnd(elem, "plugin");
if (!body->plugin_instance_name.empty()) {
WriteAttrTxt(child, "instance", body->plugin_instance_name);
} else {
WriteAttrTxt(child, "plugin", body->plugin_name);
const mjpPlugin* plugin = mjp_getPluginAtSlot(
body->plugin_instance->plugin_slot);
const char* c = &body->plugin_instance->flattened_attributes[0];
for (int i = 0; i < plugin->nattribute; ++i) {
std::string value(c);
if (!value.empty()) {
WriteAttrTxt(child, std::string("plugin:") + plugin->attributes[i],
value);
c += value.size();
}
++c;
}
}
}
// write child bodies recursively
for (i=0; i<body->bodies.size(); i++) {
Body(InsertEnd(elem, "body"), body->bodies[i]);
+62 -8
View File
@@ -35,6 +35,8 @@ using ::testing::HasSubstr;
using ::testing::NotNull;
constexpr int kNumFakePlugins = 30;
constexpr int kNumTestPlugins = 3;
const int kNumTruePlugins = mjp_pluginCount();
class BaseTestPlugin {
public:
@@ -186,6 +188,14 @@ class TestActuator : public BaseTestPlugin {
}
};
class TestPassive {
public:
TestPassive(const mjModel* m, mjData* d, int instance) {}
void Reset() {}
void Compute() {}
void Advance() {}
};
int RegisterSensorPlugin() {
mjpPlugin plugin;
mjp_defaultPlugin(&plugin);
@@ -206,6 +216,7 @@ int RegisterSensorPlugin() {
auto* sensor = new TestSensor(m, d, instance);
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(sensor);
TestSensor::InitCount()++;
return 0;
};
plugin.destroy = +[](mjData* d, int instance) {
delete reinterpret_cast<TestSensor*>(d->plugin_data[instance]);
@@ -247,6 +258,7 @@ int RegisterActuatorPlugin() {
auto* actuator = new TestActuator(m, d, instance);
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(actuator);
TestActuator::InitCount()++;
return 0;
};
plugin.destroy = +[](mjData* d, int instance) {
delete reinterpret_cast<TestActuator*>(d->plugin_data[instance]);
@@ -270,6 +282,42 @@ int RegisterActuatorPlugin() {
return mjp_registerPlugin(&plugin);
}
int RegisterPassivePlugin() {
mjpPlugin plugin;
mjp_defaultPlugin(&plugin);
plugin.name = "mujoco.test.passive";
const char* attributes[] = {"attribute"};
plugin.nattribute = sizeof(attributes) / sizeof(*attributes);
plugin.attributes = attributes;
plugin.type |= mjPLUGIN_PASSIVE;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
auto* passive = new TestPassive(m, d, instance);
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(passive);
return 0;
};
plugin.destroy = +[](mjData* d, int instance) {
delete reinterpret_cast<TestPassive*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.reset = +[](const mjModel* m, mjData* d, int instance) {
auto passive = reinterpret_cast<TestPassive*>(d->plugin_data[instance]);
passive->Reset();
};
plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) {
auto passive = reinterpret_cast<TestPassive*>(d->plugin_data[instance]);
passive->Compute();
};
return mjp_registerPlugin(&plugin);
}
class PluginTest : public MujocoTest {
public:
// register all plugins
@@ -285,6 +333,7 @@ class PluginTest : public MujocoTest {
}
RegisterActuatorPlugin();
RegisterPassivePlugin();
}
};
@@ -303,9 +352,13 @@ constexpr char xml[] = R"(
<config key="multiplier" value="0.125"/>
</instance>
</required>
<required plugin="mujoco.test.passive"/>
</extension>
<worldbody>
<body>
<plugin plugin="mujoco.test.passive">
<config key="attribute" value="0"/>
</plugin>
<geom type="capsule" size="0.1" fromto="-1 0 0 -1 0 -1"/>
<joint name="h1" type="hinge"/>
</body>
@@ -335,7 +388,7 @@ constexpr char xml[] = R"(
)";
TEST_F(PluginTest, MultiplePluginTableBlocks) {
EXPECT_EQ(mjp_pluginCount(), kNumFakePlugins + 2);
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
const mjpPlugin* last_plugin = nullptr;
int table_count = 0;
@@ -343,7 +396,7 @@ TEST_F(PluginTest, MultiplePluginTableBlocks) {
int slot;
std::string name = absl::StrFormat("mujoco.test.fake%u", i);
const mjpPlugin* plugin = mjp_getPlugin(name.c_str(), &slot);
EXPECT_EQ(slot, i);
EXPECT_EQ(slot, kNumTruePlugins+i);
EXPECT_THAT(plugin, NotNull());
if (plugin - last_plugin != 1) {
++table_count;
@@ -359,9 +412,10 @@ TEST_F(PluginTest, MultiplePluginTableBlocks) {
}
TEST_F(PluginTest, RegisterIdenticalPlugin) {
EXPECT_EQ(RegisterSensorPlugin(), 0);
EXPECT_EQ(RegisterActuatorPlugin(), kNumFakePlugins + 1);
EXPECT_EQ(mjp_pluginCount(), kNumFakePlugins + 2);
EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins);
EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1);
EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2);
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
}
TEST_F(PluginTest, SaveXml) {
@@ -431,7 +485,7 @@ TEST_F(PluginTest, SensorPlugin) {
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
EXPECT_EQ(m->nplugin, 5);
EXPECT_EQ(m->nplugin, 6);
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "twosensors"), 0);
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "threesensors"), 1);
@@ -450,7 +504,7 @@ TEST_F(PluginTest, SensorPlugin) {
testing::ElementsAreArray<int>({3*(i+1), 6*j, 3*j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[3]>(d->plugin_state +
m->plugin_stateadr[4]),
testing::ElementsAreArray<int>({5*(i+1), 10*j, 5*j}));
testing::ElementsAreArray<int>({4*(i+1), 8*j, 4*j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[18]>(d->sensordata),
testing::ElementsAreArray<int>({ i+1, 2*j, j,
5*(i+1), 10*j, 5*j,
@@ -489,7 +543,7 @@ TEST_F(PluginTest, ActuatorPlugin) {
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
EXPECT_EQ(m->nplugin, 5);
EXPECT_EQ(m->nplugin, 6);
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "actuator2"), 2);
mjData* d = mj_makeData(m);
+17
View File
@@ -0,0 +1,17 @@
# Copyright 2022 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
#
# https://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_test(cable_test)
target_link_libraries(cable_test fixture gmock cable)
+160
View File
@@ -0,0 +1,160 @@
// Copyright 2022 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.
// Tests for plugin-related functionalities.
#include <array>
#include <cstdint>
#include <cstring>
#include <sstream>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
namespace mujoco {
namespace {
using PluginTest = MujocoTest;
// -------------------------------- cable -----------------------------------
TEST_F(PluginTest, CantileverIntoCircle) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<extension>
<required plugin="mujoco.elasticity.cable"/>
</extension>
<worldbody>
<geom type="plane" size="0 0 1" quat="1 0 0 0"/>
<site name="reference" pos="0 0 0"/>
<composite type="cable" curve="s" count="41 1 1" size="1" offset="0 0 1" initial="none">
<plugin plugin="mujoco.elasticity.cable">
<config key="twist" value="1e6"/>
<config key="bend" value="1e9"/>
</plugin>
<joint kind="main" damping="2"/>
<geom type="capsule" size=".005" density="1"/>
</composite>
</worldbody>
<contact>
<exclude body1="B_first" body2="B_last"/>
</contact>
<sensor>
<framepos objtype="site" objname="S_last"/>
</sensor>
<actuator>
<motor site="S_last" gear="0 0 0 0 1 0" ctrllimited="true" ctrlrange="0 4"/>
</actuator>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
// see Oliver Weeger, Sai-Kit Yeung, Martin L. Dunn, "Isogeometric collocation methods for Cosserat rods and rod
// structures", section 7.1 (DOI: j.cma.2016.05.009), the torque for achieving an angle phi is phi * E * Iy.
mjtNum Iy = mjPI * pow(0.005, 4) / 4;
mjtNum torque = 2 * mjPI * 1e9 * Iy;
for (int i=0; i < 1300; i++) {
if (i < 300) {
d->ctrl[0] += torque / 300;
}
mj_step(m, d);
}
EXPECT_NEAR(d->sensordata[0], 0, std::numeric_limits<float>::epsilon());
EXPECT_NEAR(d->sensordata[1], 0, std::numeric_limits<float>::epsilon());
EXPECT_NEAR(d->sensordata[2], 1, std::numeric_limits<float>::epsilon());
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PluginTest, InvalidTxtAttribute) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<extension>
<required plugin="mujoco.elasticity.cable">
<instance name="invalid">
<config key="twist" value="one"/>
<config key="bend" value="1"/>
</instance>
</required>
</extension>
<worldbody>
<geom type="plane" size="0 0 1" quat="1 0 0 0"/>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::IsNull());
}
TEST_F(PluginTest, InvalidMixedAttribute) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<extension>
<required plugin="mujoco.elasticity.cable">
<instance name="invalid">
<config key="twist" value="1"/>
<config key="bend" value="1 is not a number"/>
</instance>
</required>
</extension>
<worldbody>
<geom type="plane" size="0 0 1" quat="1 0 0 0"/>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::IsNull());
}
TEST_F(PluginTest, ValidAttributes) {
static constexpr char cantilever_xml[] = R"(
<mujoco>
<extension>
<required plugin="mujoco.elasticity.cable">
<instance name="invalid">
<config key="twist" value="0.0"/>
<config key="bend" value=" 0 "/>
</instance>
</required>
</extension>
<worldbody>
<geom type="plane" size="0 0 1" quat="1 0 0 0"/>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
+3
View File
@@ -54,6 +54,9 @@ for model_dir in ${MODEL_DIRS[@]}; do
echo "Skipping $model" >&2
continue
fi
if grep -q "plugin" $model; then
continue
fi
test_model "$model"
done
done
+2 -1
View File
@@ -848,7 +848,8 @@ TEST_F(XMLWriterTest, WriteReadCompare) {
std::string xml = p.path().string();
// if file is meant to fail, skip it
if (absl::StrContains(p.path().string(), "malformed_")) {
if (absl::StrContains(p.path().string(), "malformed_") ||
absl::StrContains(p.path().string(), "plugin")) {
continue;
}
+3
View File
@@ -363,6 +363,7 @@ public enum mjtLRMode : int{
public enum mjtPluginTypeBit : int{
mjPLUGIN_ACTUATOR = 1,
mjPLUGIN_SENSOR = 2,
mjPLUGIN_PASSIVE = 4,
}
public enum mjtGridPos : int{
mjGRID_TOPLEFT = 0,
@@ -1945,6 +1946,7 @@ public unsafe struct mjModel_ {
public double* body_inertia;
public double* body_invweight0;
public double* body_user;
public int* body_plugin;
public int* jnt_type;
public int* jnt_qposadr;
public int* jnt_dofadr;
@@ -2159,6 +2161,7 @@ public unsafe struct mjModel_ {
public int* sensor_plugin;
public int* plugin;
public int* plugin_stateadr;
public int* plugin_statenum;
public char* plugin_attr;
public int* plugin_attradr;
public int* numeric_adr;