chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add adhesion actuators.

Browse Source 
- Adhesion actuators using contact normals as force transmission mechanism.
- Related video: https://youtu.be/HdBue4MUZys

Closes #229

PiperOrigin-RevId: 464389367
Change-Id: I9f69b3cd152d957e8f65870d208788463c036a6d
This commit is contained in:
Yuval Tassa
2022-07-31 08:53:29 -07:00
committed by Copybara-Service
parent 5c5449bf82
commit 3d77eb1ef4
17 changed files with 475 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+60 -5
View File
@@ -2386,6 +2386,11 @@ slidersite, cranksite.
All :ref:`muscle <muscle>` attributes are available here except: name, class, joint, jointinparent, site, tendon,
slidersite, cranksite.
:el-prefix:`default/` **adhesion** (?)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
All :ref:`adhesion <adhesion>` attributes are available here except: name, class, body.
.. _custom:
**custom** (*)
@@ -4287,7 +4292,7 @@ specify them independently.
Identical to joint, except that for ball and free joints, the 3d rotation axis given by gear is defined in the parent
frame (which is the world frame for free joints) rather than the child frame.
:at:`site`: :at-val:`string, optional`
This actuator can applies force and torque at a site. The gear vector defines a 3d translation axis followed by a 3d
This transmission can apply force and torque at a site. The gear vector defines a 3d translation axis followed by a 3d
rotation axis. Both are defined in the site's frame. This can be used to model jets and propellers. The effect is
similar to actuating a free joint, and the actuator length is again defined as zero. One difference from the joint
and jointinparent transmissions above is that here the actuator operates on a site rather than a joint, but this
@@ -4295,6 +4300,11 @@ specify them independently.
that for site transmissions both the translation and rotation axes are defined in local coordinates. In contrast,
translation is global and rotation is local for joint, and both translation and rotation are global for
jointinparent.
:at:`body`: :at-val:`string, optional`
This transmission can apply linear forces at contact points in the direction of the contact normal. The set of
contacts is all those belonging to the specified :at:`body`. This can be used to model natural active adhesion
mechanisms like the feet of geckos and insects. The actuator length is again defined as zero. For more information,
see the :ref:`adhesion<adhesion>` shortcut below.
:at:`tendon`: :at-val:`string, optional`
If specified, the actuator acts on the given tendon. The actuator length equals the tendon length times the gear
ratio. Both spatial and fixed tendons can be used.
@@ -4487,7 +4497,9 @@ This element has one custom attribute in addition to the common attributes:
:el-prefix:`actuator/` **damper** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element is an active damper which produces a force proportional to both velocity and control: ``F = - kv * velocity * control``, where ``kv`` must be nonnegative. :at:`ctrlrange` is required and must also be nonnegative. The underlying :el:`general` attributes are set as follows:
This element is an active damper which produces a force proportional to both velocity and control: ``F = - kv * velocity
* control``, where ``kv`` must be nonnegative. :at:`ctrlrange` is required and must also be nonnegative. The underlying
:el:`general` attributes are set as follows:
=========== ======= ========= =======
Attribute Setting Attribute Setting
@@ -4501,8 +4513,9 @@ ctrllimited true
This element has one custom attribute in addition to the common attributes:
.. |actuator/damper attrib list| replace::
:at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`user`
.. |actuator/damper attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`,
:at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`,
:at:`jointinparent`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`user`
|actuator/damper attrib list|
Same as in actuator/ :ref:`general <general>`.
@@ -4593,10 +4606,52 @@ This element has nine custom attributes in addition to the common attributes:
:at:`fvmax`: :at-val:`real, "1.2"`
Active force generated at saturating lengthening velocity, relative to the peak rest force.
.. _adhesion:
:el-prefix:`actuator/` **adhesion** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
.. youtube:: HdBue4MUZys
:align: right
:height: 200px
This element defines an active adhesion actuator which injects force at contacts in the normal direction. On the right
is a video demonstrating the this actuator type. The model shown in the video can be found `here
<https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_. The transmission target is a :el:`body`, and adhesive
forces are injected into all contacts invloving geoms which belong to this body. The force is devided equally between
multiple active contacts. Because it requires contact, it cannot apply force at a distance, and is more like the active
adhesion on the feet of geckos and insects rather than an industrial vaccum gripper. Adhesion actuators' length is
always 0. :at:`ctrlrange` is required and must also be nonnegative (no repulsive forces are allowed). The underlying
:el:`general` attributes are set as follows:
=========== ======= =========== ========
Attribute Setting Attribute Setting
=========== ======= =========== ========
dyntype none dynprm 1 0 0
gaintype fixed gainprm gain 0 0
biastype none biasprm 0 0 0
trntype body ctrllimited true
=========== ======= =========== ========
This element has a subset of the common attributes and two custom attributes.
.. |actuator/adhesion attrib list| replace:: :at:`name`, :at:`class`, :at:`group`,
:at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`user`
|actuator/adhesion attrib list|
Same as in actuator/ :ref:`general <general>`.
:at:`body`: :at-val:`string, required`
The actuator acts on all contacts involving this body's geoms.
:at:`gain`: :at-val:`real, "1"`
Gain of the adhesion actuator, units of force. The total adhesion force applied by the actuator is the control value
multiplied by the gain. This force is distributed equally between all the contacts involving geoms belonging to the
target body.
.. _sensor:
**sensor** (*)
~~~~~~~~~~~~~~~~~~~~~~~~~~~
~~~~~~~~~~~~~~
This is a grouping element for sensor definitions. It does not have attributes. The outputs of all sensors are
concatenated in the field mjData.sensordata which has size mjModel.nsensordata. This data is not used in any internal
doc/changelog.rst
+36 -33
View File
@@ -7,9 +7,11 @@ Upcoming version (not yet released)
General
^^^^^^^
- Added :ref:`adhesion actuators<adhesion>`.
- Added an `active adhesion example model <https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_.
- Added :ref:`mj_jacSubtreeCom` for computing the translational Jacobian of the center-of-mass of a subtree.
- Added moment of inertia computation for concave meshes. This is a breaking change, to get back to the previous
behavior set the compiler flag :at:`exactmeshinertia` to false.
- Added moment of inertia computation for concave meshes. This is currently activated by setting the compiler flag
:at:`exactmeshinertia` to ``true`` (defaults to ``false``). This default may change in the future.
- Added parameter :at:`shellinertia` in :at:`geom` for treating a mesh as a boundary mesh (shell) for inertia
computations. This is currently supported only for meshes.
- Raise error if the orientation of mesh faces is not consistent, which causes the inertia computations to be
@@ -17,6 +19,7 @@ General
Bug fixes
^^^^^^^^^
- Fixed rendering of some transparent geoms in relflection.
Version 2.2.1 (July 18, 2022)
-----------------------------
@@ -24,45 +27,45 @@ Version 2.2.1 (July 18, 2022)
General
^^^^^^^
- Added ``mjd_transitionFD`` to compute efficient finite difference approximations of the state-transition and
control-transition matrices, :ref:`see here<derivatives>` for more details.
- Added derivatives for the ellipsoid fluid model.
- Added ``ctrl`` attribute to :ref:`keyframes<keyframe>`.
- Added ``clock`` sensor which :ref:`measures time<sensor-clock>`.
- Added visualisation groups to skins.
- Added actuator visualisation for ``free`` and ``ball`` joints and for actuators with ``site`` transmission.
- Added visualisation for actuator activations.
- Added ``<intvelocity>`` actuator shortcut for "integrated velocity" actuators, documented :ref:`here <intvelocity>`.
- Added ``<damper>`` actuator shortcut for active-damping actuators, documented :ref:`here <damper>`.
- ``mju_rotVecMat`` and ``mju_rotVecMatT`` now support in-place multiplication.
- ``mjData.ctrl`` values are no longer clamped in-place, remain untouched by the engine.
- Arrays in mjData's buffer now align to 64-byte boundaries rather than 8-byte.
- Add memory poisoning when building with Address Sanitizer (ASAN) and Memory Sanitizer (MSAN). This allows ASAN to
detect reads and writes to regions in ``mjModel.buffer`` and ``mjData.buffer`` that do not lie within an array, and
for MSAN to detect reads from uninitialised fields in ``mjData`` following ``mj_resetData``.
- Add a `slider-crank example <https://github.com/deepmind/mujoco/tree/2.2.1/model/slider_crank>`_ to ``model/``.
1. Added ``mjd_transitionFD`` to compute efficient finite difference approximations of the state-transition and
control-transition matrices, :ref:`see here<derivatives>` for more details.
#. Added derivatives for the ellipsoid fluid model.
#. Added ``ctrl`` attribute to :ref:`keyframes<keyframe>`.
#. Added ``clock`` sensor which :ref:`measures time<sensor-clock>`.
#. Added visualisation groups to skins.
#. Added actuator visualisation for ``free`` and ``ball`` joints and for actuators with ``site`` transmission.
#. Added visualisation for actuator activations.
#. Added ``<intvelocity>`` actuator shortcut for "integrated velocity" actuators, documented :ref:`here <intvelocity>`.
#. Added ``<damper>`` actuator shortcut for active-damping actuators, documented :ref:`here <damper>`.
#. ``mju_rotVecMat`` and ``mju_rotVecMatT`` now support in-place multiplication.
#. ``mjData.ctrl`` values are no longer clamped in-place, remain untouched by the engine.
#. Arrays in mjData's buffer now align to 64-byte boundaries rather than 8-byte.
#. Added memory poisoning when building with Address Sanitizer (ASAN) and Memory Sanitizer (MSAN). This allows ASAN to
detect reads and writes to regions in ``mjModel.buffer`` and ``mjData.buffer`` that do not lie within an array, and
for MSAN to detect reads from uninitialised fields in ``mjData`` following ``mj_resetData``.
#. Added a `slider-crank example model <https://github.com/deepmind/mujoco/tree/main/model/slider_crank>`_.
Bug fixes
^^^^^^^^^
- :ref:`Activation clamping <CActRange>` was not being applied in the :ref:`implicit integrator<geIntegration>`.
- Stricter parsing of orientation specifiers. Before this change, a specification that included both ``quat`` and an
:ref:`alternative specifier<COrientation>` e.g., ``<geom ... quat=".1 .2 .3 .4" euler="10 20 30">``, would lead to the
``quat`` being ignored and only ``euler`` being used. After this change a parse error will be thrown.
- Stricter parsing of XML attributes. Before this change an erroneous XML snippet like ``<geom size="1/2 3 4">`` would
have been parsed as ``size="1 0 0"`` and no error would have been thrown. Now throws an error.
- Trying to load a ``NaN`` via XML like ``<geom size="1 NaN 4">``, while allowed for debugging purposes, will now print
a warning.
- Fixed null pointer dereference in ``mj_loadModel``.
- Fixed memory leaks when loading an invalid model from MJB.
- Integer overflows are now avoided when computing ``mjModel`` buffer sizes.
- Added missing warning string for ``mjWARN_BADCTRL``.
15. :ref:`Activation clamping <CActRange>` was not being applied in the :ref:`implicit integrator<geIntegration>`.
#. Stricter parsing of orientation specifiers. Before this change, a specification that included both ``quat`` and an
:ref:`alternative specifier<COrientation>` e.g., ``<geom ... quat=".1 .2 .3 .4" euler="10 20 30">``, would lead to
the ``quat`` being ignored and only ``euler`` being used. After this change a parse error will be thrown.
#. Stricter parsing of XML attributes. Before this change an erroneous XML snippet like ``<geom size="1/2 3 4">`` would
have been parsed as ``size="1 0 0"`` and no error would have been thrown. Now throws an error.
#. Trying to load a ``NaN`` via XML like ``<geom size="1 NaN 4">``, while allowed for debugging purposes, will now print
a warning.
#. Fixed null pointer dereference in ``mj_loadModel``.
#. Fixed memory leaks when loading an invalid model from MJB.
#. Integer overflows are now avoided when computing ``mjModel`` buffer sizes.
#. Added missing warning string for ``mjWARN_BADCTRL``.
Packaging
^^^^^^^^^
- Changed MacOS packaging so that the copy of ``mujoco.framework`` embedded in ``MuJoCo.app`` can be used to build
applications externally.
23. Changed MacOS packaging so that the copy of ``mujoco.framework`` embedded in ``MuJoCo.app`` can be used to build
applications externally.
Version 2.2.0 (May 23, 2022)
doc/computation.rst
+22 -6
View File
@@ -256,12 +256,28 @@ actuator works. The user can set them independently for maximum flexibility, or
<CActuator>` which instantiate common actuator types.
Transmission
Each actuator has a scalar length :math:`l_i(q)` defined by the type of transmission and its parameters. The gradient
:math:`\nabla l_i` is an :math:`n_V`-dimensional column vector of moment arms. It determines the mapping from scalar
:math:`\nabla l_i` is an :math:`n_V`-dimensional vector of moment arms. It determines the mapping from scalar
actuator force to joint force. The transmission properties are determined by the MuJoCo object to which the actuator
is attached; the possible attachment object types are joint, tendon, site and slider-crank. The latter can also be
modeled explicitly by creating MuJoCo bodies and coupling them with equality constraints to the rest of the system,
but that would be less efficient.
is attached; the possible attachment object types are :at:`joint`, :at:`tendon`, :at:`jointinparent`,
:at:`slider-crank`, :at:`site`, and :at:`body`. The :at:`joint` and :at:`tendon` transmission types act as expected
mechanically and correspond to the actuator applying forces or torques to the target object.
The :at:`jointinparent` transmission is unique to ball and free joint and asserts that rotation should be measured
in the parent rather than child frame.
:at:`slider-crank` `transmissions <https://en.wikipedia.org/wiki/Slider-crank_linkage>`_ transform a linear force to
a torque, as in a piston-driven combustion engine. `This model
<https://github.com/deepmind/mujoco/tree/main/model/slider_crank>`_ contains pedagogical examples. Slider-cranks can
also be modeled explicitly by creating MuJoCo bodies and coupling them with equality constraints to the rest of the
system, but that would be less efficient.
:at:`site` and :at:`body` are degenerate transmission targets, as their length :math:`l_i(q)` is always 0.
They can therefore not be used to maintain a desired length value, as with a position actuator. Site
transmissions correspond to applying a Cartsian force/torque at the site, while :el:`body` transmissions correspond
to applying forces at contact points belonging to a body. For more information about adhesion, see the
:ref:`adhesion<adhesion>` shorcut documentation.
Activation dynamics
Some actuators such as pneumatic and hydraulic cylinders as well as biological muscles have an internal state called
@@ -1458,14 +1474,14 @@ The top-level function :ref:`mj_step` invokes the sequence of computations below
cameras and lights. It also normalizes all quaternions, just in case.
#. Compute the body inertias and joint axes, in global frames centered at the centers of mass of the corresponding
kinematic subtrees (to improve floating-point accuracy).
#. Compute the tendon lengths and moment arms. This includes the computation of minimal-length paths for spatial
tendons.
#. Compute the actuator lengths and moment arms.
#. Compute the composite rigid body inertias and construct the joint-space inertia matrix.
#. Compute the sparse factorization of the joint-space inertia matrix.
#. Construct the list of active contacts. This includes both broad-phase and near-phase collision detection.
#. Construct the constraint Jacobian and compute the constraint residuals.
#. Compute the matrices and vectors needed by the constraint solvers.
#. Compute the tendon lengths and moment arms. This includes the computation of minimal-length paths for spatial
tendons.
#. Compute sensor data that only depends on position, and the potential energy if enabled.
#. Compute the tendon and actuator velocities.
#. Compute the body velocities and rates of change of the joint axes, again in the global coordinate frames centered at
doc/modeling.rst
+15 -5
View File
@@ -492,9 +492,10 @@ addition, the user can specify the maximum number of iterations, and tolerance l
There is also a second Noslip solver, which is a post-processing step enabled by specifying a positive number of
noslip iterations. All these algorithm settings can be specified in the :ref:`option <option>` element.
The default settings work well for most models, but in some cases it is necessary to tune the algorithm. The best way
to do this is to experiment with the relevant settings and use the visual profiler in `simulate.cc`_, which shows the timing of different computations as
well as solver statistics per iteration. We can offer the following general guidelines and observations:
The default settings work well for most models, but in some cases it is necessary to tune the algorithm. The best way to
do this is to experiment with the relevant settings and use the visual profiler in `simulate.cc`_, which shows the
timing of different computations as well as solver statistics per iteration. We can offer the following general
guidelines and observations:
- The constraint Jacobian should be dense for small models and sparse for large models. The default setting is 'auto';
it resolves to dense when the number of degrees of freedom is up to 60, and sparse over 60. Note however that the
@@ -545,9 +546,18 @@ Actuator shortcuts
~~~~~~~~~~~~~~~~~~
As explained in the :ref:`Actuation model <geActuation>` section of the Computation chapter, MuJoCo offers a flexible
actuator model with transmission, activation dynamics and force generation components that can be specified independently. The full functionality can be accessed via the XML element :ref:`general <general>` which allows the user
actuator model with transmission, activation dynamics and force generation components that can be specified
independently. The full functionality can be accessed via the XML element :ref:`general <general>` which allows the user
to create a variety of custom actuators. In addition, MJCF provides shortcuts for configuring common actuators. This is
done via the XML elements :ref:`motor <motor>`, :ref:`position <position>`, :ref:`velocity <velocity>`, :ref:`intvelocity <intvelocity>`, :ref:`damper<damper>`, :ref:`cylinder<cylinder>`, and :ref:`muscle <muscle>`. These are *not* separate model elements. Internally MuJoCo supports only one actuator type - which is why when an MJCF model is saved all actuators are written as :el:`general`. Shortcuts create general actuators implicitly, set their attributes to suitable values, and expose a subset of attributes with possibly different names. For example, :el:`position` creates a position servo with attribute :at:`kp` which is the servo gain. However :el:`general` does not have an attribute :at:`kp`. Instead the parser adjusts the gain and bias parameters of the general actuator in a coordinated way so as to mimic a position servo. The same effect could have been achieved by using :el:`general` directly, and setting its attributes to certain values as described below.
done via the XML elements :ref:`motor <motor>`, :ref:`position <position>`, :ref:`velocity <velocity>`,
:ref:`intvelocity <intvelocity>`, :ref:`damper<damper>`, :ref:`cylinder<cylinder>`, :ref:`muscle <muscle>`, and
:ref:`adhesion <adhesion>`. These are *not* separate model elements. Internally MuJoCo supports only one actuator type -
which is why when an MJCF model is saved all actuators are written as :el:`general`. Shortcuts create general actuators
implicitly, set their attributes to suitable values, and expose a subset of attributes with possibly different names.
For example, :el:`position` creates a position servo with attribute :at:`kp` which is the servo gain. However
:el:`general` does not have an attribute :at:`kp`. Instead the parser adjusts the gain and bias parameters of the
general actuator in a coordinated way so as to mimic a position servo. The same effect could have been achieved by using
:el:`general` directly, and setting its attributes to certain values as described below.
Actuator shortcuts also interact with defaults. Recall that the :ref:`default setting <CDefault>` mechanism involves
classes, each of which has a complete collection of dummy elements (one of each element type) used to initialize the
include/mujoco/mjmodel.h
+1
View File
@@ -183,6 +183,7 @@ typedef enum mjtTrn_ { // type of actuator transmission
mjTRN_SLIDERCRANK, // force via slider-crank linkage
mjTRN_TENDON, // force on tendon
mjTRN_SITE, // force on site
mjTRN_BODY, // adhesion force on a body's geoms
mjTRN_UNDEFINED = 1000 // undefined transmission type
} mjtTrn;
introspect/enums.py
+1
View File
@@ -195,6 +195,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjTRN_SLIDERCRANK', 2),
('mjTRN_TENDON', 3),
('mjTRN_SITE', 4),
('mjTRN_BODY', 5),
('mjTRN_UNDEFINED', 1000),
]),
)),
model/adhesion/README.md
+9
View File
@@ -0,0 +1,9 @@
# Active adhesion example
This example model shows how to use adhesion actuators.
The video below is a screen capture of a user interacting with the model:
[![Active adhesion example model](https://img.youtube.com/vi/HdBue4MUZys/0.jpg)](https://www.youtube.com/watch?v=HdBue4MUZys)
model/adhesion/active_adhesion.xml
+138
View File
@@ -0,0 +1,138 @@
<mujoco model="Active adhesion example">
<size nconmax="500" njmax="1000"/>
<visual>
<headlight diffuse=".2 .2 .2"/>
</visual>
<default>
<joint damping=".3" axis="0 1 0"/>
<!--
By adding 3mm of margin yet making solimp barely apply any force until 3mm penetration, we get
3mm of "action at a distance". This is important so that small changes in distance don't lead
to loss of adhesion
-->
<geom type="box" friction=".5" margin=".003" solimp="0 .99 .003 .9 6"/>
<default class="wall">
<geom rgba=".5 .5 .5 .4"/>
</default>
<default class="mechanical">
<geom rgba=".5 .5 .8 1"/>
<tendon rgba=".5 .5 .8 1"/>
</default>
<default class="active_adhesion">
<geom rgba=".8 .5 .5 1"/>
</default>
<default class="object">
<geom rgba=".5 .8 .5 1" density="100"/>
</default>
</default>
<worldbody>
<light pos="0.4 -.15 .6" dir="-1 .5 -1" diffuse=".7 .7 .7"/>
<light pos="-.4 -.15 .6" dir="1 .5 -1" diffuse=".7 .7 .7"/>
<body name="two crates">
<joint name="conveyor" type="slide" damping="100" axis="1 0 0"/>
<geom size=".2 .1 .01" pos="0 0 -.01"/>
<geom size=".01 .08 .031" pos="-.19 0 .03" class="wall"/>
<geom size=".01 .08 .031" pos="0 0 .03" class="wall"/>
<geom size=".01 .08 .031" pos="0.19 0 .03" class="wall"/>
<geom size="0.2 .01 .031" pos="0 -.09 .03" class="wall"/>
<geom size="0.2 .01 .031" pos="0 0.09 .03" class="wall"/>
</body>
<body name="arm1" pos="-.1 0 .3" childclass="mechanical">
<joint name="arm1"/>
<geom type="cylinder" size=".015 .01" zaxis="0 1 0"/>
<geom type="capsule" size=".01" fromto="0 0 0 -.12 0 -.07"/>
<body name="arm2" pos="-.12 0 -.07">
<joint name="arm2"/>
<geom type="cylinder" size=".015 .01" zaxis="0 1 0"/>
<geom type="capsule" size=".01" fromto="0 0 0 .12 0 -.07"/>
<body name="4boxes" pos=".12 0 -.08" childclass="active_adhesion">
<site name="force_sensor" group="3"/>
<joint name="arm3" damping=".01" pos="0 -.03 0"/>
<geom size=".015 .015 .01" pos="0.015 0.015 0"/>
<geom size=".015 .015 .01" pos="0.015 -.015 0"/>
<geom size=".015 .015 .01" pos="-.015 0.015 0"/>
<geom size=".015 .015 .01" pos="-.015 -.015 0"/>
</body>
</body>
</body>
<body name="box" pos="-.1 0 .05">
<freejoint/>
<geom size=".05 .05 .05" class="object"/>
</body>
<body name="winch" pos="-.01 0 .35" childclass="mechanical">
<joint name="winch" damping="1"/>
<geom type="cylinder" size=".015 .01" zaxis="0 1 0"/>
<geom type="capsule" size=".01" fromto="0 0 0 .1 0 .05"/>
<site name="anchor" pos=".1 0 .04"/>
</body>
<site name="pulley" pos=".1 0 .32"/>
<site name="hook_left" pos=".08 0 .3"/>
<site name="hook_right" pos=".12 0 .3"/>
<body name="sphere" pos=".1 0 .2" childclass="active_adhesion">
<!--
Note we are adding damping to a free joint. This is an easy yet unphysical way to prevent
the sphere from jiggling too much on the winch tendons. An alternative would be to add fluid
density or viscosity.
-->
<joint type="free" damping=".1"/>
<!--
The composite balls in the crate have only 3 linear DoFs with condim=1, effectively
frictionless point particles. In order to make them stick to the sphere we give the sphere
priority 2, to force condim=3.
-->
<geom type="sphere" size=".03" priority="2"/>
<site name="pin_left" pos="-.025 0 .025"/>
<site name="pin_right" pos=".025 0 .025"/>
</body>
<composite type="particle" count="4 4 4" spacing="0.025" offset=".11 .01 .1">
<geom size=".012" rgba=".5 .8 .5 1" solref=".005 1"/>
</composite>
</worldbody>
<equality>
<joint joint1="arm1" joint2="arm2" polycoef="0 -.5 0 0 0"/>
<joint joint1="arm3" joint2="arm1"/>
</equality>
<!--
By using divisor=3 in the pullies we increase the distance by which the hanging sphere moves
relative to the motion of the winch arm. One should imagine a double spindle with two radii that
creates a ratio of 1.5 between the motion of tendon before the pullies and after the pullies.
(1.5 rather than 3 because the tendon has 2 branches and the length is split between them)
-->
<tendon>
<spatial range="0 .19" limited="true" solreflimit=".01 2" class="mechanical">
<site site="anchor"/>
<site site="pulley"/>
<pulley divisor="3"/>
<site site="pulley"/>
<site site="hook_left"/>
<site site="pin_left"/>
<pulley divisor="3"/>
<site site="pulley"/>
<site site="hook_right"/>
<site site="pin_right"/>
</spatial>
</tendon>
<actuator>
<position name="conveyor" joint="conveyor" ctrlrange="-.2 .2" ctrllimited="true" kp="400"/>
<position name="arm" joint="arm2" ctrlrange="-.8 1" ctrllimited="true" kp="10"/>
<adhesion name="adhere_arm" body="4boxes" ctrlrange="0 1" gain="20"/>
<position name="winch" joint="winch" ctrlrange="-.7 .5" ctrllimited="true" kp="10"/>
<adhesion name="adhere_winch" body="sphere" ctrlrange="0 1" gain="20"/>
</actuator>
<sensor>
<force site="force_sensor"/>
</sensor>
</mujoco>
python/mujoco/functions.cc
+1 -1
View File
@@ -65,7 +65,7 @@ PYBIND11_MODULE(_functions, pymodule) {
DEF_WITH_OMITTED_PY_ARGS(traits::mj_printSchema,
"filename", "buffer", "buffer_sz")(
pymodule, [](bool flg_html, bool flg_pad) {
constexpr int kBufferSize = 28000;
constexpr int kBufferSize = 30000;
auto buffer = std::unique_ptr<char[]>(new char[kBufferSize]);
const int out_length = InterceptMjErrors(::mj_printSchema)(
nullptr, buffer.get(), kBufferSize, flg_html, flg_pad);
src/engine/engine_core_smooth.c
+42
View File
@@ -785,6 +785,48 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_addTo(moment+i*nv, jac, nv); // add the two
break;
case mjTRN_BODY: // body (adhesive contacts)
// cannot compute meaningful length, set to 0
length[i] = 0;
// moment is average of all contact normal Jacobians
{
// find and count all relevant contacts, mark them in efc_force
int counter = 0;
mjtNum* efc_force = mj_stackAlloc(d, d->nefc);
mju_zero(efc_force, d->nefc);
for (int j=0; j<d->ncon; j++) {
const mjContact* con = d->contact+j;
if (m->geom_bodyid[con->geom1]==id || m->geom_bodyid[con->geom2]==id) {
if (!con->exclude) {
counter++;
// condim 1 or elliptic cones: normal is in the first row
if (con->dim == 1 || m->opt.cone==mjCONE_ELLIPTIC) {
efc_force[con->efc_address] = 1;
}
// pyramidal cones: average all pyramid directions
else {
int npyramid = con->dim-1; // number of frictional directions
for (int k=0; k<2*npyramid; k++) {
efc_force[con->efc_address+k] = 0.5/npyramid;
}
}
} else if (con->exclude == 1) {
// TODO(b/240848298): compute Jacobians for excluded contact (in gap)
}
}
}
// moment is average over contact normal Jacobians, make negative for adhesion
if (counter) {
mj_mulJacTVec(m, d, moment+i*nv, efc_force);
mju_scl(moment+i*nv, moment+i*nv, -1.0/counter, nv);
}
}
break;
default:
mju_error_i("Unknown transmission type %d", m->actuator_trntype[i]); // SHOULD NOT OCCUR
}
src/engine/engine_forward.c
+1 -1
View File
@@ -97,7 +97,6 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
mj_comPos(m, d);
mj_camlight(m, d);
mj_tendon(m, d);
mj_transmission(m, d);
TM_END(mjTIMER_POS_KINEMATICS);
TM_RESTART;
@@ -111,6 +110,7 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
TM_RESTART;
mj_makeConstraint(m, d);
mj_transmission(m, d);
TM_END(mjTIMER_POS_MAKE);
TM_RESTART;
src/engine/engine_vis_visualize.c
+30 -2
View File
@@ -847,8 +847,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// site actuators
if (m->actuator_trntype[i]==mjTRN_SITE) {
// set size of the geometry
mju_scl3(sz, m->site_size+3*j, 1.1);
// inflate sizes by 5%
mju_scl3(sz, m->site_size+3*j, 1.05);
// make geom
mjv_initGeom(thisgeom,
@@ -895,6 +895,34 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
FINISH
}
// body actuators
else if (m->actuator_trntype[i]==mjTRN_BODY) {
// iterate over body's geoms
int geomnum = m->body_geomnum[j];
int geomadr = m->body_geomadr[j];
for (int k=geomadr; k<geomadr+geomnum; k++) {
int geomtype = m->geom_type[k];
// add inflated geom if it is a regular primitive
if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && geomtype != mjGEOM_MESH) {
START
// inflate sizes by 5%
mju_scl3(sz, m->geom_size+3*k, 1.05);
// make geom
mjv_initGeom(thisgeom,
m->geom_type[k], sz,
d->geom_xpos + 3*k,
d->geom_xmat + 9*k,
thisgeom->rgba);
// set interpolated color
f2f(thisgeom->rgba, rgba, 4);
FINISH
}
}
}
// spatial tendon actuators
else if (m->actuator_trntype[i]==mjTRN_TENDON && d->ten_wrapnum[j]) {
for (int k=d->ten_wrapadr[j]; k<d->ten_wrapadr[j]+d->ten_wrapnum[j]-1; k++) {
src/user/user_objects.cc
+5
View File
@@ -3461,6 +3461,11 @@ void mjCActuator::Compile(void) {
ptarget = model->FindObject(mjOBJ_SITE, target);
break;
case mjTRN_BODY:
// get body
ptarget = model->FindObject(mjOBJ_BODY, target);
break;
default:
throw mjCError(this, "invalid transmission type in actuator '%s' (id = %d)", name.c_str(), id);
}
src/xml/xml_native_reader.cc
+40 -8
View File
@@ -42,7 +42,7 @@ using tinyxml2::XMLElement;
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 163;
static const int nMJCF = 165;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -145,6 +145,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"gear", "cranklength", "user", "group",
"timeconst", "range", "force", "scale",
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "?", "6", "forcelimited", "ctrlrange", "forcerange",
"gain", "user", "group"},
{">"},
{"custom", "*", "0"},
@@ -262,10 +264,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"actuator", "*", "0"},
{"<"},
{"general", "*", "25", "name", "class", "group",
{"general", "*", "26", "name", "class", "group",
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "body",
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
{"motor", "*", "17", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
@@ -303,6 +305,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"joint", "jointinparent", "tendon", "slidersite", "cranksite",
"timeconst", "range", "force", "scale",
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "*", "9", "name", "class", "group",
"forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"},
{">"},
{"sensor", "*", "0"},
@@ -1426,7 +1430,10 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
pact->trntype = mjTRN_SITE;
cnt++;
}
if (ReadAttrTxt(elem, "body", pact->target)) {
pact->trntype = mjTRN_BODY;
cnt++;
}
// check for repeated transmission
if (cnt>1) {
throw mjXError(elem, "actuator can have at most one of transmission target");
@@ -1522,7 +1529,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
// damper
else if (type=="damper") {
// clear bias
// clear gain
mjuu_zerovec(pact->gainprm, mjNGAIN);
// explicit attributes
@@ -1531,10 +1538,10 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
throw mjXError(elem, "damping coefficient cannot be negative");
pact->gainprm[2] = -pact->gainprm[2];
// Require nonnegative range
ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text);
// require nonnegative range
ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text, true);
if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) {
throw mjXError(elem, "control range cannot be negative");
throw mjXError(elem, "damper control range cannot be negative");
}
// implied parameters
@@ -1597,6 +1604,31 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
pact->biastype = mjBIAS_MUSCLE;
}
// adhesion
else if (type=="adhesion") {
// clear bias, set default gain
mjuu_zerovec(pact->biasprm, mjNBIAS);
mjuu_zerovec(pact->gainprm, mjNGAIN);
pact->gainprm[0] = 1;
// explicit attributes
ReadAttr(elem, "gain", 1, pact->gainprm, text);
if (pact->gainprm[0]<0)
throw mjXError(elem, "adhesion gain cannot be negative");
// require nonnegative range
ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text, true);
if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) {
throw mjXError(elem, "adhesion control range cannot be negative");
}
// implied parameters
pact->ctrllimited = true;
pact->dyntype = mjDYN_NONE;
pact->gaintype = mjGAIN_FIXED;
pact->biastype = mjBIAS_NONE;
}
else { // SHOULD NOT OCCUR
throw mjXError(elem, "unrecognized actuator type: %s", type.c_str());
}
src/xml/xml_native_writer.cc
+4
View File
@@ -554,6 +554,10 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
WriteAttrTxt(elem, "site", pact->target);
break;
case mjTRN_BODY:
WriteAttrTxt(elem, "body", pact->target);
break;
default: // SHOULD NOT OCCUR
break;
}
test/engine/engine_core_smooth_test.cc
+69 -1
View File
@@ -249,7 +249,7 @@ TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) {
}
// ----------------------------- fluidshape --------------------------------
// ------------------------ ellipsoid fluid model ------------------------------
using EllipsoidFluidTest = MujocoTest;
@@ -348,5 +348,73 @@ TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
mj_deleteModel(model);
}
// -------------------------- adhesion actuators -------------------------------
using AdhesionTest = MujocoTest;
TEST_F(AdhesionTest, ExpectedAdhesionForce) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -1"/>
<worldbody>
<body name="static">
<!-- small increase to size to ensure contact -->
<geom size=".02001" pos=" .01 .01 .07"/>
<geom size=".02001" pos="-.01 .01 .07"/>
<geom size=".02001" pos=" .01 -.01 .07"/>
<geom size=".02001" pos="-.01 -.01 .07"/>
</body>
<body name="free">
<freejoint/>
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<adhesion body="static" ctrlrange="0 2"/>
<adhesion body="free" ctrlrange="0 2"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
// iterate over cone type
for (mjtCone cone : {mjCONE_ELLIPTIC, mjCONE_PYRAMIDAL}) {
// set cone
model->opt.cone = cone;
// iterate over condim
for (int condim : {1, 3, 4, 6}) {
// set condim
for (int id=0; id < model->ngeom; id++) {
model->geom_condim[id] = condim;
}
// iterate over actuators
for (int id=0; id < 2; id++) {
// set ctrl > 1, expect free body to not fall
mj_resetData(model, data);
data->ctrl[id] = 1.01;
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
// moved down at most 10 microns
EXPECT_GT(data->qpos[2], -1e-5);
// set ctrl < 1, expect free body to fall below 1cm
mj_resetData(model, data);
data->ctrl[id] = 0.99;
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
// fell lower than 1cm
EXPECT_LT(data->qpos[2], -0.01);
}
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/xml/xml_native_reader_test.cc
+1 -1
View File
@@ -375,7 +375,7 @@ TEST_F(UserDataTest, RequiresControlRange) {
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid control range for actuator"));
EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'ctrlrange'"));
}
TEST_F(UserDataTest, PositiveControlRange) {