- 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:
committed by
Copybara-Service
parent
794ef0b771
commit
e250ff0d5a
@@ -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)
|
||||
|
||||
@@ -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
@@ -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:
|
||||
|
||||
|
||||
@@ -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
|
||||
^^^^^^^^^^^^^^^
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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 ) \
|
||||
|
||||
@@ -415,6 +415,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
|
||||
values=dict([
|
||||
('mjPLUGIN_ACTUATOR', 1),
|
||||
('mjPLUGIN_SENSOR', 2),
|
||||
('mjPLUGIN_PASSIVE', 4),
|
||||
]),
|
||||
)),
|
||||
('mjtGridPos',
|
||||
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
@@ -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_
|
||||
@@ -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);
|
||||
|
||||
@@ -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
@@ -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 ) \
|
||||
|
||||
@@ -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
@@ -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
|
||||
|
||||
@@ -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_
|
||||
|
||||
@@ -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]) {
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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]);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user