Add tendon actuator force limits and tendon actuator force sensor.
PiperOrigin-RevId: 745096883 Change-Id: Ib9acb727fbbfc6b0b0323ee6a889053a7a878056
This commit is contained in:
committed by
Copybara-Service
parent
16e49f2761
commit
96dda6ea75
@@ -4630,12 +4630,28 @@ length X, as in the clip on the right of `this example model
|
|||||||
solver. If this attribute is "auto", and :at:`autolimits` is set in :ref:`compiler <compiler>`, length limits
|
solver. If this attribute is "auto", and :at:`autolimits` is set in :ref:`compiler <compiler>`, length limits
|
||||||
will be enabled if range is defined.
|
will be enabled if range is defined.
|
||||||
|
|
||||||
|
.. _tendon-spatial-actuatorfrclimited:
|
||||||
|
|
||||||
|
:at:`actuatorfrclimited`: :at-val:`[false, true, auto], "auto"`
|
||||||
|
This attribute specifies whether actuator forces acting on the tendon should be clamped. See :ref:`CForceRange` for
|
||||||
|
details. This attribute interacts with the :ref:`actuatorfrcrange<tendon-spatial-actuatorfrcrange>` attribute. If
|
||||||
|
this attribute is "false", actuator force clamping is disabled. If it is "true", actuator force clamping is enabled.
|
||||||
|
If this attribute is "auto", and :at:`autolimits` is set in :ref:`compiler <compiler>`, actuator force clamping will
|
||||||
|
be enabled if :at:`actuatorfrcrange` is defined.
|
||||||
|
|
||||||
.. _tendon-spatial-range:
|
.. _tendon-spatial-range:
|
||||||
|
|
||||||
:at:`range`: :at-val:`real(2), "0 0"`
|
:at:`range`: :at-val:`real(2), "0 0"`
|
||||||
Range of allowed tendon lengths. Setting this attribute without specifying :at:`limited` is an error, unless
|
Range of allowed tendon lengths. Setting this attribute without specifying :at:`limited` is an error, unless
|
||||||
:at:`autolimits` is set in :ref:`compiler <compiler>`.
|
:at:`autolimits` is set in :ref:`compiler <compiler>`.
|
||||||
|
|
||||||
|
.. _tendon-spatial-actuatorfrcrange:
|
||||||
|
|
||||||
|
:at:`actuatorfrcrange`: :at-val:`real(2), "0 0"`
|
||||||
|
Range for clamping total actuator forces acting on this tendon. See :ref:`CForceRange` for details. The compiler
|
||||||
|
expects the lower bound to be nonpositive and the upper bound to be nonnegative. |br| Setting this attribute without
|
||||||
|
specifying :at:`actuatorfrclimited` is an error if :at:`compiler-autolimits` is "false".
|
||||||
|
|
||||||
.. _tendon-spatial-solreflimit:
|
.. _tendon-spatial-solreflimit:
|
||||||
|
|
||||||
.. _tendon-spatial-solimplimit:
|
.. _tendon-spatial-solimplimit:
|
||||||
@@ -4820,8 +4836,12 @@ as above.
|
|||||||
|
|
||||||
.. _tendon-fixed-limited:
|
.. _tendon-fixed-limited:
|
||||||
|
|
||||||
|
.. _tendon-fixed-actuatorfrclimited:
|
||||||
|
|
||||||
.. _tendon-fixed-range:
|
.. _tendon-fixed-range:
|
||||||
|
|
||||||
|
.. _tendon-fixed-actuatorfrcrange:
|
||||||
|
|
||||||
.. _tendon-fixed-solreflimit:
|
.. _tendon-fixed-solreflimit:
|
||||||
|
|
||||||
.. _tendon-fixed-solimplimit:
|
.. _tendon-fixed-solimplimit:
|
||||||
@@ -6385,6 +6405,33 @@ joint or when a single actuator acts on multiple joints. See :ref:`CForceRange`
|
|||||||
The joint where actuator forces will be sensed. The sensor output is copied from ``mjData.qfrc_actuator``.
|
The joint where actuator forces will be sensed. The sensor output is copied from ``mjData.qfrc_actuator``.
|
||||||
|
|
||||||
|
|
||||||
|
.. _sensor-tendonactuatorfrc:
|
||||||
|
|
||||||
|
:el-prefix:`sensor/` |-| **tendonactuatorfrc** (*)
|
||||||
|
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||||
|
|
||||||
|
This element creates an actuator force sensor, measured at a tendon. The quantity being sensed is the total force
|
||||||
|
contributed by all actuators to a single tendon. This type of sensor is important when multiple actuators act on a
|
||||||
|
single tendon. See :ref:`CForceRange` for details.
|
||||||
|
|
||||||
|
|
||||||
|
.. _sensor-tendonactuatorfrc-name:
|
||||||
|
|
||||||
|
.. _sensor-tendonactuatorfrc-noise:
|
||||||
|
|
||||||
|
.. _sensor-tendonactuatorfrc-cutoff:
|
||||||
|
|
||||||
|
.. _sensor-tendonactuatorfrc-user:
|
||||||
|
|
||||||
|
:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user`
|
||||||
|
See :ref:`CSensor`.
|
||||||
|
|
||||||
|
.. _sensor-tendonactuatorfrc-tendon:
|
||||||
|
|
||||||
|
:at:`tendon`: :at-val:`string, required`
|
||||||
|
The tendon where actuator forces will be sensed.
|
||||||
|
|
||||||
|
|
||||||
.. _sensor-ballquat:
|
.. _sensor-ballquat:
|
||||||
|
|
||||||
:el-prefix:`sensor/` |-| **ballquat** (*)
|
:el-prefix:`sensor/` |-| **ballquat** (*)
|
||||||
@@ -8277,8 +8324,12 @@ if omitted.
|
|||||||
|
|
||||||
.. _default-tendon-limited:
|
.. _default-tendon-limited:
|
||||||
|
|
||||||
|
.. _default-tendon-actuatorfrclimited:
|
||||||
|
|
||||||
.. _default-tendon-range:
|
.. _default-tendon-range:
|
||||||
|
|
||||||
|
.. _default-tendon-actuatorfrcrange:
|
||||||
|
|
||||||
.. _default-tendon-solreflimit:
|
.. _default-tendon-solreflimit:
|
||||||
|
|
||||||
.. _default-tendon-solimplimit:
|
.. _default-tendon-solimplimit:
|
||||||
|
|||||||
+20
-7
@@ -621,13 +621,15 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`name<tendon-spatial-name>` | :ref:`class<tendon-spatial-class>` | :ref:`group<tendon-spatial-group>` | :ref:`limited<tendon-spatial-limited>` | |
|
| | | | :ref:`name<tendon-spatial-name>` | :ref:`class<tendon-spatial-class>` | :ref:`group<tendon-spatial-group>` | :ref:`limited<tendon-spatial-limited>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`range<tendon-spatial-range>` | :ref:`solreflimit<tendon-spatial-solreflimit>` | :ref:`solimplimit<tendon-spatial-solimplimit>` | :ref:`solreffriction<tendon-spatial-solreffriction>` | |
|
| | | | :ref:`actuatorfrclimited<tendon-spatial-actuatorfrclimited>` | :ref:`range<tendon-spatial-range>` | :ref:`actuatorfrcrange<tendon-spatial-actuatorfrcrange>` | :ref:`solreflimit<tendon-spatial-solreflimit>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`solimpfriction<tendon-spatial-solimpfriction>` | :ref:`frictionloss<tendon-spatial-frictionloss>` | :ref:`springlength<tendon-spatial-springlength>` | :ref:`width<tendon-spatial-width>` | |
|
| | | | :ref:`solimplimit<tendon-spatial-solimplimit>` | :ref:`solreffriction<tendon-spatial-solreffriction>` | :ref:`solimpfriction<tendon-spatial-solimpfriction>` | :ref:`frictionloss<tendon-spatial-frictionloss>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`material<tendon-spatial-material>` | :ref:`margin<tendon-spatial-margin>` | :ref:`stiffness<tendon-spatial-stiffness>` | :ref:`damping<tendon-spatial-damping>` | |
|
| | | | :ref:`springlength<tendon-spatial-springlength>` | :ref:`width<tendon-spatial-width>` | :ref:`material<tendon-spatial-material>` | :ref:`margin<tendon-spatial-margin>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`armature<tendon-spatial-armature>` | :ref:`rgba<tendon-spatial-rgba>` | :ref:`user<tendon-spatial-user>` | | |
|
| | | | :ref:`stiffness<tendon-spatial-stiffness>` | :ref:`damping<tendon-spatial-damping>` | :ref:`armature<tendon-spatial-armature>` | :ref:`rgba<tendon-spatial-rgba>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`user<tendon-spatial-user>` | | | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_2| spatial |br| |_2| |L| | | .. table:: |
|
| |_2| spatial |br| |_2| |L| | | .. table:: |
|
||||||
@@ -657,11 +659,13 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`name<tendon-fixed-name>` | :ref:`class<tendon-fixed-class>` | :ref:`group<tendon-fixed-group>` | :ref:`limited<tendon-fixed-limited>` | |
|
| | | | :ref:`name<tendon-fixed-name>` | :ref:`class<tendon-fixed-class>` | :ref:`group<tendon-fixed-group>` | :ref:`limited<tendon-fixed-limited>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`range<tendon-fixed-range>` | :ref:`solreflimit<tendon-fixed-solreflimit>` | :ref:`solimplimit<tendon-fixed-solimplimit>` | :ref:`solreffriction<tendon-fixed-solreffriction>` | |
|
| | | | :ref:`actuatorfrclimited<tendon-fixed-actuatorfrclimited>` | :ref:`range<tendon-fixed-range>` | :ref:`actuatorfrcrange<tendon-fixed-actuatorfrcrange>` | :ref:`solreflimit<tendon-fixed-solreflimit>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`solimpfriction<tendon-fixed-solimpfriction>` | :ref:`frictionloss<tendon-fixed-frictionloss>` | :ref:`springlength<tendon-fixed-springlength>` | :ref:`margin<tendon-fixed-margin>` | |
|
| | | | :ref:`solimplimit<tendon-fixed-solimplimit>` | :ref:`solreffriction<tendon-fixed-solreffriction>` | :ref:`solimpfriction<tendon-fixed-solimpfriction>` | :ref:`frictionloss<tendon-fixed-frictionloss>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`stiffness<tendon-fixed-stiffness>` | :ref:`damping<tendon-fixed-damping>` | :ref:`armature<tendon-fixed-armature>` | :ref:`user<tendon-fixed-user>` | |
|
| | | | :ref:`springlength<tendon-fixed-springlength>` | :ref:`margin<tendon-fixed-margin>` | :ref:`stiffness<tendon-fixed-stiffness>` | :ref:`damping<tendon-fixed-damping>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`armature<tendon-fixed-armature>` | :ref:`user<tendon-fixed-user>` | | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_2| fixed |br| |_2| |L| | | .. table:: |
|
| |_2| fixed |br| |_2| |L| | | .. table:: |
|
||||||
@@ -1004,6 +1008,15 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_| sensor |br| |_| |L| | | .. table:: |
|
| |_| sensor |br| |_| |L| | | .. table:: |
|
||||||
|
| :ref:`tendonactuatorfrc | \* | :class: mjcf-attributes |
|
||||||
|
| <sensor-tendonactuatorfrc>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`name<sensor-tendonactuatorfrc-name>` | :ref:`tendon<sensor-tendonactuatorfrc-tendon>` | :ref:`cutoff<sensor-tendonactuatorfrc-cutoff>` | :ref:`noise<sensor-tendonactuatorfrc-noise>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`user<sensor-tendonactuatorfrc-user>` | | | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| |_| sensor |br| |_| |L| | | .. table:: |
|
||||||
| :ref:`ballquat | \* | :class: mjcf-attributes |
|
| :ref:`ballquat | \* | :class: mjcf-attributes |
|
||||||
| <sensor-ballquat>` | | |
|
| <sensor-ballquat>` | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
|||||||
@@ -20,6 +20,8 @@ General
|
|||||||
bodies when saving to XML.
|
bodies when saving to XML.
|
||||||
- Added :ref:`orientation<body-composite-quat>` attribute to :ref:`composite<body-composite>`. Moreover, allow the
|
- Added :ref:`orientation<body-composite-quat>` attribute to :ref:`composite<body-composite>`. Moreover, allow the
|
||||||
composite to be the direct child of a frame.
|
composite to be the direct child of a frame.
|
||||||
|
- Added :ref:`tendon actuator force limits<tendon-spatial-actuatorfrclimited>` and
|
||||||
|
:ref:`tendon actuator force sensor<sensor-tendonactuatorfrc>`.
|
||||||
|
|
||||||
Bug fixes
|
Bug fixes
|
||||||
^^^^^^^^^
|
^^^^^^^^^
|
||||||
|
|||||||
@@ -649,6 +649,7 @@ typedef enum mjtSensor_ { // type of sensor
|
|||||||
mjSENS_ACTUATORVEL, // scalar actuator velocity
|
mjSENS_ACTUATORVEL, // scalar actuator velocity
|
||||||
mjSENS_ACTUATORFRC, // scalar actuator force
|
mjSENS_ACTUATORFRC, // scalar actuator force
|
||||||
mjSENS_JOINTACTFRC, // scalar actuator force, measured at the joint
|
mjSENS_JOINTACTFRC, // scalar actuator force, measured at the joint
|
||||||
|
mjSENS_TENDONACTFRC, // scalar actuator force, measured at the tendon
|
||||||
|
|
||||||
// sensors related to ball joints
|
// sensors related to ball joints
|
||||||
mjSENS_BALLQUAT, // 4D ball joint quaternion
|
mjSENS_BALLQUAT, // 4D ball joint quaternion
|
||||||
@@ -1330,12 +1331,14 @@ struct mjModel_ {
|
|||||||
int* tendon_matid; // material id for rendering (ntendon x 1)
|
int* tendon_matid; // material id for rendering (ntendon x 1)
|
||||||
int* tendon_group; // group for visibility (ntendon x 1)
|
int* tendon_group; // group for visibility (ntendon x 1)
|
||||||
mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1)
|
mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1)
|
||||||
|
mjtByte* tendon_actfrclimited; // does tendon have actuator force limits (ntendon x 1)
|
||||||
mjtNum* tendon_width; // width for rendering (ntendon x 1)
|
mjtNum* tendon_width; // width for rendering (ntendon x 1)
|
||||||
mjtNum* tendon_solref_lim; // constraint solver reference: limit (ntendon x mjNREF)
|
mjtNum* tendon_solref_lim; // constraint solver reference: limit (ntendon x mjNREF)
|
||||||
mjtNum* tendon_solimp_lim; // constraint solver impedance: limit (ntendon x mjNIMP)
|
mjtNum* tendon_solimp_lim; // constraint solver impedance: limit (ntendon x mjNIMP)
|
||||||
mjtNum* tendon_solref_fri; // constraint solver reference: friction (ntendon x mjNREF)
|
mjtNum* tendon_solref_fri; // constraint solver reference: friction (ntendon x mjNREF)
|
||||||
mjtNum* tendon_solimp_fri; // constraint solver impedance: friction (ntendon x mjNIMP)
|
mjtNum* tendon_solimp_fri; // constraint solver impedance: friction (ntendon x mjNIMP)
|
||||||
mjtNum* tendon_range; // tendon length limits (ntendon x 2)
|
mjtNum* tendon_range; // tendon length limits (ntendon x 2)
|
||||||
|
mjtNum* tendon_actfrcrange; // range of total actuator force (ntendon x 2)
|
||||||
mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1)
|
mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1)
|
||||||
mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1)
|
mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1)
|
||||||
mjtNum* tendon_damping; // damping coefficient (ntendon x 1)
|
mjtNum* tendon_damping; // damping coefficient (ntendon x 1)
|
||||||
@@ -2186,7 +2189,9 @@ typedef struct mjsTendon_ { // tendon specification
|
|||||||
|
|
||||||
// length range
|
// length range
|
||||||
int limited; // does tendon have limits (mjtLimited)
|
int limited; // does tendon have limits (mjtLimited)
|
||||||
|
int actfrclimited; // does tendon have actuator force limits
|
||||||
double range[2]; // length limits
|
double range[2]; // length limits
|
||||||
|
double actfrcrange[2]; // actuator force limits
|
||||||
double margin; // margin value for tendon limit detection
|
double margin; // margin value for tendon limit detection
|
||||||
mjtNum solref_limit[mjNREF]; // solver reference: tendon limits
|
mjtNum solref_limit[mjNREF]; // solver reference: tendon limits
|
||||||
mjtNum solimp_limit[mjNIMP]; // solver impedance: tendon limits
|
mjtNum solimp_limit[mjNIMP]; // solver impedance: tendon limits
|
||||||
@@ -3086,8 +3091,10 @@ struct mjvSceneState_ {
|
|||||||
int* tendon_matid;
|
int* tendon_matid;
|
||||||
int* tendon_group;
|
int* tendon_group;
|
||||||
mjtByte* tendon_limited;
|
mjtByte* tendon_limited;
|
||||||
|
mjtByte* tendon_actfrclimited;
|
||||||
mjtNum* tendon_width;
|
mjtNum* tendon_width;
|
||||||
mjtNum* tendon_range;
|
mjtNum* tendon_range;
|
||||||
|
mjtNum* tendon_actfrcrange;
|
||||||
mjtNum* tendon_stiffness;
|
mjtNum* tendon_stiffness;
|
||||||
mjtNum* tendon_damping;
|
mjtNum* tendon_damping;
|
||||||
mjtNum* tendon_frictionloss;
|
mjtNum* tendon_frictionloss;
|
||||||
|
|||||||
+4
-1
@@ -724,7 +724,10 @@ Force clamping at joint input with :ref:`joint/actuatorfrcrange<body-joint-actua
|
|||||||
:ref:`jointactuatorfrc<sensor-jointactuatorfrc>` sensor to report the total actuator force acting on a joint.
|
:ref:`jointactuatorfrc<sensor-jointactuatorfrc>` sensor to report the total actuator force acting on a joint.
|
||||||
The standard :ref:`actuatorfrc<sensor-actuatorfrc>` sensor will continue to report the pre-clamped actuator force.
|
The standard :ref:`actuatorfrc<sensor-actuatorfrc>` sensor will continue to report the pre-clamped actuator force.
|
||||||
|
|
||||||
The three clamping options above are non-exclusive and can be combined as required.
|
Force clamping at tendon input with :ref:`tendon/actuatorfrcrange<tendon-spatial-actuatorfrcrange>`:
|
||||||
|
This tendon attribute clamps input forces from all actuators acting on the tendon.
|
||||||
|
|
||||||
|
The clamping options above are non-exclusive and can be combined as required.
|
||||||
|
|
||||||
.. _CLengthRange:
|
.. _CLengthRange:
|
||||||
|
|
||||||
|
|||||||
@@ -317,6 +317,7 @@ typedef enum mjtSensor_ { // type of sensor
|
|||||||
mjSENS_ACTUATORVEL, // scalar actuator velocity
|
mjSENS_ACTUATORVEL, // scalar actuator velocity
|
||||||
mjSENS_ACTUATORFRC, // scalar actuator force
|
mjSENS_ACTUATORFRC, // scalar actuator force
|
||||||
mjSENS_JOINTACTFRC, // scalar actuator force, measured at the joint
|
mjSENS_JOINTACTFRC, // scalar actuator force, measured at the joint
|
||||||
|
mjSENS_TENDONACTFRC, // scalar actuator force, measured at the tendon
|
||||||
|
|
||||||
// sensors related to ball joints
|
// sensors related to ball joints
|
||||||
mjSENS_BALLQUAT, // 4D ball joint quaternion
|
mjSENS_BALLQUAT, // 4D ball joint quaternion
|
||||||
@@ -1031,12 +1032,14 @@ struct mjModel_ {
|
|||||||
int* tendon_matid; // material id for rendering (ntendon x 1)
|
int* tendon_matid; // material id for rendering (ntendon x 1)
|
||||||
int* tendon_group; // group for visibility (ntendon x 1)
|
int* tendon_group; // group for visibility (ntendon x 1)
|
||||||
mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1)
|
mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1)
|
||||||
|
mjtByte* tendon_actfrclimited; // does tendon have actuator force limits (ntendon x 1)
|
||||||
mjtNum* tendon_width; // width for rendering (ntendon x 1)
|
mjtNum* tendon_width; // width for rendering (ntendon x 1)
|
||||||
mjtNum* tendon_solref_lim; // constraint solver reference: limit (ntendon x mjNREF)
|
mjtNum* tendon_solref_lim; // constraint solver reference: limit (ntendon x mjNREF)
|
||||||
mjtNum* tendon_solimp_lim; // constraint solver impedance: limit (ntendon x mjNIMP)
|
mjtNum* tendon_solimp_lim; // constraint solver impedance: limit (ntendon x mjNIMP)
|
||||||
mjtNum* tendon_solref_fri; // constraint solver reference: friction (ntendon x mjNREF)
|
mjtNum* tendon_solref_fri; // constraint solver reference: friction (ntendon x mjNREF)
|
||||||
mjtNum* tendon_solimp_fri; // constraint solver impedance: friction (ntendon x mjNIMP)
|
mjtNum* tendon_solimp_fri; // constraint solver impedance: friction (ntendon x mjNIMP)
|
||||||
mjtNum* tendon_range; // tendon length limits (ntendon x 2)
|
mjtNum* tendon_range; // tendon length limits (ntendon x 2)
|
||||||
|
mjtNum* tendon_actfrcrange; // range of total actuator force (ntendon x 2)
|
||||||
mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1)
|
mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1)
|
||||||
mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1)
|
mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1)
|
||||||
mjtNum* tendon_damping; // damping coefficient (ntendon x 1)
|
mjtNum* tendon_damping; // damping coefficient (ntendon x 1)
|
||||||
|
|||||||
@@ -628,7 +628,9 @@ typedef struct mjsTendon_ { // tendon specification
|
|||||||
|
|
||||||
// length range
|
// length range
|
||||||
int limited; // does tendon have limits (mjtLimited)
|
int limited; // does tendon have limits (mjtLimited)
|
||||||
|
int actfrclimited; // does tendon have actuator force limits
|
||||||
double range[2]; // length limits
|
double range[2]; // length limits
|
||||||
|
double actfrcrange[2]; // actuator force limits
|
||||||
double margin; // margin value for tendon limit detection
|
double margin; // margin value for tendon limit detection
|
||||||
mjtNum solref_limit[mjNREF]; // solver reference: tendon limits
|
mjtNum solref_limit[mjNREF]; // solver reference: tendon limits
|
||||||
mjtNum solimp_limit[mjNIMP]; // solver impedance: tendon limits
|
mjtNum solimp_limit[mjNIMP]; // solver impedance: tendon limits
|
||||||
|
|||||||
@@ -593,8 +593,10 @@ struct mjvSceneState_ {
|
|||||||
int* tendon_matid;
|
int* tendon_matid;
|
||||||
int* tendon_group;
|
int* tendon_group;
|
||||||
mjtByte* tendon_limited;
|
mjtByte* tendon_limited;
|
||||||
|
mjtByte* tendon_actfrclimited;
|
||||||
mjtNum* tendon_width;
|
mjtNum* tendon_width;
|
||||||
mjtNum* tendon_range;
|
mjtNum* tendon_range;
|
||||||
|
mjtNum* tendon_actfrcrange;
|
||||||
mjtNum* tendon_stiffness;
|
mjtNum* tendon_stiffness;
|
||||||
mjtNum* tendon_damping;
|
mjtNum* tendon_damping;
|
||||||
mjtNum* tendon_frictionloss;
|
mjtNum* tendon_frictionloss;
|
||||||
|
|||||||
@@ -475,12 +475,14 @@
|
|||||||
XMJV( int, tendon_matid, ntendon, 1 ) \
|
XMJV( int, tendon_matid, ntendon, 1 ) \
|
||||||
XMJV( int, tendon_group, ntendon, 1 ) \
|
XMJV( int, tendon_group, ntendon, 1 ) \
|
||||||
XMJV( mjtByte, tendon_limited, ntendon, 1 ) \
|
XMJV( mjtByte, tendon_limited, ntendon, 1 ) \
|
||||||
|
XMJV( mjtByte, tendon_actfrclimited, ntendon, 1 ) \
|
||||||
XMJV( mjtNum, tendon_width, ntendon, 1 ) \
|
XMJV( mjtNum, tendon_width, ntendon, 1 ) \
|
||||||
X ( mjtNum, tendon_solref_lim, ntendon, mjNREF ) \
|
X ( mjtNum, tendon_solref_lim, ntendon, mjNREF ) \
|
||||||
X ( mjtNum, tendon_solimp_lim, ntendon, mjNIMP ) \
|
X ( mjtNum, tendon_solimp_lim, ntendon, mjNIMP ) \
|
||||||
X ( mjtNum, tendon_solref_fri, ntendon, mjNREF ) \
|
X ( mjtNum, tendon_solref_fri, ntendon, mjNREF ) \
|
||||||
X ( mjtNum, tendon_solimp_fri, ntendon, mjNIMP ) \
|
X ( mjtNum, tendon_solimp_fri, ntendon, mjNIMP ) \
|
||||||
XMJV( mjtNum, tendon_range, ntendon, 2 ) \
|
XMJV( mjtNum, tendon_range, ntendon, 2 ) \
|
||||||
|
XMJV( mjtNum, tendon_actfrcrange, ntendon, 2 ) \
|
||||||
X ( mjtNum, tendon_margin, ntendon, 1 ) \
|
X ( mjtNum, tendon_margin, ntendon, 1 ) \
|
||||||
XMJV( mjtNum, tendon_stiffness, ntendon, 1 ) \
|
XMJV( mjtNum, tendon_stiffness, ntendon, 1 ) \
|
||||||
XMJV( mjtNum, tendon_damping, ntendon, 1 ) \
|
XMJV( mjtNum, tendon_damping, ntendon, 1 ) \
|
||||||
|
|||||||
@@ -349,7 +349,7 @@ class SupportTest(parameterized.TestCase):
|
|||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
str(e.exception),
|
str(e.exception),
|
||||||
'mjSpec signature does not match mjx.Model signature:'
|
'mjSpec signature does not match mjx.Model signature:'
|
||||||
' 4300287342280373816 != 9887180086914550999',
|
' 17856615236057737915 != 12517827274439268436',
|
||||||
)
|
)
|
||||||
|
|
||||||
_CONTACTS = """
|
_CONTACTS = """
|
||||||
|
|||||||
@@ -763,11 +763,13 @@ class Model(PyTreeNode):
|
|||||||
tendon_adr: address of first object in tendon's path (ntendon,)
|
tendon_adr: address of first object in tendon's path (ntendon,)
|
||||||
tendon_num: number of objects in tendon's path (ntendon,)
|
tendon_num: number of objects in tendon's path (ntendon,)
|
||||||
tendon_limited: does tendon have length limits (ntendon,)
|
tendon_limited: does tendon have length limits (ntendon,)
|
||||||
|
tendon_actfrclimited: tendon has actuator force limits (ntendon,)
|
||||||
tendon_solref_lim: constraint solver reference: limit (ntendon, mjNREF)
|
tendon_solref_lim: constraint solver reference: limit (ntendon, mjNREF)
|
||||||
tendon_solimp_lim: constraint solver impedance: limit (ntendon, mjNIMP)
|
tendon_solimp_lim: constraint solver impedance: limit (ntendon, mjNIMP)
|
||||||
tendon_solref_fri: constraint solver reference: friction (ntendon, mjNREF)
|
tendon_solref_fri: constraint solver reference: friction (ntendon, mjNREF)
|
||||||
tendon_solimp_fri: constraint solver impedance: friction (ntendon, mjNIMP)
|
tendon_solimp_fri: constraint solver impedance: friction (ntendon, mjNIMP)
|
||||||
tendon_range: tendon length limits (ntendon, 2)
|
tendon_range: tendon length limits (ntendon, 2)
|
||||||
|
tendon_actfrcrange: tendon actuator force limits (ntendon, 2)
|
||||||
tendon_margin: min distance for limit detection (ntendon,)
|
tendon_margin: min distance for limit detection (ntendon,)
|
||||||
tendon_stiffness: stiffness coefficient (ntendon,)
|
tendon_stiffness: stiffness coefficient (ntendon,)
|
||||||
tendon_damping: damping coefficient (ntendon,)
|
tendon_damping: damping coefficient (ntendon,)
|
||||||
@@ -1106,11 +1108,13 @@ class Model(PyTreeNode):
|
|||||||
tendon_adr: np.ndarray
|
tendon_adr: np.ndarray
|
||||||
tendon_num: np.ndarray
|
tendon_num: np.ndarray
|
||||||
tendon_limited: np.ndarray
|
tendon_limited: np.ndarray
|
||||||
|
tendon_actfrclimited: np.ndarray
|
||||||
tendon_solref_lim: jax.Array
|
tendon_solref_lim: jax.Array
|
||||||
tendon_solimp_lim: jax.Array
|
tendon_solimp_lim: jax.Array
|
||||||
tendon_solref_fri: jax.Array
|
tendon_solref_fri: jax.Array
|
||||||
tendon_solimp_fri: jax.Array
|
tendon_solimp_fri: jax.Array
|
||||||
tendon_range: jax.Array
|
tendon_range: jax.Array
|
||||||
|
tendon_actfrcrange: jax.Array
|
||||||
tendon_margin: jax.Array
|
tendon_margin: jax.Array
|
||||||
tendon_stiffness: jax.Array
|
tendon_stiffness: jax.Array
|
||||||
tendon_damping: jax.Array
|
tendon_damping: jax.Array
|
||||||
|
|||||||
@@ -340,34 +340,35 @@ ENUMS: Mapping[str, EnumDecl] = dict([
|
|||||||
('mjSENS_ACTUATORVEL', 14),
|
('mjSENS_ACTUATORVEL', 14),
|
||||||
('mjSENS_ACTUATORFRC', 15),
|
('mjSENS_ACTUATORFRC', 15),
|
||||||
('mjSENS_JOINTACTFRC', 16),
|
('mjSENS_JOINTACTFRC', 16),
|
||||||
('mjSENS_BALLQUAT', 17),
|
('mjSENS_TENDONACTFRC', 17),
|
||||||
('mjSENS_BALLANGVEL', 18),
|
('mjSENS_BALLQUAT', 18),
|
||||||
('mjSENS_JOINTLIMITPOS', 19),
|
('mjSENS_BALLANGVEL', 19),
|
||||||
('mjSENS_JOINTLIMITVEL', 20),
|
('mjSENS_JOINTLIMITPOS', 20),
|
||||||
('mjSENS_JOINTLIMITFRC', 21),
|
('mjSENS_JOINTLIMITVEL', 21),
|
||||||
('mjSENS_TENDONLIMITPOS', 22),
|
('mjSENS_JOINTLIMITFRC', 22),
|
||||||
('mjSENS_TENDONLIMITVEL', 23),
|
('mjSENS_TENDONLIMITPOS', 23),
|
||||||
('mjSENS_TENDONLIMITFRC', 24),
|
('mjSENS_TENDONLIMITVEL', 24),
|
||||||
('mjSENS_FRAMEPOS', 25),
|
('mjSENS_TENDONLIMITFRC', 25),
|
||||||
('mjSENS_FRAMEQUAT', 26),
|
('mjSENS_FRAMEPOS', 26),
|
||||||
('mjSENS_FRAMEXAXIS', 27),
|
('mjSENS_FRAMEQUAT', 27),
|
||||||
('mjSENS_FRAMEYAXIS', 28),
|
('mjSENS_FRAMEXAXIS', 28),
|
||||||
('mjSENS_FRAMEZAXIS', 29),
|
('mjSENS_FRAMEYAXIS', 29),
|
||||||
('mjSENS_FRAMELINVEL', 30),
|
('mjSENS_FRAMEZAXIS', 30),
|
||||||
('mjSENS_FRAMEANGVEL', 31),
|
('mjSENS_FRAMELINVEL', 31),
|
||||||
('mjSENS_FRAMELINACC', 32),
|
('mjSENS_FRAMEANGVEL', 32),
|
||||||
('mjSENS_FRAMEANGACC', 33),
|
('mjSENS_FRAMELINACC', 33),
|
||||||
('mjSENS_SUBTREECOM', 34),
|
('mjSENS_FRAMEANGACC', 34),
|
||||||
('mjSENS_SUBTREELINVEL', 35),
|
('mjSENS_SUBTREECOM', 35),
|
||||||
('mjSENS_SUBTREEANGMOM', 36),
|
('mjSENS_SUBTREELINVEL', 36),
|
||||||
('mjSENS_GEOMDIST', 37),
|
('mjSENS_SUBTREEANGMOM', 37),
|
||||||
('mjSENS_GEOMNORMAL', 38),
|
('mjSENS_GEOMDIST', 38),
|
||||||
('mjSENS_GEOMFROMTO', 39),
|
('mjSENS_GEOMNORMAL', 39),
|
||||||
('mjSENS_E_POTENTIAL', 40),
|
('mjSENS_GEOMFROMTO', 40),
|
||||||
('mjSENS_E_KINETIC', 41),
|
('mjSENS_E_POTENTIAL', 41),
|
||||||
('mjSENS_CLOCK', 42),
|
('mjSENS_E_KINETIC', 42),
|
||||||
('mjSENS_PLUGIN', 43),
|
('mjSENS_CLOCK', 43),
|
||||||
('mjSENS_USER', 44),
|
('mjSENS_PLUGIN', 44),
|
||||||
|
('mjSENS_USER', 45),
|
||||||
]),
|
]),
|
||||||
)),
|
)),
|
||||||
('mjtStage',
|
('mjtStage',
|
||||||
|
|||||||
@@ -3615,6 +3615,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
doc='does tendon have length limits',
|
doc='does tendon have length limits',
|
||||||
array_extent=('ntendon',),
|
array_extent=('ntendon',),
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='tendon_actfrclimited',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='mjtByte'),
|
||||||
|
),
|
||||||
|
doc='does tendon have actuator force limits',
|
||||||
|
array_extent=('ntendon',),
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='tendon_width',
|
name='tendon_width',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -3663,6 +3671,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
doc='tendon length limits',
|
doc='tendon length limits',
|
||||||
array_extent=('ntendon', 2),
|
array_extent=('ntendon', 2),
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='tendon_actfrcrange',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='mjtNum'),
|
||||||
|
),
|
||||||
|
doc='range of total actuator force',
|
||||||
|
array_extent=('ntendon', 2),
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='tendon_margin',
|
name='tendon_margin',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -8135,6 +8151,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='',
|
doc='',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='tendon_actfrclimited',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='mjtByte'),
|
||||||
|
),
|
||||||
|
doc='',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='tendon_width',
|
name='tendon_width',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -8149,6 +8172,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='',
|
doc='',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='tendon_actfrcrange',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='mjtNum'),
|
||||||
|
),
|
||||||
|
doc='',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='tendon_stiffness',
|
name='tendon_stiffness',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -11342,6 +11372,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=ValueType(name='int'),
|
type=ValueType(name='int'),
|
||||||
doc='does tendon have limits (mjtLimited)',
|
doc='does tendon have limits (mjtLimited)',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='actfrclimited',
|
||||||
|
type=ValueType(name='int'),
|
||||||
|
doc='does tendon have actuator force limits',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='range',
|
name='range',
|
||||||
type=ArrayType(
|
type=ArrayType(
|
||||||
@@ -11350,6 +11385,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='length limits',
|
doc='length limits',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='actfrcrange',
|
||||||
|
type=ArrayType(
|
||||||
|
inner_type=ValueType(name='double'),
|
||||||
|
extents=(2,),
|
||||||
|
),
|
||||||
|
doc='actuator force limits',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='margin',
|
name='margin',
|
||||||
type=ValueType(name='double'),
|
type=ValueType(name='double'),
|
||||||
|
|||||||
@@ -276,7 +276,7 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, i
|
|||||||
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
|
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
|
||||||
void mj_fwdActuation(const mjModel* m, mjData* d) {
|
void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||||
TM_START;
|
TM_START;
|
||||||
int nv = m->nv, nu = m->nu;
|
int nv = m->nv, nu = m->nu, ntendon = m->ntendon;
|
||||||
mjtNum gain, bias, tau;
|
mjtNum gain, bias, tau;
|
||||||
mjtNum *prm, *force = d->actuator_force;
|
mjtNum *prm, *force = d->actuator_force;
|
||||||
|
|
||||||
@@ -289,6 +289,9 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// any tendon transmission targets with force limits
|
||||||
|
int tendon_frclimited = 0;
|
||||||
|
|
||||||
// local, clamped copy of ctrl
|
// local, clamped copy of ctrl
|
||||||
mj_markStack(d);
|
mj_markStack(d);
|
||||||
mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum);
|
mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum);
|
||||||
@@ -384,6 +387,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// check for tendon transmission with force limits
|
||||||
|
if (ntendon && !tendon_frclimited && m->actuator_trntype[i] == mjTRN_TENDON) {
|
||||||
|
tendon_frclimited = m->tendon_actfrclimited[m->actuator_trnid[2*i]];
|
||||||
|
}
|
||||||
|
|
||||||
// extract gain info
|
// extract gain info
|
||||||
prm = m->actuator_gainprm + mjNGAIN*i;
|
prm = m->actuator_gainprm + mjNGAIN*i;
|
||||||
|
|
||||||
@@ -479,6 +487,38 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// clamp tendon total actuator force
|
||||||
|
if (tendon_frclimited) {
|
||||||
|
// compute total force for each tendon
|
||||||
|
mjtNum* tendon_total_force = mjSTACKALLOC(d, ntendon, mjtNum);
|
||||||
|
mju_zero(tendon_total_force, ntendon);
|
||||||
|
for (int i=0; i < nu; i++) {
|
||||||
|
if (m->actuator_trntype[i] == mjTRN_TENDON) {
|
||||||
|
int tendon_id = m->actuator_trnid[2*i];
|
||||||
|
if (m->tendon_actfrclimited[tendon_id]) {
|
||||||
|
tendon_total_force[tendon_id] += force[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// scale tendon actuator forces if limited and outside range
|
||||||
|
for (int i=0; i < nu; i++) {
|
||||||
|
if (m->actuator_trntype[i] != mjTRN_TENDON) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
int tendon_id = m->actuator_trnid[2*i];
|
||||||
|
mjtNum tendon_force = tendon_total_force[tendon_id];
|
||||||
|
if (m->tendon_actfrclimited[tendon_id] && tendon_force) {
|
||||||
|
const mjtNum* range = m->tendon_actfrcrange + 2 * tendon_id;
|
||||||
|
if (tendon_force < range[0]) {
|
||||||
|
force[i] *= range[0] / tendon_force;
|
||||||
|
} else if (tendon_force > range[1]) {
|
||||||
|
force[i] *= range[1] / tendon_force;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// clamp actuator_force
|
// clamp actuator_force
|
||||||
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
|
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
|
||||||
|
|
||||||
|
|||||||
@@ -2079,6 +2079,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
|
|||||||
case mjSENS_ACTUATORVEL:
|
case mjSENS_ACTUATORVEL:
|
||||||
case mjSENS_ACTUATORFRC:
|
case mjSENS_ACTUATORFRC:
|
||||||
case mjSENS_JOINTACTFRC:
|
case mjSENS_JOINTACTFRC:
|
||||||
|
case mjSENS_TENDONACTFRC:
|
||||||
case mjSENS_JOINTLIMITPOS:
|
case mjSENS_JOINTLIMITPOS:
|
||||||
case mjSENS_JOINTLIMITVEL:
|
case mjSENS_JOINTLIMITVEL:
|
||||||
case mjSENS_JOINTLIMITFRC:
|
case mjSENS_JOINTLIMITFRC:
|
||||||
|
|||||||
@@ -698,8 +698,8 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
|
|||||||
// acceleration/force-dependent sensors
|
// acceleration/force-dependent sensors
|
||||||
void mj_sensorAcc(const mjModel* m, mjData* d) {
|
void mj_sensorAcc(const mjModel* m, mjData* d) {
|
||||||
int rootid, bodyid, objtype, objid, adr, nusersensor = 0;
|
int rootid, bodyid, objtype, objid, adr, nusersensor = 0;
|
||||||
int ne = d->ne, nf = d->nf, nefc = d->nefc;
|
int ne = d->ne, nf = d->nf, nefc = d->nefc, nu = m->nu;
|
||||||
mjtNum tmp[6], conforce[6], conray[3];
|
mjtNum tmp[6], conforce[6], conray[3], frc;
|
||||||
mjContact* con;
|
mjContact* con;
|
||||||
|
|
||||||
// disabled sensors: return
|
// disabled sensors: return
|
||||||
@@ -825,6 +825,16 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
|
|||||||
d->sensordata[adr] = d->qfrc_actuator[m->jnt_dofadr[objid]];
|
d->sensordata[adr] = d->qfrc_actuator[m->jnt_dofadr[objid]];
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case mjSENS_TENDONACTFRC: // tendonactfrc
|
||||||
|
frc = 0.0;
|
||||||
|
for (int j=0; j < nu; j++) {
|
||||||
|
if (m->actuator_trntype[j] == mjTRN_TENDON && m->actuator_trnid[2*j] == objid) {
|
||||||
|
frc += d->actuator_force[j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
d->sensordata[adr] = frc;
|
||||||
|
break;
|
||||||
|
|
||||||
case mjSENS_JOINTLIMITFRC: // jointlimitfrc
|
case mjSENS_JOINTLIMITFRC: // jointlimitfrc
|
||||||
d->sensordata[adr] = 0;
|
d->sensordata[adr] = 0;
|
||||||
for (int j=ne+nf; j < nefc; j++) {
|
for (int j=ne+nf; j < nefc; j++) {
|
||||||
|
|||||||
@@ -3351,6 +3351,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
m->tendon_matid[i] = pte->matid;
|
m->tendon_matid[i] = pte->matid;
|
||||||
m->tendon_group[i] = pte->group;
|
m->tendon_group[i] = pte->group;
|
||||||
m->tendon_limited[i] = (mjtByte)pte->is_limited();
|
m->tendon_limited[i] = (mjtByte)pte->is_limited();
|
||||||
|
m->tendon_actfrclimited[i] = (mjtByte)pte->is_actfrclimited();
|
||||||
m->tendon_width[i] = (mjtNum)pte->width;
|
m->tendon_width[i] = (mjtNum)pte->width;
|
||||||
mjuu_copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF);
|
mjuu_copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF);
|
||||||
mjuu_copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP);
|
mjuu_copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP);
|
||||||
@@ -3358,6 +3359,8 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
mjuu_copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP);
|
mjuu_copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP);
|
||||||
m->tendon_range[2*i] = (mjtNum)pte->range[0];
|
m->tendon_range[2*i] = (mjtNum)pte->range[0];
|
||||||
m->tendon_range[2*i+1] = (mjtNum)pte->range[1];
|
m->tendon_range[2*i+1] = (mjtNum)pte->range[1];
|
||||||
|
m->tendon_actfrcrange[2*i] = (mjtNum)pte->actfrcrange[0];
|
||||||
|
m->tendon_actfrcrange[2*i+1] = (mjtNum)pte->actfrcrange[1];
|
||||||
m->tendon_margin[i] = (mjtNum)pte->margin;
|
m->tendon_margin[i] = (mjtNum)pte->margin;
|
||||||
m->tendon_stiffness[i] = (mjtNum)pte->stiffness;
|
m->tendon_stiffness[i] = (mjtNum)pte->stiffness;
|
||||||
m->tendon_damping[i] = (mjtNum)pte->damping;
|
m->tendon_damping[i] = (mjtNum)pte->damping;
|
||||||
@@ -5005,6 +5008,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
// tendons
|
// tendons
|
||||||
for (int i=0; i < ntendon; i++) {
|
for (int i=0; i < ntendon; i++) {
|
||||||
mjuu_copyvec(tendons_[i]->range, m->tendon_range+2*i, 2);
|
mjuu_copyvec(tendons_[i]->range, m->tendon_range+2*i, 2);
|
||||||
|
mjuu_copyvec(tendons_[i]->actfrcrange, m->tendon_actfrcrange+2*i, 2);
|
||||||
mjuu_copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF);
|
mjuu_copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF);
|
||||||
mjuu_copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP);
|
mjuu_copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP);
|
||||||
mjuu_copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF);
|
mjuu_copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF);
|
||||||
|
|||||||
@@ -5388,7 +5388,9 @@ mjCTendon& mjCTendon::operator=(const mjCTendon& other) {
|
|||||||
bool mjCTendon::is_limited() const {
|
bool mjCTendon::is_limited() const {
|
||||||
return islimited(limited, range);
|
return islimited(limited, range);
|
||||||
}
|
}
|
||||||
|
bool mjCTendon::is_actfrclimited() const {
|
||||||
|
return islimited(actfrclimited, actfrcrange);
|
||||||
|
}
|
||||||
|
|
||||||
void mjCTendon::PointToLocal() {
|
void mjCTendon::PointToLocal() {
|
||||||
spec.element = static_cast<mjsElement*>(this);
|
spec.element = static_cast<mjsElement*>(this);
|
||||||
@@ -5686,6 +5688,21 @@ void mjCTendon::Compile(void) {
|
|||||||
throw mjCError(this, "invalid limits in tendon");
|
throw mjCError(this, "invalid limits in tendon");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// if limited is auto, set to 1 if range is specified, otherwise unlimited
|
||||||
|
if (actfrclimited == mjLIMITED_AUTO) {
|
||||||
|
bool hasactfrcrange = !(actfrcrange[0] == 0 && actfrcrange[1] == 0);
|
||||||
|
checklimited(this, compiler->autolimits, "tendon", "", actfrclimited,
|
||||||
|
hasactfrcrange);
|
||||||
|
}
|
||||||
|
|
||||||
|
// check actfrclimits
|
||||||
|
if (actfrcrange[0] >= actfrcrange[1] && is_actfrclimited()) {
|
||||||
|
throw mjCError(this, "invalid actuatorfrcrange in tendon");
|
||||||
|
}
|
||||||
|
if ((actfrcrange[0] > 0 || actfrcrange[1] < 0) && is_actfrclimited()) {
|
||||||
|
throw mjCError(this, "invalid actuatorfrcrange in tendon");
|
||||||
|
}
|
||||||
|
|
||||||
// check springlength
|
// check springlength
|
||||||
if (springlength[0] > springlength[1]) {
|
if (springlength[0] > springlength[1]) {
|
||||||
throw mjCError(this, "invalid springlength in tendon");
|
throw mjCError(this, "invalid springlength in tendon");
|
||||||
@@ -6479,6 +6496,18 @@ void mjCSensor::Compile(void) {
|
|||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case mjSENS_TENDONACTFRC:
|
||||||
|
// must be attached to tendon
|
||||||
|
if (objtype != mjOBJ_TENDON) {
|
||||||
|
throw mjCError(this, "sensor must be attached to tendon");
|
||||||
|
}
|
||||||
|
|
||||||
|
// set
|
||||||
|
dim = 1;
|
||||||
|
datatype = mjDATATYPE_REAL;
|
||||||
|
needstage = mjSTAGE_ACC;
|
||||||
|
break;
|
||||||
|
|
||||||
case mjSENS_TENDONPOS:
|
case mjSENS_TENDONPOS:
|
||||||
case mjSENS_TENDONVEL:
|
case mjSENS_TENDONVEL:
|
||||||
// must be attached to tendon
|
// must be attached to tendon
|
||||||
|
|||||||
@@ -1523,6 +1523,7 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
|
|||||||
void SetModel(mjCModel* _model);
|
void SetModel(mjCModel* _model);
|
||||||
|
|
||||||
bool is_limited() const;
|
bool is_limited() const;
|
||||||
|
bool is_actfrclimited() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void Compile(void); // compiler
|
void Compile(void); // compiler
|
||||||
|
|||||||
@@ -363,8 +363,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
|
|||||||
|
|
||||||
{"tendon", "*", "0"},
|
{"tendon", "*", "0"},
|
||||||
{"<"},
|
{"<"},
|
||||||
{"spatial", "*", "19", "name", "class", "group", "limited", "range",
|
{"spatial", "*", "21", "name", "class", "group", "limited", "actuatorfrclimited", "range",
|
||||||
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
|
"actuatorfrcrange", "solreflimit", "solimplimit", "solreffriction", "solimpfriction",
|
||||||
"frictionloss", "springlength", "width", "material",
|
"frictionloss", "springlength", "width", "material",
|
||||||
"margin", "stiffness", "damping", "armature", "rgba", "user"},
|
"margin", "stiffness", "damping", "armature", "rgba", "user"},
|
||||||
{"<"},
|
{"<"},
|
||||||
@@ -372,8 +372,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
|
|||||||
{"geom", "*", "2", "geom", "sidesite"},
|
{"geom", "*", "2", "geom", "sidesite"},
|
||||||
{"pulley", "*", "1", "divisor"},
|
{"pulley", "*", "1", "divisor"},
|
||||||
{">"},
|
{">"},
|
||||||
{"fixed", "*", "16", "name", "class", "group", "limited", "range",
|
{"fixed", "*", "18", "name", "class", "group", "limited", "actuatorfrclimited", "range",
|
||||||
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
|
"actuatorfrcrange","solreflimit", "solimplimit", "solreffriction", "solimpfriction",
|
||||||
"frictionloss", "springlength", "margin", "stiffness", "damping", "armature", "user"},
|
"frictionloss", "springlength", "margin", "stiffness", "damping", "armature", "user"},
|
||||||
{"<"},
|
{"<"},
|
||||||
{"joint", "*", "2", "joint", "coef"},
|
{"joint", "*", "2", "joint", "coef"},
|
||||||
@@ -455,6 +455,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
|
|||||||
{"actuatorvel", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
|
{"actuatorvel", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
|
||||||
{"actuatorfrc", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
|
{"actuatorfrc", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
|
||||||
{"jointactuatorfrc", "*", "5", "name", "joint", "cutoff", "noise", "user"},
|
{"jointactuatorfrc", "*", "5", "name", "joint", "cutoff", "noise", "user"},
|
||||||
|
{"tendonactuatorfrc", "*", "5", "name", "tendon", "cutoff", "noise", "user"},
|
||||||
{"ballquat", "*", "5", "name", "joint", "cutoff", "noise", "user"},
|
{"ballquat", "*", "5", "name", "joint", "cutoff", "noise", "user"},
|
||||||
{"ballangvel", "*", "5", "name", "joint", "cutoff", "noise", "user"},
|
{"ballangvel", "*", "5", "name", "joint", "cutoff", "noise", "user"},
|
||||||
{"jointlimitpos", "*", "5", "name", "joint", "cutoff", "noise", "user"},
|
{"jointlimitpos", "*", "5", "name", "joint", "cutoff", "noise", "user"},
|
||||||
@@ -2052,12 +2053,14 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) {
|
|||||||
mjs_setString(tendon->material, material.c_str());
|
mjs_setString(tendon->material, material.c_str());
|
||||||
}
|
}
|
||||||
MapValue(elem, "limited", &tendon->limited, TFAuto_map, 3);
|
MapValue(elem, "limited", &tendon->limited, TFAuto_map, 3);
|
||||||
|
MapValue(elem, "actuatorfrclimited", &tendon->actfrclimited, TFAuto_map, 3);
|
||||||
ReadAttr(elem, "width", 1, &tendon->width, text);
|
ReadAttr(elem, "width", 1, &tendon->width, text);
|
||||||
ReadAttr(elem, "solreflimit", mjNREF, tendon->solref_limit, text, false, false);
|
ReadAttr(elem, "solreflimit", mjNREF, tendon->solref_limit, text, false, false);
|
||||||
ReadAttr(elem, "solimplimit", mjNIMP, tendon->solimp_limit, text, false, false);
|
ReadAttr(elem, "solimplimit", mjNIMP, tendon->solimp_limit, text, false, false);
|
||||||
ReadAttr(elem, "solreffriction", mjNREF, tendon->solref_friction, text, false, false);
|
ReadAttr(elem, "solreffriction", mjNREF, tendon->solref_friction, text, false, false);
|
||||||
ReadAttr(elem, "solimpfriction", mjNIMP, tendon->solimp_friction, text, false, false);
|
ReadAttr(elem, "solimpfriction", mjNIMP, tendon->solimp_friction, text, false, false);
|
||||||
ReadAttr(elem, "range", 2, tendon->range, text);
|
ReadAttr(elem, "range", 2, tendon->range, text);
|
||||||
|
ReadAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, text);
|
||||||
ReadAttr(elem, "margin", 1, &tendon->margin, text);
|
ReadAttr(elem, "margin", 1, &tendon->margin, text);
|
||||||
ReadAttr(elem, "stiffness", 1, &tendon->stiffness, text);
|
ReadAttr(elem, "stiffness", 1, &tendon->stiffness, text);
|
||||||
ReadAttr(elem, "damping", 1, &tendon->damping, text);
|
ReadAttr(elem, "damping", 1, &tendon->damping, text);
|
||||||
@@ -3973,6 +3976,10 @@ void mjXReader::Sensor(XMLElement* section) {
|
|||||||
sensor->type = mjSENS_JOINTACTFRC;
|
sensor->type = mjSENS_JOINTACTFRC;
|
||||||
sensor->objtype = mjOBJ_JOINT;
|
sensor->objtype = mjOBJ_JOINT;
|
||||||
ReadAttrTxt(elem, "joint", objname, true);
|
ReadAttrTxt(elem, "joint", objname, true);
|
||||||
|
} else if (type=="tendonactuatorfrc") {
|
||||||
|
sensor->type = mjSENS_TENDONACTFRC;
|
||||||
|
sensor->objtype = mjOBJ_TENDON;
|
||||||
|
ReadAttrTxt(elem, "tendon", objname, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
// sensors related to ball joints
|
// sensors related to ball joints
|
||||||
|
|||||||
@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// MJCF schema
|
// MJCF schema
|
||||||
#define nMJCF 237
|
#define nMJCF 238
|
||||||
extern const char* MJCF[nMJCF][mjXATTRNUM];
|
extern const char* MJCF[nMJCF][mjXATTRNUM];
|
||||||
|
|
||||||
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
|
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
|
||||||
|
|||||||
@@ -725,7 +725,9 @@ void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def
|
|||||||
WriteAttr(elem, "solimpfriction", mjNIMP, tendon->solimp_friction, def->Tendon().solimp_friction,
|
WriteAttr(elem, "solimpfriction", mjNIMP, tendon->solimp_friction, def->Tendon().solimp_friction,
|
||||||
true);
|
true);
|
||||||
WriteAttrKey(elem, "limited", TFAuto_map, 3, tendon->limited, def->Tendon().limited);
|
WriteAttrKey(elem, "limited", TFAuto_map, 3, tendon->limited, def->Tendon().limited);
|
||||||
|
WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, tendon->actfrclimited, def->Tendon().actfrclimited);
|
||||||
WriteAttr(elem, "range", 2, tendon->range, def->Tendon().range);
|
WriteAttr(elem, "range", 2, tendon->range, def->Tendon().range);
|
||||||
|
WriteAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, def->Tendon().actfrcrange);
|
||||||
WriteAttr(elem, "margin", 1, &tendon->margin, &def->Tendon().margin);
|
WriteAttr(elem, "margin", 1, &tendon->margin, &def->Tendon().margin);
|
||||||
WriteAttr(elem, "stiffness", 1, &tendon->stiffness, &def->Tendon().stiffness);
|
WriteAttr(elem, "stiffness", 1, &tendon->stiffness, &def->Tendon().stiffness);
|
||||||
WriteAttr(elem, "damping", 1, &tendon->damping, &def->Tendon().damping);
|
WriteAttr(elem, "damping", 1, &tendon->damping, &def->Tendon().damping);
|
||||||
@@ -2033,6 +2035,10 @@ void mjXWriter::Sensor(XMLElement* root) {
|
|||||||
elem = InsertEnd(section, "jointactuatorfrc");
|
elem = InsertEnd(section, "jointactuatorfrc");
|
||||||
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
||||||
break;
|
break;
|
||||||
|
case mjSENS_TENDONACTFRC:
|
||||||
|
elem = InsertEnd(section, "tendonactuatorfrc");
|
||||||
|
WriteAttrTxt(elem, "tendon", sensor->get_objname());
|
||||||
|
break;
|
||||||
|
|
||||||
// sensors related to ball joints
|
// sensors related to ball joints
|
||||||
case mjSENS_BALLQUAT:
|
case mjSENS_BALLQUAT:
|
||||||
|
|||||||
@@ -46,6 +46,8 @@ static const char* const kDampedActuatorsPath =
|
|||||||
"engine/testdata/derivative/damped_actuators.xml";
|
"engine/testdata/derivative/damped_actuators.xml";
|
||||||
static const char* const kJointForceClamp =
|
static const char* const kJointForceClamp =
|
||||||
"engine/testdata/actuation/joint_force_clamp.xml";
|
"engine/testdata/actuation/joint_force_clamp.xml";
|
||||||
|
static const char* const kTendonForceClamp =
|
||||||
|
"engine/testdata/actuation/tendon_force_clamp.xml";
|
||||||
|
|
||||||
using ::testing::Pointwise;
|
using ::testing::Pointwise;
|
||||||
using ::testing::DoubleNear;
|
using ::testing::DoubleNear;
|
||||||
@@ -1393,5 +1395,57 @@ TEST_F(ActuatorTest, DisableActuatorOutOfRange) {
|
|||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST_F(ActuatorTest, TendonActuatorForceRange) {
|
||||||
|
const std::string xml_path = GetTestDataFilePath(kTendonForceClamp);
|
||||||
|
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||||
|
mjData* data = mj_makeData(model);
|
||||||
|
|
||||||
|
EXPECT_EQ(model->tendon_actfrclimited[0], 0);
|
||||||
|
EXPECT_EQ(model->tendon_actfrcrange[0], 0);
|
||||||
|
EXPECT_EQ(model->tendon_actfrcrange[1], 0);
|
||||||
|
|
||||||
|
EXPECT_EQ(model->tendon_actfrclimited[1], 1);
|
||||||
|
EXPECT_EQ(model->tendon_actfrcrange[2], -1);
|
||||||
|
EXPECT_EQ(model->tendon_actfrcrange[3], 1);
|
||||||
|
|
||||||
|
EXPECT_EQ(model->tendon_actfrclimited[2], 1);
|
||||||
|
EXPECT_EQ(model->tendon_actfrcrange[4], -10);
|
||||||
|
EXPECT_EQ(model->tendon_actfrcrange[5], 10);
|
||||||
|
|
||||||
|
EXPECT_EQ(model->tendon_actfrclimited[3], 1);
|
||||||
|
EXPECT_EQ(model->tendon_actfrcrange[6], 0);
|
||||||
|
EXPECT_EQ(model->tendon_actfrcrange[7], 1);
|
||||||
|
|
||||||
|
data->ctrl[0] = 1;
|
||||||
|
data->ctrl[1] = 1;
|
||||||
|
data->ctrl[2] = 1;
|
||||||
|
|
||||||
|
data->ctrl[3] = -1;
|
||||||
|
data->ctrl[4] = 1;
|
||||||
|
|
||||||
|
data->ctrl[5] = -20;
|
||||||
|
data->ctrl[6] = 5;
|
||||||
|
data->ctrl[7] = -5;
|
||||||
|
|
||||||
|
mj_forward(model, data);
|
||||||
|
|
||||||
|
EXPECT_NEAR(data->actuator_force[0], 1, 1e-6);
|
||||||
|
EXPECT_NEAR(data->actuator_force[1], 1, 1e-6);
|
||||||
|
EXPECT_NEAR(data->actuator_force[2], 1, 1e-6);
|
||||||
|
EXPECT_NEAR(data->actuator_force[3], -1, 1e-6);
|
||||||
|
EXPECT_NEAR(data->actuator_force[4], 1, 1e-6);
|
||||||
|
EXPECT_NEAR(data->actuator_force[5], -10, 1e-6);
|
||||||
|
EXPECT_NEAR(data->actuator_force[6], 5, 1e-6);
|
||||||
|
EXPECT_NEAR(data->actuator_force[7], -5, 1e-6);
|
||||||
|
|
||||||
|
EXPECT_EQ(data->sensordata[0], 3);
|
||||||
|
EXPECT_EQ(data->sensordata[1], 0);
|
||||||
|
EXPECT_EQ(data->sensordata[2], -10);
|
||||||
|
EXPECT_EQ(data->sensordata[3], 0);
|
||||||
|
|
||||||
|
mj_deleteData(data);
|
||||||
|
mj_deleteModel(model);
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|||||||
@@ -0,0 +1,55 @@
|
|||||||
|
<mujoco model="fixed_site">
|
||||||
|
<worldbody>
|
||||||
|
<body>
|
||||||
|
<joint name="joint0" type="hinge" axis="0 1 0"/>
|
||||||
|
<geom type="capsule" size="0.05 0.5" fromto="0 0 0 0.5 0 0"/>
|
||||||
|
<site name="site0" pos="0.25 0 0.1" size="0.025"/>
|
||||||
|
<body pos="0.5 0 0">
|
||||||
|
<joint name="joint1" type="hinge" axis="0 1 0"/>
|
||||||
|
<geom type="capsule" size="0.05 0.5" fromto="0 0 0 0.5 0 0"/>
|
||||||
|
<site name="site1" pos="0.25 0 0.1" size="0.025"/>
|
||||||
|
<body pos="0.5 0 0">
|
||||||
|
<joint name="joint2" type="hinge" axis="0 1 0"/>
|
||||||
|
<geom type="capsule" size="0.05 0.5" fromto="0 0 0 0.5 0 0"/>
|
||||||
|
<site name="site2" pos="0.25 0 0.1" size="0.025"/>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
<tendon>
|
||||||
|
<spatial name="spatial0" width="0.0125">
|
||||||
|
<site site="site0"/>
|
||||||
|
<site site="site1"/>
|
||||||
|
<site site="site2"/>
|
||||||
|
</spatial>
|
||||||
|
<spatial name="spatial1" width="0.0125" actuatorfrclimited="true" actuatorfrcrange="-1 1">
|
||||||
|
<site site="site1"/>
|
||||||
|
<site site="site2"/>
|
||||||
|
</spatial>
|
||||||
|
<fixed name="fixed0" actuatorfrclimited="true" actuatorfrcrange="-10 10">
|
||||||
|
<joint joint="joint0" coef=".1"/>
|
||||||
|
<joint joint="joint1" coef=".2"/>
|
||||||
|
<joint joint="joint2" coef=".3"/>
|
||||||
|
</fixed>
|
||||||
|
<fixed name="fixed1" actuatorfrclimited="true" actuatorfrcrange="0 1">
|
||||||
|
<joint joint="joint0" coef=".1"/>
|
||||||
|
<joint joint="joint2" coef=".3"/>
|
||||||
|
</fixed>
|
||||||
|
</tendon>
|
||||||
|
<actuator>
|
||||||
|
<motor tendon="spatial0"/>
|
||||||
|
<motor tendon="spatial0"/>
|
||||||
|
<motor tendon="spatial0"/>
|
||||||
|
<motor tendon="spatial1"/>
|
||||||
|
<motor tendon="spatial1"/>
|
||||||
|
<motor tendon="fixed0"/>
|
||||||
|
<motor tendon="fixed1"/>
|
||||||
|
<motor tendon="fixed1"/>
|
||||||
|
</actuator>
|
||||||
|
<sensor>
|
||||||
|
<tendonactuatorfrc tendon="spatial0"/>
|
||||||
|
<tendonactuatorfrc tendon="spatial1"/>
|
||||||
|
<tendonactuatorfrc tendon="fixed0"/>
|
||||||
|
<tendonactuatorfrc tendon="fixed1"/>
|
||||||
|
</sensor>
|
||||||
|
</mujoco>
|
||||||
@@ -2070,6 +2070,51 @@ TEST_F(TendonTest, SiteBetweenPulleyNotAllowed) {
|
|||||||
EXPECT_THAT(error.data(), HasSubstr("line 9"));
|
EXPECT_THAT(error.data(), HasSubstr("line 9"));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST_F(TendonTest, ActuatorForceRangeNotAllowed) {
|
||||||
|
std::string xml = R"(
|
||||||
|
<mujoco>
|
||||||
|
<worldbody>
|
||||||
|
<site name="site0"/>
|
||||||
|
<site name="site1"/>
|
||||||
|
</worldbody>
|
||||||
|
<tendon>
|
||||||
|
<spatial name="spatial" actuatorfrclimited="true" actuatorfrcrange="{}">
|
||||||
|
<site site="site0"/>
|
||||||
|
<site site="site1"/>
|
||||||
|
</spatial>
|
||||||
|
</tendon>
|
||||||
|
<actuator>
|
||||||
|
<motor tendon="spatial"/>
|
||||||
|
</actuator>
|
||||||
|
</mujoco>
|
||||||
|
)";
|
||||||
|
|
||||||
|
std::array<char, 1024> error;
|
||||||
|
std::string str_replace = "{}";
|
||||||
|
size_t rng_ind = xml.find(str_replace);
|
||||||
|
|
||||||
|
std::string xml0 = xml;
|
||||||
|
std::string range0 = "-2 -1";
|
||||||
|
xml0.replace(rng_ind, str_replace.length(), range0);
|
||||||
|
mjModel* m0 = LoadModelFromString(xml0.c_str(), error.data(), error.size());
|
||||||
|
EXPECT_THAT(m0, IsNull());
|
||||||
|
EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon"));
|
||||||
|
|
||||||
|
std::string xml1 = xml;
|
||||||
|
std::string range1 = "1 2";
|
||||||
|
xml1.replace(rng_ind, str_replace.length(), range1);
|
||||||
|
mjModel* m1 = LoadModelFromString(xml1.c_str(), error.data(), error.size());
|
||||||
|
EXPECT_THAT(m1, IsNull());
|
||||||
|
EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon"));
|
||||||
|
|
||||||
|
std::string xml2 = xml;
|
||||||
|
std::string range2 = "1 0";
|
||||||
|
xml2.replace(rng_ind, str_replace.length(), range2);
|
||||||
|
mjModel* m2 = LoadModelFromString(xml2.c_str(), error.data(), error.size());
|
||||||
|
EXPECT_THAT(m2, IsNull());
|
||||||
|
EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon"));
|
||||||
|
}
|
||||||
|
|
||||||
// ------------- tests for tendon springrange ----------------------------------
|
// ------------- tests for tendon springrange ----------------------------------
|
||||||
|
|
||||||
using SpringrangeTest = MujocoTest;
|
using SpringrangeTest = MujocoTest;
|
||||||
|
|||||||
@@ -357,34 +357,35 @@ public enum mjtSensor : int{
|
|||||||
mjSENS_ACTUATORVEL = 14,
|
mjSENS_ACTUATORVEL = 14,
|
||||||
mjSENS_ACTUATORFRC = 15,
|
mjSENS_ACTUATORFRC = 15,
|
||||||
mjSENS_JOINTACTFRC = 16,
|
mjSENS_JOINTACTFRC = 16,
|
||||||
mjSENS_BALLQUAT = 17,
|
mjSENS_TENDONACTFRC = 17,
|
||||||
mjSENS_BALLANGVEL = 18,
|
mjSENS_BALLQUAT = 18,
|
||||||
mjSENS_JOINTLIMITPOS = 19,
|
mjSENS_BALLANGVEL = 19,
|
||||||
mjSENS_JOINTLIMITVEL = 20,
|
mjSENS_JOINTLIMITPOS = 20,
|
||||||
mjSENS_JOINTLIMITFRC = 21,
|
mjSENS_JOINTLIMITVEL = 21,
|
||||||
mjSENS_TENDONLIMITPOS = 22,
|
mjSENS_JOINTLIMITFRC = 22,
|
||||||
mjSENS_TENDONLIMITVEL = 23,
|
mjSENS_TENDONLIMITPOS = 23,
|
||||||
mjSENS_TENDONLIMITFRC = 24,
|
mjSENS_TENDONLIMITVEL = 24,
|
||||||
mjSENS_FRAMEPOS = 25,
|
mjSENS_TENDONLIMITFRC = 25,
|
||||||
mjSENS_FRAMEQUAT = 26,
|
mjSENS_FRAMEPOS = 26,
|
||||||
mjSENS_FRAMEXAXIS = 27,
|
mjSENS_FRAMEQUAT = 27,
|
||||||
mjSENS_FRAMEYAXIS = 28,
|
mjSENS_FRAMEXAXIS = 28,
|
||||||
mjSENS_FRAMEZAXIS = 29,
|
mjSENS_FRAMEYAXIS = 29,
|
||||||
mjSENS_FRAMELINVEL = 30,
|
mjSENS_FRAMEZAXIS = 30,
|
||||||
mjSENS_FRAMEANGVEL = 31,
|
mjSENS_FRAMELINVEL = 31,
|
||||||
mjSENS_FRAMELINACC = 32,
|
mjSENS_FRAMEANGVEL = 32,
|
||||||
mjSENS_FRAMEANGACC = 33,
|
mjSENS_FRAMELINACC = 33,
|
||||||
mjSENS_SUBTREECOM = 34,
|
mjSENS_FRAMEANGACC = 34,
|
||||||
mjSENS_SUBTREELINVEL = 35,
|
mjSENS_SUBTREECOM = 35,
|
||||||
mjSENS_SUBTREEANGMOM = 36,
|
mjSENS_SUBTREELINVEL = 36,
|
||||||
mjSENS_GEOMDIST = 37,
|
mjSENS_SUBTREEANGMOM = 37,
|
||||||
mjSENS_GEOMNORMAL = 38,
|
mjSENS_GEOMDIST = 38,
|
||||||
mjSENS_GEOMFROMTO = 39,
|
mjSENS_GEOMNORMAL = 39,
|
||||||
mjSENS_E_POTENTIAL = 40,
|
mjSENS_GEOMFROMTO = 40,
|
||||||
mjSENS_E_KINETIC = 41,
|
mjSENS_E_POTENTIAL = 41,
|
||||||
mjSENS_CLOCK = 42,
|
mjSENS_E_KINETIC = 42,
|
||||||
mjSENS_PLUGIN = 43,
|
mjSENS_CLOCK = 43,
|
||||||
mjSENS_USER = 44,
|
mjSENS_PLUGIN = 44,
|
||||||
|
mjSENS_USER = 45,
|
||||||
}
|
}
|
||||||
public enum mjtStage : int{
|
public enum mjtStage : int{
|
||||||
mjSTAGE_NONE = 0,
|
mjSTAGE_NONE = 0,
|
||||||
@@ -5569,12 +5570,14 @@ public unsafe struct mjModel_ {
|
|||||||
public int* tendon_matid;
|
public int* tendon_matid;
|
||||||
public int* tendon_group;
|
public int* tendon_group;
|
||||||
public byte* tendon_limited;
|
public byte* tendon_limited;
|
||||||
|
public byte* tendon_actfrclimited;
|
||||||
public double* tendon_width;
|
public double* tendon_width;
|
||||||
public double* tendon_solref_lim;
|
public double* tendon_solref_lim;
|
||||||
public double* tendon_solimp_lim;
|
public double* tendon_solimp_lim;
|
||||||
public double* tendon_solref_fri;
|
public double* tendon_solref_fri;
|
||||||
public double* tendon_solimp_fri;
|
public double* tendon_solimp_fri;
|
||||||
public double* tendon_range;
|
public double* tendon_range;
|
||||||
|
public double* tendon_actfrcrange;
|
||||||
public double* tendon_margin;
|
public double* tendon_margin;
|
||||||
public double* tendon_stiffness;
|
public double* tendon_stiffness;
|
||||||
public double* tendon_damping;
|
public double* tendon_damping;
|
||||||
@@ -6398,8 +6401,10 @@ public unsafe struct model {
|
|||||||
public int* tendon_matid;
|
public int* tendon_matid;
|
||||||
public int* tendon_group;
|
public int* tendon_group;
|
||||||
public byte* tendon_limited;
|
public byte* tendon_limited;
|
||||||
|
public byte* tendon_actfrclimited;
|
||||||
public double* tendon_width;
|
public double* tendon_width;
|
||||||
public double* tendon_range;
|
public double* tendon_range;
|
||||||
|
public double* tendon_actfrcrange;
|
||||||
public double* tendon_stiffness;
|
public double* tendon_stiffness;
|
||||||
public double* tendon_damping;
|
public double* tendon_damping;
|
||||||
public double* tendon_frictionloss;
|
public double* tendon_frictionloss;
|
||||||
|
|||||||
Reference in New Issue
Block a user