From e250ff0d5adf24efd8d83617f1d704f640941667 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 10 Oct 2022 02:45:59 -0700 Subject: [PATCH] - 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 --- CMakeLists.txt | 1 + doc/APIreference.rst | 18 + doc/XMLreference.rst | 18 +- doc/changelog.rst | 18 + include/mujoco/mjmodel.h | 6 +- include/mujoco/mjplugin.h | 7 +- include/mujoco/mjxmacro.h | 2 + introspect/enums.py | 1 + model/plugin/belt.xml | 76 +++++ model/plugin/cable.xml | 56 ++++ model/plugin/coil.xml | 104 ++++++ model/plugin/scene.xml | 41 +++ plugin/elasticity/CMakeLists.txt | 29 ++ plugin/elasticity/cable.cc | 257 ++++++++++++++ plugin/elasticity/cable.h | 52 +++ python/mujoco/functions.cc | 2 +- src/engine/engine_core_smooth.c | 20 ++ src/engine/engine_io.c | 18 +- src/engine/engine_util_blas.h | 2 +- src/user/user_composite.cc | 409 +++++++++++++++++++++-- src/user/user_composite.h | 41 ++- src/user/user_model.cc | 10 +- src/user/user_objects.cc | 21 ++ src/user/user_objects.h | 9 +- src/user/user_util.cc | 50 +++ src/user/user_util.h | 13 + src/xml/xml_native_reader.cc | 77 ++++- src/xml/xml_native_writer.cc | 22 ++ test/engine/engine_plugin_test.cc | 70 +++- test/plugin/elasticity/CMakeLists.txt | 17 + test/plugin/elasticity/cable_test.cc | 160 +++++++++ test/sample/testspeed_test.sh | 3 + test/xml/xml_native_writer_test.cc | 3 +- unity/Runtime/Bindings/MujocoBindings.cs | 3 + 34 files changed, 1564 insertions(+), 72 deletions(-) create mode 100644 model/plugin/belt.xml create mode 100644 model/plugin/cable.xml create mode 100644 model/plugin/coil.xml create mode 100644 model/plugin/scene.xml create mode 100644 plugin/elasticity/CMakeLists.txt create mode 100644 plugin/elasticity/cable.cc create mode 100644 plugin/elasticity/cable.h create mode 100644 test/plugin/elasticity/CMakeLists.txt create mode 100644 test/plugin/elasticity/cable_test.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 50b920cd..b98d90b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/doc/APIreference.rst b/doc/APIreference.rst index 0dac539f..9acdda32 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -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; diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 98807f9e..da1c7a01 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -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: diff --git a/doc/changelog.rst b/doc/changelog.rst index 370e4748..c2315f74 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -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 `_ example + showing the formation of plectoneme. + * New `coil.xml `_ example + showing a curved equilibrium configuration. + * New `belt.xml `_ example + showing interaction between twist and anisotropy. + * Added test using cantilever exact solution. + Python bindings ^^^^^^^^^^^^^^^ diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 490929c9..6539f315 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -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) diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index b5c68383..1ce1b5e5 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -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); diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index d6600773..9faebdb0 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -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 ) \ diff --git a/introspect/enums.py b/introspect/enums.py index afc479ae..f9152bf4 100755 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -415,6 +415,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ values=dict([ ('mjPLUGIN_ACTUATOR', 1), ('mjPLUGIN_SENSOR', 2), + ('mjPLUGIN_PASSIVE', 4), ]), )), ('mjtGridPos', diff --git a/model/plugin/belt.xml b/model/plugin/belt.xml new file mode 100644 index 00000000..050785ca --- /dev/null +++ b/model/plugin/belt.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/model/plugin/cable.xml b/model/plugin/cable.xml new file mode 100644 index 00000000..bcda386e --- /dev/null +++ b/model/plugin/cable.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/model/plugin/coil.xml b/model/plugin/coil.xml new file mode 100644 index 00000000..8686941c --- /dev/null +++ b/model/plugin/coil.xml @@ -0,0 +1,104 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/model/plugin/scene.xml b/model/plugin/scene.xml new file mode 100644 index 00000000..0e4cb347 --- /dev/null +++ b/model/plugin/scene.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/plugin/elasticity/CMakeLists.txt b/plugin/elasticity/CMakeLists.txt new file mode 100644 index 00000000..43de259e --- /dev/null +++ b/plugin/elasticity/CMakeLists.txt @@ -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) + diff --git a/plugin/elasticity/cable.cc b/plugin/elasticity/cable.cc new file mode 100644 index 00000000..25e12806 --- /dev/null +++ b/plugin/elasticity/cable.cc @@ -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 +#include +#include +#include + +#include +#include +#include +#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::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( + new Elasticity(std::move(*elasticity_or_null))); +return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) { + auto* elasticity = reinterpret_cast(d->plugin_data[instance]); + elasticity->Compute(m, d, instance); + }; + + mjp_registerPlugin(&plugin); +} + +} // namespace mujoco::plugin::elasticity diff --git a/plugin/elasticity/cable.h b/plugin/elasticity/cable.h new file mode 100644 index 00000000..b7e001f0 --- /dev/null +++ b/plugin/elasticity/cable.h @@ -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 +#include + +#include +#include +#include + + +namespace mujoco::plugin::elasticity { + +class Elasticity { + public: + // Creates a new Elasticity instance (allocated with `new`) or + // returns null on failure. + static std::optional 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 prev; // indices of previous bodies (n x 1) + std::vector next; // indices of next bodies (n x 1) + std::vector stiffness; // stiffness parameters (n x 4) + std::vector 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_ diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index e0445b12..2e718316 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -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(new char[kBufferSize]); const int out_length = InterceptMjErrors(::mj_printSchema)( nullptr, buffer.get(), kBufferSize, flg_html, flg_pad); diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index bfcbc460..1cb6b7e7 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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; inplugin; 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); + } + } + } } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index bfa34dbd..6d65f2e6 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -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 ) \ diff --git a/src/engine/engine_util_blas.h b/src/engine/engine_util_blas.h index 057d6ceb..55af33a8 100644 --- a/src/engine/engine_util_blas.h +++ b/src/engine/engine_util_blas.h @@ -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) diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 1648ac1c..f24b9a40 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -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; i1) { + 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; ix0) { + 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; ixbodyname.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 vertid; + vector 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]-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]-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; ixbodyname.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 vertid; - vector 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 diff --git a/src/user/user_composite.h b/src/user/user_composite.h index 36fcc5ad..3a5cc803 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -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& 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 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_ diff --git a/src/user/user_model.cc b/src/user/user_model.cc index ed18a43e..69a11d02 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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> 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]) { diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 5e0b7767..67546e13 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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; iCompile(); + + // 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); + } + } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 35d62796..70d36bc8 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -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 }; diff --git a/src/user/user_util.cc b/src/user/user_util.cc index 2067396f..7ffec0c1 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -23,6 +23,7 @@ #include #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 diff --git a/src/user/user_util.h b/src/user/user_util.h index 745923e1..4575dbfb 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -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); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 1bdad5b2..20d405af 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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 diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index aa89eec4..0bb0e327 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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; ibodies.size(); i++) { Body(InsertEnd(elem, "body"), body->bodies[i]); diff --git a/test/engine/engine_plugin_test.cc b/test/engine/engine_plugin_test.cc index e40826ee..c6b46d64 100644 --- a/test/engine/engine_plugin_test.cc +++ b/test/engine/engine_plugin_test.cc @@ -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(sensor); TestSensor::InitCount()++; + return 0; }; plugin.destroy = +[](mjData* d, int instance) { delete reinterpret_cast(d->plugin_data[instance]); @@ -247,6 +258,7 @@ int RegisterActuatorPlugin() { auto* actuator = new TestActuator(m, d, instance); d->plugin_data[instance] = reinterpret_cast(actuator); TestActuator::InitCount()++; + return 0; }; plugin.destroy = +[](mjData* d, int instance) { delete reinterpret_cast(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(passive); + return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + + plugin.reset = +[](const mjModel* m, mjData* d, int instance) { + auto passive = reinterpret_cast(d->plugin_data[instance]); + passive->Reset(); + }; + plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) { + auto passive = reinterpret_cast(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"( + + + + @@ -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({3*(i+1), 6*j, 3*j})); EXPECT_THAT(*reinterpret_cast(d->plugin_state + m->plugin_stateadr[4]), - testing::ElementsAreArray({5*(i+1), 10*j, 5*j})); + testing::ElementsAreArray({4*(i+1), 8*j, 4*j})); EXPECT_THAT(*reinterpret_cast(d->sensordata), testing::ElementsAreArray({ 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); diff --git a/test/plugin/elasticity/CMakeLists.txt b/test/plugin/elasticity/CMakeLists.txt new file mode 100644 index 00000000..0b24696b --- /dev/null +++ b/test/plugin/elasticity/CMakeLists.txt @@ -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) + diff --git a/test/plugin/elasticity/cable_test.cc b/test/plugin/elasticity/cable_test.cc new file mode 100644 index 00000000..c5e70674 --- /dev/null +++ b/test/plugin/elasticity/cable_test.cc @@ -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 +#include +#include +#include +#include + +#include +#include +#include +#include "test/fixture.h" + +namespace mujoco { +namespace { +using PluginTest = MujocoTest; + + +// -------------------------------- cable ----------------------------------- + +TEST_F(PluginTest, CantileverIntoCircle) { + static constexpr char cantilever_xml[] = R"( + + + )"; + + 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::epsilon()); + EXPECT_NEAR(d->sensordata[1], 0, std::numeric_limits::epsilon()); + EXPECT_NEAR(d->sensordata[2], 1, std::numeric_limits::epsilon()); + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(PluginTest, InvalidTxtAttribute) { + static constexpr char cantilever_xml[] = R"( + + + + + + + + + + + + + + )"; + + 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"( + + + + + + + + + + + + + + )"; + + 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"( + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + + mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m, testing::NotNull()) << error; + mj_deleteModel(m); +} + +} // namespace +} // namespace mujoco diff --git a/test/sample/testspeed_test.sh b/test/sample/testspeed_test.sh index bb0e8e10..c26c3f07 100755 --- a/test/sample/testspeed_test.sh +++ b/test/sample/testspeed_test.sh @@ -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 diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 75c7e742..288740e8 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -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; } diff --git a/unity/Runtime/Bindings/MujocoBindings.cs b/unity/Runtime/Bindings/MujocoBindings.cs index 9c19b403..af7f877e 100644 --- a/unity/Runtime/Bindings/MujocoBindings.cs +++ b/unity/Runtime/Bindings/MujocoBindings.cs @@ -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;