From 0f37deca0dd131b34ef63c6e2935274666d2a23c Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 6 Nov 2023 05:55:15 -0800 Subject: [PATCH] Documentation improvements. Fixes #598 #691 #1071 #1072 PiperOrigin-RevId: 579816915 Change-Id: Ica5e62516e2888d47030b0f44619efa2621f329a --- doc/APIreference/functions.rst | 32 ++++++++++++++---- doc/APIreference/functions_override.rst | 44 +++++++++++++++++++++---- doc/XMLreference.rst | 23 +++++++++++-- doc/computation/index.rst | 4 +++ doc/overview.rst | 19 +++++++---- 5 files changed, 100 insertions(+), 22 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index beb45b49..afa607d1 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -392,7 +392,9 @@ mj_objectAcceleration .. mujoco-include:: mj_objectAcceleration -Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation. +Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation. If acceleration or force +sensors are not present in the model, :ref:`mj_rnePostConstraint` must be manually called in order to calculate +:ref:`mjData`.cacc -- the total body acceleration, including contributions from the constraint solver. .. _mj_contactForce: @@ -812,7 +814,10 @@ mj_subtreeVel .. mujoco-include:: mj_subtreeVel -Sub-tree linear velocity and angular momentum: compute subtree_linvel, subtree_angmom. +Sub-tree linear velocity and angular momentum: compute ``subtree_linvel``, ``subtree_angmom``. +This function is triggered automatically if the subtree :ref:`velocity` or +:ref:`momentum` sensors are present in the model. +It is also triggered for :ref:`user sensors` of :ref:`stage` "vel". .. _mj_rne: @@ -821,7 +826,8 @@ mj_rne .. mujoco-include:: mj_rne -RNE: compute M(qpos)*qacc + C(qpos,qvel); flg_acc=0 removes inertial term. +Recursive Newton Euler: compute :math:`M(q) \ddot q + C(q,\dot q)`. ``flg_acc=0`` removes the inertial term (i.e. +assumes :math:`\ddot q = 0`). .. _mj_rnePostConstraint: @@ -830,7 +836,21 @@ mj_rnePostConstraint .. mujoco-include:: mj_rnePostConstraint -RNE with complete data: compute cacc, cfrc_ext, cfrc_int. +Recursive Newton Euler with final computed forces and accelerations. +Computes three body-level ``nv x 6`` arrays, all defined in the subtreecom-based +:ref:`c-frame` and arranged in ``[rotation(3), translation(3)]`` order. + +- ``cacc``: Body acceleration, required for :ref:`mj_objectAcceleration`. +- ``cfrc_int``: Interaction force with the parent body. +- ``cfrc_ext``: External force acting on the body. + +This function is triggered automatically if the following sensors are present in the model: +:ref:`accelerometer`, :ref:`force`, :ref:`torque`, +:ref:`framelinacc`, :ref:`frameangacc`. +It is also triggered for :ref:`user sensors` of :ref:`stage` "acc". + +The computed force arrays ``cfrc_int`` and ``cfrc_ext`` currently suffer from a know bug, they do not take into account +the effect of spatial tendons, see :github:issue:`832`. .. _mj_collision: @@ -884,8 +904,8 @@ mj_constraintUpdate .. mujoco-include:: mj_constraintUpdate -Compute efc_state, efc_force, qfrc_constraint, and (optionally) cone Hessians. If cost is not NULL, set \*cost = s(jar) -where jar = Jac*qacc-aref. +Compute ``efc_state``, ``efc_force``, ``qfrc_constraint``, and (optionally) cone Hessians. +If ``cost`` is not ``NULL``, set ``*cost = s(jar)`` where ``jar = Jac*qacc - aref``. .. _Raycollisions: diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index 675cb0e2..b81199f5 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -2,10 +2,6 @@ This file contains each section text along with function doc overrides. By default the docs use the function doc pulled from the header files. -.. _Activation: - -The functions in this section are maintained for backward compatibility with the now-removed activation mechanism. - .. _Virtualfilesystem: Virtual file system (VFS) enables the user to load all necessary files in memory, including MJB binary model files, XML @@ -90,10 +86,40 @@ Solve linear system :math:`M x = y` using factorization: :math:`x = (L^T D L)^{- Half of linear solve: :math:`x = \sqrt{D^{-1}} (L^T)^{-1} y` +.. _mj_subtreeVel: + +Sub-tree linear velocity and angular momentum: compute ``subtree_linvel``, ``subtree_angmom``. +This function is triggered automatically if the subtree :ref:`velocity` or +:ref:`momentum` sensors are present in the model. +It is also triggered for :ref:`user sensors` of :ref:`stage` "vel". + +.. _mj_rne: + +Recursive Newton Euler: compute :math:`M(q) \ddot q + C(q,\dot q)`. ``flg_acc=0`` removes the inertial term (i.e. +assumes :math:`\ddot q = 0`). + +.. _mj_rnePostConstraint: + +Recursive Newton Euler with final computed forces and accelerations. +Computes three body-level ``nv x 6`` arrays, all defined in the subtreecom-based +:ref:`c-frame` and arranged in ``[rotation(3), translation(3)]`` order. + +- ``cacc``: Body acceleration, required for :ref:`mj_objectAcceleration`. +- ``cfrc_int``: Interaction force with the parent body. +- ``cfrc_ext``: External force acting on the body. + +This function is triggered automatically if the following sensors are present in the model: +:ref:`accelerometer`, :ref:`force`, :ref:`torque`, +:ref:`framelinacc`, :ref:`frameangacc`. +It is also triggered for :ref:`user sensors` of :ref:`stage` "acc". + +The computed force arrays ``cfrc_int`` and ``cfrc_ext`` currently suffer from a know bug, they do not take into account +the effect of spatial tendons, see :github:issue:`832`. + .. _mj_constraintUpdate: -Compute efc_state, efc_force, qfrc_constraint, and (optionally) cone Hessians. If cost is not NULL, set \*cost = s(jar) -where jar = Jac*qacc-aref. +Compute ``efc_state``, ``efc_force``, ``qfrc_constraint``, and (optionally) cone Hessians. +If ``cost`` is not ``NULL``, set ``*cost = s(jar)`` where ``jar = Jac*qacc - aref``. .. _Support: @@ -154,6 +180,12 @@ This function can be used to apply a Cartesian force and torque to a point on a mjData.qfrc_applied of all applied forces. Note that the function requires a pointer to this vector, because sometimes we want to add the result to a different vector. +.. _mj_objectAcceleration: + +Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation. If acceleration or force +sensors are not present in the model, :ref:`mj_rnePostConstraint` must be manually called in order to calculate +mjData.cacc -- the total body acceleration, including contributions from the constraint solver. + .. _mj_differentiatePos: This function subtracts two vectors in the format of qpos (and divides the result by dt), while respecting the diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 8b986bf5..6ff6b45d 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -5886,6 +5886,8 @@ This element creates a 3-axis accelerometer. The sensor is mounted at a site, an as the site frame. This sensor outputs three numbers, which are the linear acceleration of the site (including gravity) in local coordinates. +The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstraint` during sensor computation. + .. _sensor-accelerometer-name: .. _sensor-accelerometer-noise: @@ -5966,6 +5968,8 @@ the child body, and the force points from the child towards the parent. The comp forces acting on the system, including contacts as well as external perturbations. Using this sensor often requires creating a dummy body welded to its parent (i.e., having no joint elements). +The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstraint` during sensor computation. + .. _sensor-force-name: .. _sensor-force-noise: @@ -5995,6 +5999,8 @@ creating a dummy body welded to its parent (i.e., having no joint elements). This element creates a 3-axis torque sensor. This is similar to the :ref:`force ` sensor above, but measures torque rather than force. +The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstraint` during sensor computation. + .. _sensor-torque-name: .. _sensor-torque-noise: @@ -6798,6 +6804,8 @@ coordinates. This element creates a sensor that returns the 3D linear acceleration of the spatial frame of the object, in global coordinates. +The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstraint` during sensor computation. + .. _sensor-framelinacc-name: .. _sensor-framelinacc-noise: @@ -6828,6 +6836,8 @@ coordinates. This element creates a sensor that returns the 3D angular acceleration of the spatial frame of the object, in global coordinates. +The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstraint` during sensor computation. + .. _sensor-frameangacc-name: .. _sensor-frameangacc-noise: @@ -6883,6 +6893,8 @@ global coordinates. This element creates sensor that returns the linear velocity of the center of mass of the kinematic subtree rooted at a specified body, in global coordinates. +The presence of this sensor in a model triggers a call to :ref:`mj_subtreeVel` during sensor computation. + .. _sensor-subtreelinvel-name: .. _sensor-subtreelinvel-noise: @@ -6908,6 +6920,8 @@ specified body, in global coordinates. This element creates sensor that returns the angular momentum around the center of mass of the kinematic subtree rooted at a specified body, in global coordinates. +The presence of this sensor in a model triggers a call to :ref:`mj_subtreeVel` during sensor computation. + .. _sensor-subtreeangmom-name: .. _sensor-subtreeangmom-noise: @@ -6950,12 +6964,15 @@ This element creates sensor that returns the simulation time. ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This element creates a user sensor. MuJoCo does not know how to compute the output of this sensor. Instead the user -should install the callback :ref:`mjcb_sensor` which is expected to fill in the sensor data in mjData.sensordata. The -specification in the XML is used to allocate space for this sensor, and also determine which MuJoCo object it is +should install the callback :ref:`mjcb_sensor` which is expected to fill in the sensor data in ``mjData.sensordata``. +The specification in the XML is used to allocate space for this sensor, and also determine which MuJoCo object it is attached to and what stage of computation it needs before the data can be computed. Note that the MuJoCo object -referenced here can be a tuple, which in turn can reference a custom collection of MuJoCo objects - for example several +referenced here can be a tuple, which in turn can reference a custom collection of MuJoCo objects -- for example several bodies whose center of mass is of interest. +If a user sensor is of :ref:`stage` "vel" or "acc", then :ref:`mj_subtreeVel` and +:ref:`mj_rnePostConstraint` will be triggered, respectively. + .. _sensor-user-name: .. _sensor-user-noise: diff --git a/doc/computation/index.rst b/doc/computation/index.rst index aa0a8716..ca7c9e98 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1556,6 +1556,7 @@ The top-level function :ref:`mj_step` invokes the sequence of computations below the subtree centers of mass. #. Compute all passive forces: spring-dampers in joints and tendons, and fluid dynamics forces. #. Compute sensor data that depends on velocity, and the kinetic energy if enabled. + If required by sensors, call :ref:`mj_subtreeVel`. #. Compute the reference constraint acceleration. #. Compute the vector of Coriolis, centrifugal and gravitational forces. #. Compute the actuator forces and activation dynamics if defined. @@ -1563,6 +1564,7 @@ The top-level function :ref:`mj_step` invokes the sequence of computations below #. Compute the constraint forces with the selected solver, and update the joint acceleration so as to account for the constraint forces. This yields the vector ``mjData.qacc`` which is the main output of forward dynamics. #. Compute sensor data that depends on force and acceleration if enabled. + If required by sensors, call :ref:`mj_rnePostConstraint`. #. Check the acceleration for invalid or unacceptably large real values. If divergence is detected, the state is automatically reset and the corresponding warning is raised. #. Compare the results of forward and inverse dynamics, so as to diagnose poor solver convergence in the forward @@ -1589,6 +1591,7 @@ The top-level function :ref:`mj_inverse` invokes the following sequence of compu #. Compute the tendon and actuator velocities. #. Compute the body velocities and joint axes rates of change. #. Compute sensor data that depends on velocity, and the kinetic energy if enabled. + If required by sensors, call :ref:`mj_subtreeVel`. #. Compute all passive forces. #. Compute the reference constraint acceleration. #. If the :ref:`invdiscrete` flag is set and the :ref:`integrator` is not @@ -1596,6 +1599,7 @@ The top-level function :ref:`mj_inverse` invokes the following sequence of compu #. Compute the constraint force. This is done analytically, without using a numerical solver. #. Compute the inverse dynamics for the unconstrained system. #. Compute sensor data that depends on force and acceleration if enabled. + If required by sensors, call :ref:`mj_rnePostConstraint`. #. Compute the vector ``mjData.qfrc_inverse`` by combining all results. This is the main output of inverse dynamics. It equals the sum of external and actuation forces. diff --git a/doc/overview.rst b/doc/overview.rst index da10f6e4..7c3c574a 100644 --- a/doc/overview.rst +++ b/doc/overview.rst @@ -944,15 +944,20 @@ can be obtained as: int qposadr = -1, qveladr = -1; // make sure we have a floating body: it has a single free joint - if( bodyid>=0 && m->body_jntnum[bodyid]==1 && - m->jnt_type[m->body_jntadr[bodyid]]==mjJNT_FREE ) - { - // extract the addresses from the joint specification - qposadr = m->jnt_qposadr[m->body_jntadr[bodyid]]; - qveladr = m->jnt_dofadr[m->body_jntadr[bodyid]]; - } + if( bodyid>=0 && m->body_jntnum[bodyid]==1 && m->jnt_type[m->body_jntadr[bodyid]]==mjJNT_FREE ) { + // extract the addresses from the joint specification + qposadr = m->jnt_qposadr[m->body_jntadr[bodyid]]; + qveladr = m->jnt_dofadr[m->body_jntadr[bodyid]]; + } Now if everything went well (i.e., "myfloatingbody" was indeed a floating body), qposadr and qveladr are the addresses in qpos and qvel where the data for our floating body/joint lives. The position data is 7 numbers (3D position followed by unit quaternion) while the velocity data is 6 numbers (3D linear velocity followed by 3D angular velocity). These numbers can now be set to the desired pose and velocity of the body. + +The semantics of free joints are as follows. The linear postions of free joints are in the global frame, as are +linear velocities. The orientation of a free joint (the quaternion) is also in the global frame. However, the rotational +velocities of a free joint are in the local body frame. This is not so much a design decision but rather correct +use of the topology of quaternions. Angular velocities live in the quaternion tangent space, which is defined locally +for a certain orientation, so frame-local angular velocities are a natural parameterization. +Accelerations are defined in the same space as the corresponding velocities.