diff --git a/doc/changelog.rst b/doc/changelog.rst index d6bf60f8..b4fc3405 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -31,6 +31,11 @@ General error instead of the generic "could not decode content" message. - Added support for resource writing via :ref:`mju_writeResource` and the ``write`` callback in :ref:`mjpResourceProvider`. - Added support for :ref:`multiccd ` with arbitrarily large meshes. +- Refactored actuator infrastructure in preparation for MIMO (multi-input multi-output) actuator support. Each actuator + now has ``ctrlnum`` (number of controls) and ``outnum`` (number of force outputs). The total counts + ``nu = sum(ctrlnum)`` and ``nout = sum(outnum)`` dimension ``mjData.ctrl`` and ``mjData.actuator_force``, + respectively, ``nactuator`` is the number of actuators. For existing actuators ``ctrnum = outnum = 1``, so + ``nactuator == nu == nout`` and existing code is unaffected. .. admonition:: Breaking API changes :class: attention diff --git a/doc/computation/index.rst b/doc/computation/index.rst index a1fce90c..412495ce 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -262,12 +262,21 @@ detailed summary of the :ref:`simulation pipeline ` is given at the en Actuation model ~~~~~~~~~~~~~~~ -MuJoCo provides a flexible actuator model. All actuators are single-input-single-output (SISO). The input to actuator -:math:`i` is a scalar control :math:`u_i` specified by the user. The output is a scalar force :math:`p_i` which is -mapped to joint coordinates by a vector of moment arms determined by the transmission. An actuator can also have -activation state :math:`w_i` with its own dynamics. The control inputs for all actuators are stored in ``mjData.ctrl``, -the force outputs are stored in ``mjData.actuator_force``, and the activation states (if any) are stored in -``mjData.act``. +MuJoCo provides a flexible actuator model. Most actuators are single-input-single-output (SISO): the input to +actuator :math:`i` is a scalar control :math:`u_i` specified by the user, and the output is a scalar force +:math:`p_i`, which is mapped to joint coordinates by a vector of moment arms determined by the transmission. An +actuator can also have activation state :math:`w_i` with its own dynamics. The control inputs for all actuators are +stored in ``mjData.ctrl``, the force outputs are stored in ``mjData.actuator_force``, and the activation states (if +any) are stored in ``mjData.act``. + +More generally, an actuator can have several control inputs, several force outputs, or both. For example, a servo +which accepts both a position setpoint and a velocity setpoint has two inputs and one output. The number of inputs +is a property of the actuator type, and the number of outputs is a property of its transmission; neither is +specified directly by the user. Since the number of inputs and the number of outputs can vary per actuator, the model +has three separate counts: the number of actuators ``nactuator``, the total number of controls ``nu``, and the total +number of force outputs ``nout``. In a model where all actuators are SISO, all three are equal. Note that outputs are +counted in actuation space, before the mapping to joint coordinates: a tendon actuator with a single force output can +move many joints. These three components of an actuator -- transmission, activation dynamics, and force generation -- determine how the actuator works. The user can set them independently for maximum flexibility, or use :ref:`Actuator shortcuts @@ -278,11 +287,13 @@ actuator works. The user can set them independently for maximum flexibility, or 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:`\nv`-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 :at:`joint`, :at:`tendon`, :at:`jointinparent`, -:at:`slider-crank`, :at:`site`, and :at:`body`. +The transmission attaches the actuator to the rest of the system. Each force output of the actuator has a scalar +length :math:`l_k(q)`, defined by the type of transmission and its parameters, for example the +:ref:`gear` vector. The gradient :math:`\nabla l_k` is an :math:`\nv`-dimensional vector of +moment arms; it determines the mapping from the scalar output force to joint forces. The number of force outputs is +likewise determined by the transmission; all the types listed below define a single output. The transmission +properties are determined by the MuJoCo object to which the actuator is attached; the possible attachment object +types are :at:`joint`, :at:`tendon`, :at:`jointinparent`, :at:`slider-crank`, :at:`site`, and :at:`body`. :at:`joint` and :at:`tendon` The :at:`joint` and :at:`tendon` transmission types act as expected and correspond to the actuator applying forces or @@ -369,14 +380,10 @@ diverge for :math:`\texttt{t} < h`, while exactly-integrated filters are stable Force generation ^^^^^^^^^^^^^^^^ -Each actuator generates a scalar force :math:`p_i` which is some function - -.. math:: - p_i \left( u_i, w_i, l_i, \dot{l}_i \right) - -Similarly to activation dynamics, the force generation mechanism is actuator-specific and cannot interact with the -other actuators in the model. Currently the force is affine in the activation state when present, and in the control -otherwise: +Each actuator generates a scalar force for each of its outputs, as a function of its controls, activation states, +and lengths and velocities. Similarly to activation dynamics, force generation is actuator-specific: the inputs and +outputs of a single actuator can interact, but different actuators cannot affect each other. For SISO actuators the +force is affine in the activation state when present, and in the control otherwise: .. math:: p_i = (a w_i \; \text{or} \; a u_i) + b_0 + b_1 l_i + b_2 \dot{l}_i @@ -384,17 +391,17 @@ otherwise: Here :math:`a` is an actuator-specific gain parameter and :math:`b_0, b_1, b_2` are actuator-specific bias parameters, stored in ``mjModel.actuator_gainprm`` and ``mjModel.actuator_biasprm`` respectively. Different settings of the gain and bias parameters can be used to model direct force control as well as position and velocity servos -in -which case the control/activation has the meaning of reference position or velocity. One can also compute custom gain -and bias terms by installing the callbacks :ref:`mjcb_act_gain` and :ref:`mjcb_act_bias` and setting the gain and -bias type to "user". Note that affine force generation makes it possible to infer the controls/activations from the -applied force computed in inverse dynamics, using the pseudo-inverse of the matrix of moment arms. However some of -the actuators used in the real world are not affine (especially those that have embedded low-level controllers), so -we are considering extensions to the above model. +which case the control/activation has the meaning of reference position or velocity. Multi-input actuator types +define their own force laws. One can also compute custom gain and bias terms for SISO actuators by installing the +callbacks :ref:`mjcb_act_gain` and :ref:`mjcb_act_bias` and setting the gain and bias type to "user". Note that +affine force generation makes it possible to infer the controls/activations from the applied force computed in +inverse dynamics, using the pseudo-inverse of the matrix of moment arms. -Putting all this together, the net force in generalized coordinates contributed by all actuators is +Putting all this together, the net force in generalized coordinates contributed by all actuators is the sum over all +force outputs .. math:: - \sum_i \nabla l_i(q) \; p_i \left(u_i, w_i, l_i(q), \dot{l}_i(q, v) \right) + \sum_k \nabla l_k(q) \; p_k This quantity is stored in ``mjData.qfrc_actuator``. It is added to the applied force vector :math:`\tau`, together with any user-defined forces in joint or Cartesian coordinates (which are stored in ``mjData.qfrc_applied`` and diff --git a/doc/includes/references.h b/doc/includes/references.h index 2dba3b1a..52323950 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -215,9 +215,9 @@ typedef struct mjData_ { mjtNum* wrap_xpos; // Cartesian 3D points in all paths (nwrap x 6) // computed by mj_fwdPosition/mj_transmission - mjtNum* actuator_length; // actuator lengths (nu x 1) - int* moment_rownnz; // number of non-zeros in actuator_moment row (nu x 1) - int* moment_rowadr; // row start address in colind array (nu x 1) + mjtNum* actuator_length; // actuator lengths, one per force output (nout x 1) + int* moment_rownnz; // number of non-zeros in actuator_moment row (nout x 1) + int* moment_rowadr; // row start address in colind array (nout x 1) int* moment_colind; // column indices in sparse Jacobian (nJmom x 1) mjtNum* actuator_moment; // actuator moments (nJmom x 1) @@ -244,7 +244,7 @@ typedef struct mjData_ { // computed by mj_fwdVelocity mjtNum* flexedge_velocity; // flex edge velocities (nflexedge x 1) mjtNum* ten_velocity; // tendon velocities (ntendon x 1) - mjtNum* actuator_velocity; // actuator velocities (nu x 1) + mjtNum* actuator_velocity; // actuator velocities, one per force output (nout x 1) // computed by mj_fwdVelocity/mj_comVel mjtNum* cvel; // com-based velocity (rot:lin) (nbody x 6) @@ -277,8 +277,8 @@ typedef struct mjData_ { //-------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent // computed by mj_fwdActuation - mjtNum* actuator_force; // actuator force in actuation space (nu x 1) - mjtNum* qfrc_actuator; // actuator force (nv x 1) + mjtNum* actuator_force; // actuator force in actuation space (nout x 1) + mjtNum* qfrc_actuator; // actuator force in joint space (nv x 1) // computed by mj_fwdAcceleration mjtNum* qfrc_smooth; // net unconstrained force (nv x 1) @@ -557,7 +557,9 @@ typedef struct mjModel_ { // sizes needed at mjModel construction mjtSize nq; // number of generalized coordinates = dim(qpos) mjtSize nv; // number of degrees of freedom = dim(qvel) - mjtSize nu; // number of actuators/controls = dim(ctrl) + mjtSize nu; // number of scalar controls = dim(ctrl) + mjtSize nactuator; // number of actuators + mjtSize nout; // number of force outputs, derived from transmission type mjtSize na; // number of activation states = dim(act) mjtSize nbody; // number of bodies mjtSize nbvh; // number of total bounding volumes in all bodies @@ -1072,37 +1074,41 @@ typedef struct mjModel_ { mjtNum* wrap_prm; // divisor, joint coef, or site id (nwrap x 1) // actuators - int* actuator_trntype; // transmission type (mjtTrn) (nu x 1) - int* actuator_dyntype; // dynamics type (mjtDyn) (nu x 1) - int* actuator_gaintype; // gain type (mjtGain) (nu x 1) - int* actuator_biastype; // bias type (mjtBias) (nu x 1) - int* actuator_trnid; // transmission id: joint, tendon, site (nu x 2) - mjtNum* actuator_damping; // linear damping coefficient (nu x 1) - mjtNum* actuator_dampingpoly; // high-order damping coefficients (nu x mjNPOLY) - mjtNum* actuator_armature; // armature added to target (joint, tendon) (nu x 1) - int* actuator_actadr; // first activation address; -1: stateless (nu x 1) - int* actuator_actnum; // number of activation variables (nu x 1) - int* actuator_group; // group for visibility (nu x 1) - int* actuator_history; // history buffer: [nsample, interp] (nu x 2) - int* actuator_historyadr; // address in history buffer; -1: none (nu x 1) - mjtNum* actuator_delay; // delay time in seconds; 0: no delay (nu x 1) + int* actuator_trntype; // transmission type (mjtTrn) (nactuator x 1) + int* actuator_dyntype; // dynamics type (mjtDyn) (nactuator x 1) + int* actuator_gaintype; // gain type (mjtGain) (nactuator x 1) + int* actuator_biastype; // bias type (mjtBias) (nactuator x 1) + int* actuator_ctrladr; // address of first control (nactuator x 1) + int* actuator_ctrlnum; // number of controls (nactuator x 1) + int* actuator_outadr; // address of first force output (nactuator x 1) + int* actuator_outnum; // number of force outputs, from trntype (nactuator x 1) + int* actuator_actadr; // first activation address; -1: stateless (nactuator x 1) + int* actuator_actnum; // number of activation variables (nactuator x 1) + int* actuator_trnid; // transmission id: joint, tendon, site (nactuator x 2) + mjtNum* actuator_cranklength; // crank length for slider-crank (nactuator x 1) + mjtNum* actuator_dynprm; // dynamics parameters (nactuator x mjNDYN) + mjtNum* actuator_gainprm; // gain parameters (nactuator x mjNGAIN) + mjtNum* actuator_biasprm; // bias parameters (nactuator x mjNBIAS) + mjtBool* actuator_actlimited; // is activation limited (nactuator x 1) + mjtNum* actuator_actrange; // range of activations (nactuator x 2) + mjtBool* actuator_actearly; // step activation before force (nactuator x 1) + int* actuator_history; // history buffer: [nsample, interp] (nactuator x 2) + int* actuator_historyadr; // address in history buffer; -1: none (nactuator x 1) + mjtNum* actuator_delay; // delay time; 0: no delay (nactuator x 1) + mjtNum* actuator_damping; // linear damping coefficient (nactuator x 1) + mjtNum* actuator_dampingpoly; // high-order damping coefficients (nactuator x mjNPOLY) + mjtNum* actuator_armature; // armature added to target (joint, tendon) (nactuator x 1) + int* actuator_group; // group for visibility (nactuator x 1) + mjtNum* actuator_user; // user data (nactuator x nuser_actuator) + int* actuator_plugin; // plugin instance id; -1: not a plugin (nactuator x 1) mjtBool* actuator_ctrllimited; // is control limited (nu x 1) - mjtBool* actuator_forcelimited;// is force limited (nu x 1) - mjtBool* actuator_actlimited; // is activation limited (nu x 1) - mjtNum* actuator_dynprm; // dynamics parameters (nu x mjNDYN) - mjtNum* actuator_gainprm; // gain parameters (nu x mjNGAIN) - mjtNum* actuator_biasprm; // bias parameters (nu x mjNBIAS) - mjtBool* actuator_actearly; // step activation before force (nu x 1) mjtNum* actuator_ctrlrange; // range of controls (nu x 2) - mjtNum* actuator_forcerange; // range of forces (nu x 2) - mjtNum* actuator_actrange; // range of activations (nu x 2) - mjtNum* actuator_gear; // scale length and transmitted force (nu x 6) - mjtNum* actuator_cranklength; // crank length for slider-crank (nu x 1) - mjtNum* actuator_acc0; // acceleration from unit force in qpos0 (nu x 1) - mjtNum* actuator_length0; // actuator length in qpos0 (nu x 1) - mjtNum* actuator_lengthrange; // feasible actuator length range (nu x 2) - mjtNum* actuator_user; // user data (nu x nuser_actuator) - int* actuator_plugin; // plugin instance id; -1: not a plugin (nu x 1) + mjtNum* actuator_gear; // scale length and transmitted force (nout x 6) + mjtBool* actuator_forcelimited;// is force limited (nout x 1) + mjtNum* actuator_forcerange; // range of forces (nout x 2) + mjtNum* actuator_acc0; // acceleration from unit force in qpos0 (nout x 1) + mjtNum* actuator_length0; // actuator length in qpos0 (nout x 1) + mjtNum* actuator_lengthrange; // feasible actuator length range (nout x 2) // sensors int* sensor_type; // sensor type (mjtSensor) (nsensor x 1) @@ -1174,7 +1180,7 @@ typedef struct mjModel_ { int* name_excludeadr; // exclude name pointers (nexclude x 1) int* name_eqadr; // equality constraint name pointers (neq x 1) int* name_tendonadr; // tendon name pointers (ntendon x 1) - int* name_actuatoradr; // actuator name pointers (nu x 1) + int* name_actuatoradr; // actuator name pointers (nactuator x 1) int* name_sensoradr; // sensor name pointers (nsensor x 1) int* name_numericadr; // numeric name pointers (nnumeric x 1) int* name_textadr; // text name pointers (ntext x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 9c9e9938..15afe8b2 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -239,9 +239,9 @@ typedef struct mjData_ { mjtNum* wrap_xpos; // Cartesian 3D points in all paths (nwrap x 6) // computed by mj_fwdPosition/mj_transmission - mjtNum* actuator_length; // actuator lengths (nu x 1) - int* moment_rownnz; // number of non-zeros in actuator_moment row (nu x 1) - int* moment_rowadr; // row start address in colind array (nu x 1) + mjtNum* actuator_length; // actuator lengths, one per force output (nout x 1) + int* moment_rownnz; // number of non-zeros in actuator_moment row (nout x 1) + int* moment_rowadr; // row start address in colind array (nout x 1) int* moment_colind; // column indices in sparse Jacobian (nJmom x 1) mjtNum* actuator_moment; // actuator moments (nJmom x 1) @@ -268,7 +268,7 @@ typedef struct mjData_ { // computed by mj_fwdVelocity mjtNum* flexedge_velocity; // flex edge velocities (nflexedge x 1) mjtNum* ten_velocity; // tendon velocities (ntendon x 1) - mjtNum* actuator_velocity; // actuator velocities (nu x 1) + mjtNum* actuator_velocity; // actuator velocities, one per force output (nout x 1) // computed by mj_fwdVelocity/mj_comVel mjtNum* cvel; // com-based velocity (rot:lin) (nbody x 6) @@ -301,8 +301,8 @@ typedef struct mjData_ { //-------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent // computed by mj_fwdActuation - mjtNum* actuator_force; // actuator force in actuation space (nu x 1) - mjtNum* qfrc_actuator; // actuator force (nv x 1) + mjtNum* actuator_force; // actuator force in actuation space (nout x 1) + mjtNum* qfrc_actuator; // actuator force in joint space (nv x 1) // computed by mj_fwdAcceleration mjtNum* qfrc_smooth; // net unconstrained force (nv x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index efd76ba3..a27b6bf0 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -245,7 +245,9 @@ typedef struct mjModel_ { // sizes needed at mjModel construction mjtSize nq; // number of generalized coordinates = dim(qpos) mjtSize nv; // number of degrees of freedom = dim(qvel) - mjtSize nu; // number of actuators/controls = dim(ctrl) + mjtSize nu; // number of scalar controls = dim(ctrl) + mjtSize nactuator; // number of actuators + mjtSize nout; // number of force outputs, derived from transmission type mjtSize na; // number of activation states = dim(act) mjtSize nbody; // number of bodies mjtSize nbvh; // number of total bounding volumes in all bodies @@ -760,37 +762,41 @@ typedef struct mjModel_ { mjtNum* wrap_prm; // divisor, joint coef, or site id (nwrap x 1) // actuators - int* actuator_trntype; // transmission type (mjtTrn) (nu x 1) - int* actuator_dyntype; // dynamics type (mjtDyn) (nu x 1) - int* actuator_gaintype; // gain type (mjtGain) (nu x 1) - int* actuator_biastype; // bias type (mjtBias) (nu x 1) - int* actuator_trnid; // transmission id: joint, tendon, site (nu x 2) - mjtNum* actuator_damping; // linear damping coefficient (nu x 1) - mjtNum* actuator_dampingpoly; // high-order damping coefficients (nu x mjNPOLY) - mjtNum* actuator_armature; // armature added to target (joint, tendon) (nu x 1) - int* actuator_actadr; // first activation address; -1: stateless (nu x 1) - int* actuator_actnum; // number of activation variables (nu x 1) - int* actuator_group; // group for visibility (nu x 1) - int* actuator_history; // history buffer: [nsample, interp] (nu x 2) - int* actuator_historyadr; // address in history buffer; -1: none (nu x 1) - mjtNum* actuator_delay; // delay time in seconds; 0: no delay (nu x 1) + int* actuator_trntype; // transmission type (mjtTrn) (nactuator x 1) + int* actuator_dyntype; // dynamics type (mjtDyn) (nactuator x 1) + int* actuator_gaintype; // gain type (mjtGain) (nactuator x 1) + int* actuator_biastype; // bias type (mjtBias) (nactuator x 1) + int* actuator_ctrladr; // address of first control (nactuator x 1) + int* actuator_ctrlnum; // number of controls (nactuator x 1) + int* actuator_outadr; // address of first force output (nactuator x 1) + int* actuator_outnum; // number of force outputs, from trntype (nactuator x 1) + int* actuator_actadr; // first activation address; -1: stateless (nactuator x 1) + int* actuator_actnum; // number of activation variables (nactuator x 1) + int* actuator_trnid; // transmission id: joint, tendon, site (nactuator x 2) + mjtNum* actuator_cranklength; // crank length for slider-crank (nactuator x 1) + mjtNum* actuator_dynprm; // dynamics parameters (nactuator x mjNDYN) + mjtNum* actuator_gainprm; // gain parameters (nactuator x mjNGAIN) + mjtNum* actuator_biasprm; // bias parameters (nactuator x mjNBIAS) + mjtBool* actuator_actlimited; // is activation limited (nactuator x 1) + mjtNum* actuator_actrange; // range of activations (nactuator x 2) + mjtBool* actuator_actearly; // step activation before force (nactuator x 1) + int* actuator_history; // history buffer: [nsample, interp] (nactuator x 2) + int* actuator_historyadr; // address in history buffer; -1: none (nactuator x 1) + mjtNum* actuator_delay; // delay time; 0: no delay (nactuator x 1) + mjtNum* actuator_damping; // linear damping coefficient (nactuator x 1) + mjtNum* actuator_dampingpoly; // high-order damping coefficients (nactuator x mjNPOLY) + mjtNum* actuator_armature; // armature added to target (joint, tendon) (nactuator x 1) + int* actuator_group; // group for visibility (nactuator x 1) + mjtNum* actuator_user; // user data (nactuator x nuser_actuator) + int* actuator_plugin; // plugin instance id; -1: not a plugin (nactuator x 1) mjtBool* actuator_ctrllimited; // is control limited (nu x 1) - mjtBool* actuator_forcelimited;// is force limited (nu x 1) - mjtBool* actuator_actlimited; // is activation limited (nu x 1) - mjtNum* actuator_dynprm; // dynamics parameters (nu x mjNDYN) - mjtNum* actuator_gainprm; // gain parameters (nu x mjNGAIN) - mjtNum* actuator_biasprm; // bias parameters (nu x mjNBIAS) - mjtBool* actuator_actearly; // step activation before force (nu x 1) mjtNum* actuator_ctrlrange; // range of controls (nu x 2) - mjtNum* actuator_forcerange; // range of forces (nu x 2) - mjtNum* actuator_actrange; // range of activations (nu x 2) - mjtNum* actuator_gear; // scale length and transmitted force (nu x 6) - mjtNum* actuator_cranklength; // crank length for slider-crank (nu x 1) - mjtNum* actuator_acc0; // acceleration from unit force in qpos0 (nu x 1) - mjtNum* actuator_length0; // actuator length in qpos0 (nu x 1) - mjtNum* actuator_lengthrange; // feasible actuator length range (nu x 2) - mjtNum* actuator_user; // user data (nu x nuser_actuator) - int* actuator_plugin; // plugin instance id; -1: not a plugin (nu x 1) + mjtNum* actuator_gear; // scale length and transmitted force (nout x 6) + mjtBool* actuator_forcelimited;// is force limited (nout x 1) + mjtNum* actuator_forcerange; // range of forces (nout x 2) + mjtNum* actuator_acc0; // acceleration from unit force in qpos0 (nout x 1) + mjtNum* actuator_length0; // actuator length in qpos0 (nout x 1) + mjtNum* actuator_lengthrange; // feasible actuator length range (nout x 2) // sensors int* sensor_type; // sensor type (mjtSensor) (nsensor x 1) @@ -862,7 +868,7 @@ typedef struct mjModel_ { int* name_excludeadr; // exclude name pointers (nexclude x 1) int* name_eqadr; // equality constraint name pointers (neq x 1) int* name_tendonadr; // tendon name pointers (ntendon x 1) - int* name_actuatoradr; // actuator name pointers (nu x 1) + int* name_actuatoradr; // actuator name pointers (nactuator x 1) int* name_sensoradr; // sensor name pointers (nsensor x 1) int* name_numericadr; // numeric name pointers (nnumeric x 1) int* name_textadr; // text name pointers (ntext x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 5667a17f..845c9ffd 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -163,6 +163,8 @@ X( nq ) \ X( nv ) \ X( nu ) \ + X( nactuator ) \ + X( nout ) \ X( na ) \ X( nbody ) \ X( nbvh ) \ @@ -666,37 +668,41 @@ X ( float, tendon_rgba, ntendon, 4 ) #define MJMODEL_POINTERS_ACTUATOR \ - X ( int, actuator_trntype, nu, 1 ) \ - X ( int, actuator_dyntype, nu, 1 ) \ - X ( int, actuator_gaintype, nu, 1 ) \ - X ( int, actuator_biastype, nu, 1 ) \ - X ( int, actuator_trnid, nu, 2 ) \ - X ( mjtNum, actuator_damping, nu, 1 ) \ - X ( mjtNum, actuator_dampingpoly, nu, mjNPOLY ) \ - X ( mjtNum, actuator_armature, nu, 1 ) \ - X ( int, actuator_actadr, nu, 1 ) \ - X ( int, actuator_actnum, nu, 1 ) \ - X ( int, actuator_group, nu, 1 ) \ - X ( int, actuator_history, nu, 2 ) \ - X ( int, actuator_historyadr, nu, 1 ) \ - X ( mjtNum, actuator_delay, nu, 1 ) \ + X ( int, actuator_trntype, nactuator, 1 ) \ + X ( int, actuator_dyntype, nactuator, 1 ) \ + X ( int, actuator_gaintype, nactuator, 1 ) \ + X ( int, actuator_biastype, nactuator, 1 ) \ + X ( int, actuator_ctrladr, nactuator, 1 ) \ + X ( int, actuator_ctrlnum, nactuator, 1 ) \ + X ( int, actuator_outadr, nactuator, 1 ) \ + X ( int, actuator_outnum, nactuator, 1 ) \ + X ( int, actuator_actadr, nactuator, 1 ) \ + X ( int, actuator_actnum, nactuator, 1 ) \ + X ( int, actuator_trnid, nactuator, 2 ) \ + X ( mjtNum, actuator_cranklength, nactuator, 1 ) \ + X ( mjtNum, actuator_dynprm, nactuator, mjNDYN ) \ + X ( mjtNum, actuator_gainprm, nactuator, mjNGAIN ) \ + X ( mjtNum, actuator_biasprm, nactuator, mjNBIAS ) \ + X ( mjtBool, actuator_actlimited, nactuator, 1 ) \ + X ( mjtNum, actuator_actrange, nactuator, 2 ) \ + X ( mjtBool, actuator_actearly, nactuator, 1 ) \ + X ( int, actuator_history, nactuator, 2 ) \ + X ( int, actuator_historyadr, nactuator, 1 ) \ + X ( mjtNum, actuator_delay, nactuator, 1 ) \ + X ( mjtNum, actuator_damping, nactuator, 1 ) \ + X ( mjtNum, actuator_dampingpoly, nactuator, mjNPOLY ) \ + X ( mjtNum, actuator_armature, nactuator, 1 ) \ + X ( int, actuator_group, nactuator, 1 ) \ + X ( mjtNum, actuator_user, nactuator, MJ_M(nuser_actuator) ) \ + X ( int, actuator_plugin, nactuator, 1 ) \ X ( mjtBool, actuator_ctrllimited, nu, 1 ) \ - X ( mjtBool, actuator_forcelimited, nu, 1 ) \ - X ( mjtBool, actuator_actlimited, nu, 1 ) \ - X ( mjtNum, actuator_dynprm, nu, mjNDYN ) \ - X ( mjtNum, actuator_gainprm, nu, mjNGAIN ) \ - X ( mjtNum, actuator_biasprm, nu, mjNBIAS ) \ - X ( mjtBool, actuator_actearly, nu, 1 ) \ X ( mjtNum, actuator_ctrlrange, nu, 2 ) \ - X ( mjtNum, actuator_forcerange, nu, 2 ) \ - X ( mjtNum, actuator_actrange, nu, 2 ) \ - X ( mjtNum, actuator_gear, nu, 6 ) \ - X ( mjtNum, actuator_cranklength, nu, 1 ) \ - X ( mjtNum, actuator_acc0, nu, 1 ) \ - X ( mjtNum, actuator_length0, nu, 1 ) \ - X ( mjtNum, actuator_lengthrange, nu, 2 ) \ - X ( mjtNum, actuator_user, nu, MJ_M(nuser_actuator) ) \ - X ( int, actuator_plugin, nu, 1 ) + X ( mjtNum, actuator_gear, nout, 6 ) \ + X ( mjtBool, actuator_forcelimited, nout, 1 ) \ + X ( mjtNum, actuator_forcerange, nout, 2 ) \ + X ( mjtNum, actuator_acc0, nout, 1 ) \ + X ( mjtNum, actuator_length0, nout, 1 ) \ + X ( mjtNum, actuator_lengthrange, nout, 2 ) #define MJMODEL_POINTERS_SENSOR \ X ( int, sensor_type, nsensor, 1 ) \ @@ -791,7 +797,7 @@ X ( int, name_excludeadr, nexclude, 1 ) \ X ( int, name_eqadr, neq, 1 ) \ X ( int, name_tendonadr, ntendon, 1 ) \ - X ( int, name_actuatoradr, nu, 1 ) \ + X ( int, name_actuatoradr, nactuator, 1 ) \ X ( int, name_sensoradr, nsensor, 1 ) \ X ( int, name_numericadr, nnumeric, 1 ) \ X ( int, name_textadr, ntext, 1 ) \ @@ -871,9 +877,9 @@ X ( mjtNum, ten_length, ntendon, 1 ) \ X ( int, wrap_obj, nwrap, 2 ) \ X ( mjtNum, wrap_xpos, nwrap, 6 ) \ - X ( mjtNum, actuator_length, nu, 1 ) \ - X ( int, moment_rownnz, nu, 1 ) \ - X ( int, moment_rowadr, nu, 1 ) \ + X ( mjtNum, actuator_length, nout, 1 ) \ + X ( int, moment_rownnz, nout, 1 ) \ + X ( int, moment_rowadr, nout, 1 ) \ X ( int, moment_colind, nJmom, 1 ) \ X ( mjtNum, actuator_moment, nJmom, 1 ) \ XNV ( mjtNum, crb, nbody, 10 ) \ @@ -888,7 +894,7 @@ X ( int, dof_awake_ind, nv, 1 ) \ X ( mjtNum, flexedge_velocity, nflexedge, 1 ) \ X ( mjtNum, ten_velocity, ntendon, 1 ) \ - X ( mjtNum, actuator_velocity, nu, 1 ) \ + X ( mjtNum, actuator_velocity, nout, 1 ) \ X ( mjtNum, cvel, nbody, 6 ) \ X ( mjtNum, cdof_dot, nv, 6 ) \ X ( mjtNum, qfrc_bias, nv, 1 ) \ @@ -903,7 +909,7 @@ X ( mjtNum, qHDiagInv, nv, 1 ) \ XNV ( mjtNum, qDeriv, nD, 1 ) \ XNV ( mjtNum, qLU, nD, 1 ) \ - X ( mjtNum, actuator_force, nu, 1 ) \ + X ( mjtNum, actuator_force, nout, 1 ) \ X ( mjtNum, qfrc_actuator, nv, 1 ) \ X ( mjtNum, qfrc_smooth, nv, 1 ) \ X ( mjtNum, qacc_smooth, nv, 1 ) \ diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 78c73700..708293e5 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -1266,24 +1266,24 @@ class Model: actuator_dyntype: dynamics type (DynType) (nu,) actuator_gaintype: gain type (GainType) (nu,) actuator_biastype: bias type (BiasType) (nu,) - actuator_trnid: transmission id: joint, tendon, site (nu, 2) actuator_actadr: first activation address; -1: stateless (nu,) actuator_actnum: number of activation variables (nu,) + actuator_trnid: transmission id: joint, tendon, site (nu, 2) + actuator_cranklength: crank length for slider-crank (*, nu) + actuator_dynprm: dynamics parameters (*, nu, mjNDYN) + actuator_gainprm: gain parameters (*, nu, mjNGAIN) + actuator_biasprm: bias parameters (*, nu, mjNBIAS) + actuator_actlimited: is activation limited (nu,) + actuator_actrange: range of activations (*, nu, 2) + actuator_actearly: step activation before force (nu,) actuator_history: history buffer sizes (nu, 2) actuator_historyadr: history buffer address (nu,) actuator_delay: delay in seconds (nu,) actuator_ctrllimited: is control limited (nu,) - actuator_forcelimited: is force limited (nu,) - actuator_actlimited: is activation limited (nu,) - actuator_dynprm: dynamics parameters (*, nu, mjNDYN) - actuator_gainprm: gain parameters (*, nu, mjNGAIN) - actuator_biasprm: bias parameters (*, nu, mjNBIAS) - actuator_actearly: step activation before force (nu,) actuator_ctrlrange: range of controls (*, nu, 2) - actuator_forcerange: range of forces (*, nu, 2) - actuator_actrange: range of activations (*, nu, 2) actuator_gear: scale length and transmitted force (*, nu, 6) - actuator_cranklength: crank length for slider-crank (*, nu) + actuator_forcelimited: is force limited (nu,) + actuator_forcerange: range of forces (*, nu, 2) actuator_acc0: acceleration from unit force in qpos0 (*, nu) actuator_lengthrange: feasible actuator length range (*, nu, 2) sensor_type: sensor type (SensorType) (nsensor,) @@ -1749,24 +1749,24 @@ class Model: actuator_dyntype: array("nu", int) actuator_gaintype: array("nu", int) actuator_biastype: array("nu", int) - actuator_trnid: array("nu", wp.vec2i) actuator_actadr: array("nu", int) actuator_actnum: array("nu", int) + actuator_trnid: array("nu", wp.vec2i) + actuator_cranklength: array("*", "nu", float) + actuator_dynprm: array("*", "nu", vec10f) + actuator_gainprm: array("*", "nu", vec10f) + actuator_biasprm: array("*", "nu", vec10f) + actuator_actlimited: array("nu", bool) + actuator_actrange: array("*", "nu", wp.vec2) + actuator_actearly: array("nu", bool) actuator_history: array("nu", wp.vec2i) actuator_historyadr: array("nu", int) actuator_delay: array("nu", float) actuator_ctrllimited: array("nu", bool) - actuator_forcelimited: array("nu", bool) - actuator_actlimited: array("nu", bool) - actuator_dynprm: array("*", "nu", vec10f) - actuator_gainprm: array("*", "nu", vec10f) - actuator_biasprm: array("*", "nu", vec10f) - actuator_actearly: array("nu", bool) actuator_ctrlrange: array("*", "nu", wp.vec2) - actuator_forcerange: array("*", "nu", wp.vec2) - actuator_actrange: array("*", "nu", wp.vec2) actuator_gear: array("*", "nu", wp.spatial_vector) - actuator_cranklength: array("*", "nu", float) + actuator_forcelimited: array("nu", bool) + actuator_forcerange: array("*", "nu", wp.vec2) actuator_acc0: array("*", "nu", float) actuator_lengthrange: array("*", "nu", wp.vec2) sensor_type: array("nsensor", int) diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 1af344dc..b4f256e9 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -946,7 +946,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='nu', type=ValueType(name='mjtSize'), - doc='number of actuators/controls = dim(ctrl)', + doc='number of scalar controls = dim(ctrl)', + ), + StructFieldDecl( + name='nactuator', + type=ValueType(name='mjtSize'), + doc='number of actuators', + ), + StructFieldDecl( + name='nout', + type=ValueType(name='mjtSize'), + doc='number of force outputs, derived from transmission type', ), StructFieldDecl( name='na', @@ -4311,7 +4321,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='transmission type (mjtTrn)', - array_extent=('nu',), + array_extent=('nactuator',), ), StructFieldDecl( name='actuator_dyntype', @@ -4319,7 +4329,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='dynamics type (mjtDyn)', - array_extent=('nu',), + array_extent=('nactuator',), ), StructFieldDecl( name='actuator_gaintype', @@ -4327,7 +4337,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='gain type (mjtGain)', - array_extent=('nu',), + array_extent=('nactuator',), ), StructFieldDecl( name='actuator_biastype', @@ -4335,39 +4345,39 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='bias type (mjtBias)', - array_extent=('nu',), + array_extent=('nactuator',), ), StructFieldDecl( - name='actuator_trnid', + name='actuator_ctrladr', type=PointerType( inner_type=ValueType(name='int'), ), - doc='transmission id: joint, tendon, site', - array_extent=('nu', 2), + doc='address of first control', + array_extent=('nactuator',), ), StructFieldDecl( - name='actuator_damping', + name='actuator_ctrlnum', type=PointerType( - inner_type=ValueType(name='mjtNum'), + inner_type=ValueType(name='int'), ), - doc='linear damping coefficient', - array_extent=('nu',), + doc='number of controls', + array_extent=('nactuator',), ), StructFieldDecl( - name='actuator_dampingpoly', + name='actuator_outadr', type=PointerType( - inner_type=ValueType(name='mjtNum'), + inner_type=ValueType(name='int'), ), - doc='high-order damping coefficients', - array_extent=('nu', 'mjNPOLY'), + doc='address of first force output', + array_extent=('nactuator',), ), StructFieldDecl( - name='actuator_armature', + name='actuator_outnum', type=PointerType( - inner_type=ValueType(name='mjtNum'), + inner_type=ValueType(name='int'), ), - doc='armature added to target (joint, tendon)', - array_extent=('nu',), + doc='number of force outputs, from trntype', + array_extent=('nactuator',), ), StructFieldDecl( name='actuator_actadr', @@ -4375,7 +4385,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='first activation address; -1: stateless', - array_extent=('nu',), + array_extent=('nactuator',), ), StructFieldDecl( name='actuator_actnum', @@ -4383,63 +4393,23 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='number of activation variables', - array_extent=('nu',), + array_extent=('nactuator',), ), StructFieldDecl( - name='actuator_group', + name='actuator_trnid', type=PointerType( inner_type=ValueType(name='int'), ), - doc='group for visibility', - array_extent=('nu',), + doc='transmission id: joint, tendon, site', + array_extent=('nactuator', 2), ), StructFieldDecl( - name='actuator_history', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='history buffer: [nsample, interp]', - array_extent=('nu', 2), - ), - StructFieldDecl( - name='actuator_historyadr', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='address in history buffer; -1: none', - array_extent=('nu',), - ), - StructFieldDecl( - name='actuator_delay', + name='actuator_cranklength', type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='delay time in seconds; 0: no delay', - array_extent=('nu',), - ), - StructFieldDecl( - name='actuator_ctrllimited', - type=PointerType( - inner_type=ValueType(name='mjtBool'), - ), - doc='is control limited', - array_extent=('nu',), - ), - StructFieldDecl( - name='actuator_forcelimited', - type=PointerType( - inner_type=ValueType(name='mjtBool'), - ), - doc='is force limited', - array_extent=('nu',), - ), - StructFieldDecl( - name='actuator_actlimited', - type=PointerType( - inner_type=ValueType(name='mjtBool'), - ), - doc='is activation limited', - array_extent=('nu',), + doc='crank length for slider-crank', + array_extent=('nactuator',), ), StructFieldDecl( name='actuator_dynprm', @@ -4447,7 +4417,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='dynamics parameters', - array_extent=('nu', 'mjNDYN'), + array_extent=('nactuator', 'mjNDYN'), ), StructFieldDecl( name='actuator_gainprm', @@ -4455,7 +4425,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='gain parameters', - array_extent=('nu', 'mjNGAIN'), + array_extent=('nactuator', 'mjNGAIN'), ), StructFieldDecl( name='actuator_biasprm', @@ -4463,7 +4433,23 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='bias parameters', - array_extent=('nu', 'mjNBIAS'), + array_extent=('nactuator', 'mjNBIAS'), + ), + StructFieldDecl( + name='actuator_actlimited', + type=PointerType( + inner_type=ValueType(name='mjtBool'), + ), + doc='is activation limited', + array_extent=('nactuator',), + ), + StructFieldDecl( + name='actuator_actrange', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='range of activations', + array_extent=('nactuator', 2), ), StructFieldDecl( name='actuator_actearly', @@ -4471,6 +4457,86 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtBool'), ), doc='step activation before force', + array_extent=('nactuator',), + ), + StructFieldDecl( + name='actuator_history', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='history buffer: [nsample, interp]', + array_extent=('nactuator', 2), + ), + StructFieldDecl( + name='actuator_historyadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='address in history buffer; -1: none', + array_extent=('nactuator',), + ), + StructFieldDecl( + name='actuator_delay', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='delay time; 0: no delay', + array_extent=('nactuator',), + ), + StructFieldDecl( + name='actuator_damping', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='linear damping coefficient', + array_extent=('nactuator',), + ), + StructFieldDecl( + name='actuator_dampingpoly', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='high-order damping coefficients', + array_extent=('nactuator', 'mjNPOLY'), + ), + StructFieldDecl( + name='actuator_armature', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='armature added to target (joint, tendon)', + array_extent=('nactuator',), + ), + StructFieldDecl( + name='actuator_group', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='group for visibility', + array_extent=('nactuator',), + ), + StructFieldDecl( + name='actuator_user', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='user data', + array_extent=('nactuator', 'nuser_actuator'), + ), + StructFieldDecl( + name='actuator_plugin', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='plugin instance id; -1: not a plugin', + array_extent=('nactuator',), + ), + StructFieldDecl( + name='actuator_ctrllimited', + type=PointerType( + inner_type=ValueType(name='mjtBool'), + ), + doc='is control limited', array_extent=('nu',), ), StructFieldDecl( @@ -4481,37 +4547,29 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='range of controls', array_extent=('nu', 2), ), - StructFieldDecl( - name='actuator_forcerange', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='range of forces', - array_extent=('nu', 2), - ), - StructFieldDecl( - name='actuator_actrange', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='range of activations', - array_extent=('nu', 2), - ), StructFieldDecl( name='actuator_gear', type=PointerType( inner_type=ValueType(name='mjtNum'), ), doc='scale length and transmitted force', - array_extent=('nu', 6), + array_extent=('nout', 6), ), StructFieldDecl( - name='actuator_cranklength', + name='actuator_forcelimited', + type=PointerType( + inner_type=ValueType(name='mjtBool'), + ), + doc='is force limited', + array_extent=('nout',), + ), + StructFieldDecl( + name='actuator_forcerange', type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='crank length for slider-crank', - array_extent=('nu',), + doc='range of forces', + array_extent=('nout', 2), ), StructFieldDecl( name='actuator_acc0', @@ -4519,7 +4577,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='acceleration from unit force in qpos0', - array_extent=('nu',), + array_extent=('nout',), ), StructFieldDecl( name='actuator_length0', @@ -4527,7 +4585,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='actuator length in qpos0', - array_extent=('nu',), + array_extent=('nout',), ), StructFieldDecl( name='actuator_lengthrange', @@ -4535,23 +4593,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='feasible actuator length range', - array_extent=('nu', 2), - ), - StructFieldDecl( - name='actuator_user', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='user data', - array_extent=('nu', 'nuser_actuator'), - ), - StructFieldDecl( - name='actuator_plugin', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='plugin instance id; -1: not a plugin', - array_extent=('nu',), + array_extent=('nout', 2), ), StructFieldDecl( name='sensor_type', @@ -5015,7 +5057,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='actuator name pointers', - array_extent=('nu',), + array_extent=('nactuator',), ), StructFieldDecl( name='name_sensoradr', @@ -6082,8 +6124,8 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='actuator lengths', - array_extent=('nu',), + doc='actuator lengths, one per force output', + array_extent=('nout',), ), StructFieldDecl( name='moment_rownnz', @@ -6091,7 +6133,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='number of non-zeros in actuator_moment row', - array_extent=('nu',), + array_extent=('nout',), ), StructFieldDecl( name='moment_rowadr', @@ -6099,7 +6141,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='row start address in colind array', - array_extent=('nu',), + array_extent=('nout',), ), StructFieldDecl( name='moment_colind', @@ -6218,8 +6260,8 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='actuator velocities', - array_extent=('nu',), + doc='actuator velocities, one per force output', + array_extent=('nout',), ), StructFieldDecl( name='cvel', @@ -6339,14 +6381,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='actuator force in actuation space', - array_extent=('nu',), + array_extent=('nout',), ), StructFieldDecl( name='qfrc_actuator', type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='actuator force', + doc='actuator force in joint space', array_extent=('nv',), ), StructFieldDecl( diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 2f118b0d..dc75e577 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1263,10 +1263,10 @@ mjtNum mj_tendonDot(const mjModel* m, mjData* d, int id, const mjtNum* vec) { // compute actuator/transmission lengths and moments void mj_transmission(const mjModel* m, mjData* d) { - int nv = m->nv, nu = m->nu; + int nv = m->nv, nactuator = m->nactuator; // nothing to do - if (!nu) { + if (!nactuator) { return; } @@ -1295,19 +1295,21 @@ void mj_transmission(const mjModel* m, mjData* d) { int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < nv; // compute lengths and moments - for (int i=0; i < nu; i++) { - rowadr[i] = i == 0 ? 0 : rowadr[i-1] + rownnz[i-1]; - int nnz, adr = rowadr[i]; + for (int i=0; i < nactuator; i++) { + // address of the actuator's output block (single row for all current types) + int out = m->actuator_outadr[i]; + rowadr[out] = out == 0 ? 0 : rowadr[out-1] + rownnz[out-1]; + int nnz, adr = rowadr[out]; // skip sleeping actuator if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) { - rownnz[i] = 0; + rownnz[out] = 0; continue; } // extract info int id = m->actuator_trnid[2*i]; - mjtNum* gear = m->actuator_gear+6*i; + mjtNum* gear = m->actuator_gear+6*out; // process according to transmission type switch ((mjtTrn) m->actuator_trntype[i]) { @@ -1316,10 +1318,10 @@ void mj_transmission(const mjModel* m, mjData* d) { // slide and hinge joint: scalar gear if (m->jnt_type[id] == mjJNT_SLIDE || m->jnt_type[id] == mjJNT_HINGE) { // sparsity - rownnz[i] = 1; + rownnz[out] = 1; colind[adr] = m->jnt_dofadr[id]; - length[i] = d->qpos[m->jnt_qposadr[id]]*gear[0]; + length[out] = d->qpos[m->jnt_qposadr[id]]*gear[0]; moment[adr] = gear[0]; } @@ -1341,7 +1343,7 @@ void mj_transmission(const mjModel* m, mjData* d) { } // length: axis*gearAxis - length[i] = mju_dot3(axis, gearAxis); + length[out] = mju_dot3(axis, gearAxis); // dof start address int jnt_dofadr = m->jnt_dofadr[id]; @@ -1350,7 +1352,7 @@ void mj_transmission(const mjModel* m, mjData* d) { for (int j = 0; j < 3; j++) { colind[adr+j] = jnt_dofadr + j; } - rownnz[i] = 3; + rownnz[out] = 3; // moment: gearAxis mji_copy3(moment+adr, gearAxis); @@ -1359,7 +1361,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // free joint: 6D wrench gear else { // cannot compute meaningful length, set to 0 - length[i] = 0; + length[out] = 0; // gearAxis: rotate to world frame if necessary mjtNum gearAxis[3]; @@ -1380,7 +1382,7 @@ void mj_transmission(const mjModel* m, mjData* d) { for (int j = 0; j < 6; j++) { colind[adr+j] = jnt_dofadr + j; } - rownnz[i] = 6; + rownnz[out] = 6; // moment: gear(tran), gearAxis mji_copy3(moment+adr, gear); @@ -1392,7 +1394,7 @@ void mj_transmission(const mjModel* m, mjData* d) { { // get data int idslider = m->actuator_trnid[2*i+1]; - mjtNum rod = m->actuator_cranklength[i]; + mjtNum rod = m->actuator_cranklength[out]; mjtNum axis[3] = {d->site_xmat[9 * idslider + 2], d->site_xmat[9 * idslider + 5], d->site_xmat[9 * idslider + 8]}; @@ -1407,10 +1409,10 @@ void mj_transmission(const mjModel* m, mjData* d) { if (det <= 0) { ok = 0; sdet = 0; - length[i] = av; + length[out] = av; } else { sdet = mju_sqrt(det); - length[i] = av - sdet; + length[out] = av - sdet; } // compute derivatives of length w.r.t. vec and axis @@ -1445,7 +1447,7 @@ void mj_transmission(const mjModel* m, mjData* d) { } // scale by gear ratio - length[i] *= gear[0]; + length[out] *= gear[0]; // sparsity (compress) nnz = 0; @@ -1456,18 +1458,18 @@ void mj_transmission(const mjModel* m, mjData* d) { nnz++; } } - rownnz[i] = nnz; + rownnz[out] = nnz; } break; case mjTRN_TENDON: // tendon - length[i] = d->ten_length[id]*gear[0]; + length[out] = d->ten_length[id]*gear[0]; // moment { int ten_J_rownnz = m->ten_J_rownnz[id]; int ten_J_rowadr = m->ten_J_rowadr[id]; - rownnz[i] = ten_J_rownnz; + rownnz[out] = ten_J_rownnz; mju_copyInt(colind + adr, m->ten_J_colind + ten_J_rowadr, ten_J_rownnz); mju_scl(moment + adr, d->ten_J + ten_J_rowadr, gear[0], ten_J_rownnz); @@ -1479,7 +1481,7 @@ void mj_transmission(const mjModel* m, mjData* d) { mj_jacSite(m, d, jac, jacS, id); // clear length - length[i] = 0; + length[out] = 0; if (!moment_row) moment_row = mjSTACKALLOC(d, nv, mjtNum); @@ -1540,7 +1542,7 @@ void mj_transmission(const mjModel* m, mjData* d) { mju_mulMatTVec3(vec, d->site_xmat+9*refid, vec); // length: dot product with gear - length[i] += mju_dot3(vec, gear); + length[out] += mju_dot3(vec, gear); // jacref: global Jacobian of reference site mj_jacSite(m, d, jacref, NULL, refid); @@ -1579,7 +1581,7 @@ void mj_transmission(const mjModel* m, mjData* d) { mji_subQuat(vec, quat, refquat); // add length: dot product with gear - length[i] += mju_dot3(vec, gear+3); + length[out] += mju_dot3(vec, gear+3); // jacref: global rotational Jacobian of reference site mj_jacSite(m, d, NULL, jacref, refid); @@ -1616,13 +1618,13 @@ void mj_transmission(const mjModel* m, mjData* d) { nnz++; } } - rownnz[i] = nnz; + rownnz[out] = nnz; break; case mjTRN_BODY: // body (adhesive contacts) // cannot compute meaningful length, set to 0 - length[i] = 0; + length[out] = 0; // clear moment if (!moment_row) moment_row = mjSTACKALLOC(d, nv, mjtNum); @@ -1730,7 +1732,7 @@ void mj_transmission(const mjModel* m, mjData* d) { nnz++; } } - rownnz[i] = nnz; + rownnz[out] = nnz; break; diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index 008b279a..ebb00313 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -1102,7 +1102,7 @@ mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjN // single actuator contributes damping if (actuatorid >= 0) { - mjtNum gear2 = m->actuator_gear[6*actuatorid] * m->actuator_gear[6*actuatorid]; + mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[actuatorid]] * m->actuator_gear[6*m->actuator_outadr[actuatorid]]; damping = m->actuator_damping[actuatorid] * gear2; for (int k = 0; k < mjNPOLY; k++) { poly[k] += m->actuator_dampingpoly[mjNPOLY*actuatorid+k] * gear2; @@ -1111,7 +1111,7 @@ mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjN // actuatorid < -1: scan all actuators for contributions else { - for (int k = 0; k < m->nu; k++) { + for (int k = 0; k < m->nactuator; k++) { // skip actuators that don't actuate the given joint/tendon if (m->actuator_trnid[2*k] != id) { continue; @@ -1126,7 +1126,7 @@ mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjN } // accumulate damping contribution - mjtNum gear2 = m->actuator_gear[6*k] * m->actuator_gear[6*k]; + mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[k]] * m->actuator_gear[6*m->actuator_outadr[k]]; damping += m->actuator_damping[k] * gear2; for (int j = 0; j < mjNPOLY; j++) { poly[j] += m->actuator_dampingpoly[mjNPOLY*k+j] * gear2; @@ -1157,13 +1157,13 @@ mjtNum mj_actuatorArmature(const mjModel* m, mjtObj type, int id) { // single actuator contributes armature if (actuatorid >= 0) { - mjtNum gear2 = m->actuator_gear[6*actuatorid] * m->actuator_gear[6*actuatorid]; + mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[actuatorid]] * m->actuator_gear[6*m->actuator_outadr[actuatorid]]; armature = m->actuator_armature[actuatorid] * gear2; } // actuatorid < -1: scan all actuators for contributions else { - for (int k = 0; k < m->nu; k++) { + for (int k = 0; k < m->nactuator; k++) { // skip actuators that don't actuate the given joint/tendon if (m->actuator_trnid[2*k] != id) { continue; @@ -1178,7 +1178,7 @@ mjtNum mj_actuatorArmature(const mjModel* m, mjtObj type, int id) { } // accumulate armature contribution - mjtNum gear2 = m->actuator_gear[6*k] * m->actuator_gear[6*k]; + mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[k]] * m->actuator_gear[6*m->actuator_outadr[k]]; armature += m->actuator_armature[k] * gear2; } } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 8c2f26d7..48c59d1c 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1211,7 +1211,7 @@ void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* ve // add (d qfrc_actuator / d qvel) to qDeriv void mjd_actuator_vel(const mjModel* m, mjData* d) { - int nu = m->nu; + int nactuator = m->nactuator; int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree; // disabled: nothing to add @@ -1220,7 +1220,10 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { } // process actuators - for (int i=0; i < nu; i++) { + for (int i=0; i < nactuator; i++) { + int uadr = m->actuator_ctrladr[i]; + int oadr = m->actuator_outadr[i]; + // skip if disabled if (mj_actuatorDisabled(m, i)) { continue; @@ -1233,8 +1236,8 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { // skip if force is clamped by forcerange if (m->actuator_forcelimited[i]) { - mjtNum force = d->actuator_force[i]; - mjtNum* range = m->actuator_forcerange + 2*i; + mjtNum force = d->actuator_force[oadr]; + mjtNum* range = m->actuator_forcerange + 2*oadr; if (force <= range[0] || force >= range[1]) { continue; } @@ -1267,10 +1270,10 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { // muscle gain else if (m->actuator_gaintype[i] == mjGAIN_MUSCLE) { - gain_vel = mjd_muscleGain_vel(d->actuator_length[i], - d->actuator_velocity[i], - m->actuator_lengthrange+2*i, - m->actuator_acc0[i], + gain_vel = mjd_muscleGain_vel(d->actuator_length[oadr], + d->actuator_velocity[oadr], + m->actuator_lengthrange+2*oadr, + m->actuator_acc0[oadr], m->actuator_gainprm + mjNGAIN*i); } @@ -1311,7 +1314,7 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { // force = gain .* [ctrl/act] if (gain_vel != 0) { if (m->actuator_dyntype[i] == mjDYN_NONE) { - bias_vel += gain_vel * d->ctrl[i]; + bias_vel += gain_vel * d->ctrl[uadr]; } else { int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1; mjtNum act = d->act[act_adr]; @@ -1327,7 +1330,7 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { // add if (bias_vel != 0) { - addJTBJSparse(m, d, d->actuator_moment, &bias_vel, 1, i, + addJTBJSparse(m, d, d->actuator_moment, &bias_vel, 1, oadr, d->moment_rownnz, d->moment_rowadr, d->moment_colind); } } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 9bec1185..28ad4960 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -197,10 +197,10 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { // actuator velocity: always sparse if (!mjDISABLED(mjDSBL_ACTUATION)) { - mju_mulMatVecSparse(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu, + mju_mulMatVecSparse(d->actuator_velocity, d->actuator_moment, d->qvel, m->nout, d->moment_rownnz, d->moment_rowadr, d->moment_colind, NULL); } else { - mju_zero(d->actuator_velocity, m->nu); + mju_zero(d->actuator_velocity, m->nout); } // com-based velocities, passive forces, constraint references @@ -264,17 +264,17 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtBool* limited, i // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot) void mj_fwdActuation(const mjModel* m, mjData* d) { TM_START; - int nv = m->nv, nu = m->nu, ntendon = m->ntendon; + int nv = m->nv, nu = m->nu, nactuator = m->nactuator, nout = m->nout, ntendon = m->ntendon; mjtNum gain, bias, tau; mjtNum *force = d->actuator_force; // clear actuator_force - mju_zero(force, nu); + mju_zero(force, nout); int sleep_filter = mjENABLED(mjENBL_SLEEP); // disabled or no actuation: return - if (nu == 0 || mjDISABLED(mjDSBL_ACTUATION)) { + if (nactuator == 0 || mjDISABLED(mjDSBL_ACTUATION)) { mju_zero(d->qfrc_actuator, nv); return; } @@ -287,9 +287,15 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum); // read from ctrl or history buffer for delayed actuators - for (int i = 0; i < nu; i++) { - int interp = m->actuator_history[2*i+1]; - ctrl[i] = m->actuator_delay[i] ? mj_readCtrl(m, d, i, d->time, interp) : d->ctrl[i]; + for (int i = 0; i < nactuator; i++) { + int adr = m->actuator_ctrladr[i]; + if (m->actuator_delay[i]) { + // delayed: read from history buffer (scalar input) + int interp = m->actuator_history[2*i+1]; + ctrl[adr] = mj_readCtrl(m, d, i, d->time, interp); + } else { + mju_copy(ctrl + adr, d->ctrl + adr, m->actuator_ctrlnum[i]); + } } // clamp local copy @@ -307,7 +313,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // act_dot for stateful actuators - for (int i=0; i < nu; i++) { + for (int i=0; i < nactuator; i++) { if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) { continue; } @@ -317,6 +323,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { continue; } + // addresses of the actuator's input and output blocks + int uadr = m->actuator_ctrladr[i]; + int oadr = m->actuator_outadr[i]; + // zero act_dot for actuator plugins int actnum = m->actuator_actnum[i]; if (actnum) { @@ -334,17 +344,17 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // compute act_dot according to dynamics type switch (dyntype) { case mjDYN_INTEGRATOR: // simple integrator - d->act_dot[act_last] = ctrl[i]; + d->act_dot[act_last] = ctrl[uadr]; break; case mjDYN_FILTER: // linear filter: dynprm = tau case mjDYN_FILTEREXACT: tau = mju_max(mjMINVAL, dynprm[0]); - d->act_dot[act_last] = (ctrl[i] - d->act[act_last]) / tau; + d->act_dot[act_last] = (ctrl[uadr] - d->act[act_last]) / tau; break; case mjDYN_MUSCLE: // muscle model: dynprm = (tau_act, tau_deact) - d->act_dot[act_last] = mju_muscleDynamics(ctrl[i], d->act[act_last], dynprm); + d->act_dot[act_last] = mju_muscleDynamics(ctrl[uadr], d->act[act_last], dynprm); break; case mjDYN_DCMOTOR: { // DC motor: up to 5 optional states @@ -356,7 +366,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } int adr = act_first; - mjtNum velocity = d->actuator_velocity[i]; + mjtNum velocity = d->actuator_velocity[oadr]; mjtNum R = gainprm[0]; // resistance mjtNum K = gainprm[1]; // motor constant mjtNum ki = gainprm[5]; // integral gain @@ -369,9 +379,9 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { if (slew_s > 0) { mjtNum u_prev = d->act[adr]; mjtNum slew = slew_s * m->opt.timestep; - mjtNum u_eff = mju_clip(ctrl[i], u_prev - slew, u_prev + slew); + mjtNum u_eff = mju_clip(ctrl[uadr], u_prev - slew, u_prev + slew); d->act_dot[adr] = (u_eff - u_prev) / m->opt.timestep; - ctrl[i] = u_eff; + ctrl[uadr] = u_eff; adr++; } @@ -381,11 +391,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { x_I = d->act[adr]; int input_mode = (int)gainprm[8]; mjtNum Imax = dynprm[8]; // integral clamp - mjtNum act_dot = ctrl[i]; // default raw accumulator for voltage and velocity modes + mjtNum act_dot = ctrl[uadr]; // default raw accumulator for voltage and velocity modes // position mode if (input_mode == 1) { - act_dot = ctrl[i] - d->actuator_length[i]; + act_dot = ctrl[uadr] - d->actuator_length[oadr]; } // clamp act_dot based on integral state @@ -401,7 +411,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // compute physical voltage to feed into current and temperature equations - mjtNum V = dcmotorVoltage(ctrl[i], d->actuator_length[i], velocity, x_I, gainprm); + mjtNum V = dcmotorVoltage(ctrl[uadr], d->actuator_length[oadr], velocity, x_I, gainprm); // temperature: dT/dt = (R*i^2 - T/RT) / C, where T = delta above ambient mjtNum RT = dynprm[2]; // thermal resistance @@ -476,7 +486,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // force = gain .* [ctrl/act] + bias - for (int i=0; i < nu; i++) { + for (int i=0; i < nactuator; i++) { // skip if sleeping if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) { continue; @@ -492,6 +502,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { continue; } + // addresses of the actuator's input and output blocks + int uadr = m->actuator_ctrladr[i]; + int oadr = m->actuator_outadr[i]; + // 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]]; @@ -510,14 +524,15 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { break; case mjGAIN_AFFINE: // affine: prm = [const, kp, kv] - gain = gainprm[0] + gainprm[1]*d->actuator_length[i] + gainprm[2]*d->actuator_velocity[i]; + gain = gainprm[0] + gainprm[1]*d->actuator_length[oadr] + + gainprm[2]*d->actuator_velocity[oadr]; break; case mjGAIN_MUSCLE: // muscle gain - gain = mju_muscleGain(d->actuator_length[i], - d->actuator_velocity[i], - m->actuator_lengthrange+2*i, - m->actuator_acc0[i], + gain = mju_muscleGain(d->actuator_length[oadr], + d->actuator_velocity[oadr], + m->actuator_lengthrange+2*oadr, + m->actuator_acc0[oadr], gainprm); break; @@ -546,11 +561,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // stateless: gain = K/R, force = K/R * ctrl (condition below) gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R); - // controller: compute voltage, override ctrl[i] for force computation + // controller: compute voltage, override ctrl[uadr] for force computation if ((int)gainprm[8] > 0) { mjtNum x_I = (slots.integral >= 0) ? d->act[adr + slots.integral] : 0; - ctrl[i] = dcmotorVoltage(ctrl[i], d->actuator_length[i], - d->actuator_velocity[i], x_I, gainprm); + ctrl[uadr] = dcmotorVoltage(ctrl[uadr], d->actuator_length[oadr], + d->actuator_velocity[oadr], x_I, gainprm); } break; } @@ -568,7 +583,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // DC motor without current state: use ctrl even if other activations exist int dcmotor_no_current = (gaintype == mjGAIN_DCMOTOR && dynprm[0] <= 0); if (actnum == 0 || dcmotor_no_current) { - force[i] = gain * ctrl[i]; + force[oadr] = gain * ctrl[uadr]; } else { // use last activation variable associated with actuator i int act_adr = m->actuator_actadr[i] + actnum - 1; @@ -579,7 +594,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } else { act = d->act[act_adr]; } - force[i] = gain * act; + force[oadr] = gain * act; } // extract bias info @@ -593,13 +608,14 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { break; case mjBIAS_AFFINE: // affine: biasprm = [const, kp, kv] - bias = biasprm[0] + biasprm[1]*d->actuator_length[i] + biasprm[2]*d->actuator_velocity[i]; + bias = biasprm[0] + biasprm[1]*d->actuator_length[oadr] + + biasprm[2]*d->actuator_velocity[oadr]; break; case mjBIAS_MUSCLE: // muscle passive force - bias = mju_muscleBias(d->actuator_length[i], - m->actuator_lengthrange+2*i, - m->actuator_acc0[i], + bias = mju_muscleBias(d->actuator_length[oadr], + m->actuator_lengthrange+2*oadr, + m->actuator_acc0[oadr], biasprm); break; @@ -610,7 +626,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { mjtNum te = m->actuator_dynprm[mjNDYN*i]; // electrical time constant if (te <= 0) { mjtNum K = gainprm[1]; // motor constant - bias -= gain * K * d->actuator_velocity[i]; + bias -= gain * K * d->actuator_velocity[oadr]; } break; } @@ -624,7 +640,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // add bias - force[i] += bias; + force[oadr] += bias; } // handle actuator plugins @@ -650,17 +666,17 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // 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++) { + for (int i=0; i < nactuator; 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]; + tendon_total_force[tendon_id] += force[m->actuator_outadr[i]]; } } } // scale tendon actuator forces if limited and outside range - for (int i=0; i < nu; i++) { + for (int i=0; i < nactuator; i++) { if (m->actuator_trntype[i] != mjTRN_TENDON) { continue; } @@ -669,19 +685,19 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { 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; + force[m->actuator_outadr[i]] *= range[0] / tendon_force; } else if (tendon_force > range[1]) { - force[i] *= range[1] / tendon_force; + force[m->actuator_outadr[i]] *= range[1] / tendon_force; } } } } // clamp actuator_force - clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL); + clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nout, NULL); // add DC motor mechanical forces (not subject to current limits) - for (int i=0; i < nu; i++) { + for (int i=0; i < nactuator; i++) { if (m->actuator_biastype[i] != mjBIAS_DCMOTOR) { continue; } @@ -694,13 +710,14 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i; const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; + int oadr = m->actuator_outadr[i]; // cogging torque mjtNum A = biasprm[0]; if (A != 0) { mjtNum Np = biasprm[1]; mjtNum phi = biasprm[2]; - force[i] += A * mju_sin(Np*d->actuator_length[i] + phi); + force[oadr] += A * mju_sin(Np*d->actuator_length[oadr] + phi); } // LuGre friction @@ -711,12 +728,12 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { int adr = m->actuator_actadr[i] + slots.bristle; mjtNum z = d->act[adr]; mjtNum z_dot = d->act_dot[adr]; - force[i] -= sigma0 * z + sigma1 * z_dot; + force[oadr] -= sigma0 * z + sigma1 * z_dot; } } // qfrc_actuator = moment' * force - mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nu, nv, + mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nout, nv, d->moment_rownnz, d->moment_rowadr, d->moment_colind); // actuator-level gravity compensation @@ -980,18 +997,18 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) { // qvel: optional velocity used for position integration; if NULL, use d->qvel static void mj_advance(const mjModel* m, mjData* d, const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) { - int nu = m->nu, nsensor = m->nsensor; + int nactuator = m->nactuator, nsensor = m->nsensor; // advance history buffers if (m->nhistory > 0) { // advance ctrl history buffers - for (int i = 0; i < nu; i++) { + for (int i = 0; i < nactuator; i++) { int nsample = m->actuator_history[2*i]; if (nsample == 0) continue; // get history buffer pointer and insert ctrl at current time mjtNum* buf = d->history + m->actuator_historyadr[i]; - *mju_historyInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[i]; + *mju_historyInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[m->actuator_ctrladr[i]]; } // advance sensor history buffers @@ -1033,7 +1050,7 @@ static void mj_advance(const mjModel* m, mjData* d, // advance activations if (m->na && !mjDISABLED(mjDSBL_ACTUATION)) { - for (int i=0; i < nu; i++) { + for (int i=0; i < nactuator; i++) { int actadr = m->actuator_actadr[i]; int actadr_end = actadr + m->actuator_actnum[i]; for (int j=actadr; j < actadr_end; j++) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index c312a424..4b909641 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -225,7 +225,8 @@ static void freeModelBuffers(mjModel* m) { // allocate and initialize mjModel structure void mj_makeModel(mjModel** dest, - mjtSize nq, mjtSize nv, mjtSize nu, mjtSize na, mjtSize nbody, mjtSize nbvh, mjtSize nbvhstatic, + mjtSize nq, mjtSize nv, mjtSize nu, mjtSize nactuator, mjtSize nout, mjtSize na, + mjtSize nbody, mjtSize nbvh, mjtSize nbvhstatic, mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB, mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight, mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem, @@ -297,6 +298,8 @@ void mj_makeModel(mjModel** dest, m->nq = nq; m->nv = nv; m->nu = nu; + m->nactuator = nactuator; + m->nout = nout; m->na = na; m->nbody = nbody; m->nbvh = nbvh; @@ -374,7 +377,7 @@ void mj_makeModel(mjModel** dest, m->nuser_sensor = nuser_sensor; m->nnames = nnames; long nnames_map = (long)nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh + nskin + - nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nu + nsensor + + nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nactuator + nsensor + nnumeric + ntext + ntuple + nkey + nplugin; if (nnames_map >= INT_MAX / mjLOAD_MULTIPLE) { if (allocate) mju_free(m); @@ -428,7 +431,8 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { // allocate new model if needed if (!dest) { mj_makeModel( - &dest, src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, src->nbvhstatic, + &dest, src->nq, src->nv, src->nu, src->nactuator, src->nout, src->na, + src->nbody, src->nbvh, src->nbvhstatic, src->nbvhdynamic, src->noct, src->njnt, src->ntree, src->nM, src->nB, src->nC, src->nD, src->ngeom, src->nsite, src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge, src->nflexelem, src->nflexelemdata, src->nflexstiffness, @@ -611,7 +615,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) { sizes[56], sizes[57], sizes[58], sizes[59], sizes[60], sizes[61], sizes[62], sizes[63], sizes[64], sizes[65], sizes[66], sizes[67], sizes[68], sizes[69], sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76], - sizes[77], sizes[78], sizes[79]); + sizes[77], sizes[78], sizes[79], sizes[80], sizes[81]); // mj_makeModel may fail if the input buffer has invalid sizes if (!m) { @@ -1384,7 +1388,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { mju_zero(d->mocap_quat, 4*m->nmocap); // initialize ctrl history buffers: timestamps at [-n*dt, ..., -dt] - for (int i = 0; i < m->nu; i++) { + for (int i = 0; i < m->nactuator; i++) { int n = m->actuator_history[2*i]; if (n > 0) { mjtNum* buf = d->history + m->actuator_historyadr[i]; @@ -1785,7 +1789,7 @@ static int numObjects(const mjModel* m, mjtObj objtype) { case mjOBJ_TENDON: return m->ntendon; case mjOBJ_ACTUATOR: - return m->nu; + return m->nactuator; case mjOBJ_SENSOR: return m->nsensor; case mjOBJ_NUMERIC: @@ -1871,8 +1875,10 @@ const char* mj_validateReferences(const mjModel* m) { X(skin_bonevertid, nskinbonevert, nskinvert , 0 ) \ X(pair_geom1, npair, ngeom , 0 ) \ X(pair_geom2, npair, ngeom , 0 ) \ - X(actuator_plugin, nu, nplugin , 0 ) \ - X(actuator_actadr, nu, na , m->actuator_actnum ) \ + X(actuator_plugin, nactuator, nplugin , 0 ) \ + X(actuator_actadr, nactuator, na , m->actuator_actnum ) \ + X(actuator_ctrladr, nactuator, nu , m->actuator_ctrlnum ) \ + X(actuator_outadr, nactuator, nout , m->actuator_outnum ) \ X(sensor_plugin, nsensor, nplugin , 0 ) \ X(plugin_stateadr, nplugin, npluginstate , m->plugin_statenum ) \ X(plugin_attradr, nplugin, npluginattr , 0 ) \ @@ -1897,7 +1903,7 @@ const char* mj_validateReferences(const mjModel* m) { X(name_excludeadr, nexclude, nnames , 0 ) \ X(name_eqadr, neq, nnames , 0 ) \ X(name_tendonadr, ntendon, nnames , 0 ) \ - X(name_actuatoradr, nu, nnames , 0 ) \ + X(name_actuatoradr, nactuator, nnames , 0 ) \ X(name_sensoradr, nsensor, nnames , 0 ) \ X(name_numericadr, nnumeric, nnames , 0 ) \ X(name_textadr, ntext, nnames , 0 ) \ @@ -2085,7 +2091,7 @@ const char* mj_validateReferences(const mjModel* m) { break; } } - for (int i=0; i < m->nu; i++) { + for (int i=0; i < m->nactuator; i++) { int actuator_trntype = m->actuator_trntype[i]; int id = m->actuator_trnid[2*i]; int idslider = m->actuator_trnid[2*i+1]; diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index dccd55a4..7a8c57fa 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -48,7 +48,8 @@ void mj_defaultStatistic(mjStatistic* stat); // allocate mjModel void mj_makeModel(mjModel** dest, - mjtSize nq, mjtSize nv, mjtSize nu, mjtSize na, mjtSize nbody, mjtSize nbvh, mjtSize nbvhstatic, + mjtSize nq, mjtSize nv, mjtSize nu, mjtSize nactuator, mjtSize nout, mjtSize na, + mjtSize nbody, mjtSize nbvh, mjtSize nbvhstatic, mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB, mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight, mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem, diff --git a/src/engine/engine_name.c b/src/engine/engine_name.c index 71b48dd1..1dcec37b 100644 --- a/src/engine/engine_name.c +++ b/src/engine/engine_name.c @@ -160,10 +160,10 @@ static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) { mjFALLTHROUGH; case mjOBJ_ACTUATOR: - *mapadr -= mjLOAD_MULTIPLE*m->nu; + *mapadr -= mjLOAD_MULTIPLE*m->nactuator; if (num < 0) { *padr = m->name_actuatoradr; - num = m->nu; + num = m->nactuator; } mjFALLTHROUGH; diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index df259f2a..e21241bd 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -503,6 +503,7 @@ static bool validateFloatFormat(const char* float_format) { // print mjModel to text file, specifying format. float_format must be a // valid printf-style format string for a single float value +// NOLINTBEGIN(readability/fn_size) void mj_printFormattedModel(const mjModel* m, const char* filename, const char* float_format) { // get file FILE* fp; @@ -938,15 +939,35 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* } if (m->ntendon) fprintf(fp, "\n"); - // actuators - object_class = &m->nu; - for (int i=0; i < m->nu; i++) { + // actuators: per-actuator fields, then per-input (nu) and per-output (nout) blocks + for (int i=0; i < m->nactuator; i++) { fprintf(fp, "\nACTUATOR %d:\n", i); fprintf(fp, " " NAME_FORMAT, "name"); fprintf(fp, " %s\n", m->names + m->name_actuatoradr[i]); + object_class = &m->nactuator; MJMODEL_POINTERS_ACTUATOR + { + int actuator = i; + int ctrladr = m->actuator_ctrladr[actuator]; + int ctrlnum = m->actuator_ctrlnum[actuator]; + int outadr = m->actuator_outadr[actuator]; + int outnum = m->actuator_outnum[actuator]; + object_class = &m->nu; + i = ctrladr; + while (i < ctrladr + ctrlnum) { + MJMODEL_POINTERS_ACTUATOR + i++; + } + object_class = &m->nout; + i = outadr; + while (i < outadr + outnum) { + MJMODEL_POINTERS_ACTUATOR + i++; + } + i = actuator; + } } - if (m->nu) fprintf(fp, "\n"); + if (m->nactuator) fprintf(fp, "\n"); // sensors object_class = &m->nsensor; @@ -1170,6 +1191,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* fclose(fp); } } +// NOLINTEND(readability/fn_size) // print mjModel to text file void mj_printModel(const mjModel* m, const char* filename) { @@ -1335,7 +1357,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena fprintf(fp, "DELAY\n"); // actuator history buffers - for (int i = 0; i < m->nu; i++) { + for (int i = 0; i < m->nactuator; i++) { int adr = m->actuator_historyadr[i]; if (adr >= 0) { char name[100]; @@ -1423,10 +1445,10 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena fprintf(fp, "\n"); } - printArray2d("ACTUATOR_LENGTH", m->nu, 1, d->actuator_length, fp, float_format); - mj_printSparsity("actuator_moment", m->nu, m->nv, + printArray2d("ACTUATOR_LENGTH", m->nout, 1, d->actuator_length, fp, float_format); + mj_printSparsity("actuator_moment", m->nout, m->nv, d->moment_rowadr, NULL, d->moment_rownnz, NULL, d->moment_colind, fp); - printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nu, d->moment_rownnz, + printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nout, d->moment_rownnz, d->moment_rowadr, d->moment_colind, fp, float_format); printArray2d("CRB", m->nbody, 10, d->crb, fp, float_format); printSparse("M", d->M, m->nv, m->M_rownnz, @@ -1568,7 +1590,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena printArray2d("FLEXEDGE_VELOCITY", m->nflexedge, 1, d->flexedge_velocity, fp, float_format); printArray2d("TEN_VELOCITY", m->ntendon, 1, d->ten_velocity, fp, float_format); - printArray2d("ACTUATOR_VELOCITY", m->nu, 1, d->actuator_velocity, fp, float_format); + printArray2d("ACTUATOR_VELOCITY", m->nout, 1, d->actuator_velocity, fp, float_format); printArray2d("CVEL", m->nbody, 6, d->cvel, fp, float_format); printArray2d("CDOF_DOT", m->nv, 6, d->cdof_dot, fp, float_format); @@ -1587,7 +1609,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena printArray2d("SUBTREE_LINVEL", m->nbody, 3, d->subtree_linvel, fp, float_format); printArray2d("SUBTREE_ANGMOM", m->nbody, 3, d->subtree_angmom, fp, float_format); - printArray2d("ACTUATOR_FORCE", m->nu, 1, d->actuator_force, fp, float_format); + printArray2d("ACTUATOR_FORCE", m->nout, 1, d->actuator_force, fp, float_format); printArray2d("QFRC_ACTUATOR", m->nv, 1, d->qfrc_actuator, fp, float_format); printArray2d("QFRC_SMOOTH", m->nv, 1, d->qfrc_smooth, fp, float_format); diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index fe28a292..0f11af4c 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -650,7 +650,7 @@ static void mj_computeSensorPos(const mjModel* m, mjData* d, int i, mjtNum* sens break; case mjSENS_ACTUATORPOS: // actuator position - sensordata[0] = d->actuator_length[objid]; + sensordata[0] = d->actuator_length[m->actuator_outadr[objid]]; break; case mjSENS_BALLQUAT: // ball joint quaternion @@ -879,7 +879,7 @@ static void mj_computeSensorVel(const mjModel* m, mjData* d, int i, mjtNum* sens break; case mjSENS_ACTUATORVEL: // actuator velocity - sensordata[0] = d->actuator_velocity[objid]; + sensordata[0] = d->actuator_velocity[m->actuator_outadr[objid]]; break; case mjSENS_BALLANGVEL: // ball joint angular velocity @@ -956,7 +956,7 @@ static void mj_computeSensorVel(const mjModel* m, mjData* d, int i, mjtNum* sens // compute acceleration-stage sensor value, write to data buffer static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sensordata) { - int ne = d->ne, nf = d->nf, nefc = d->nefc, nu = m->nu; + int ne = d->ne, nf = d->nf, nefc = d->nefc, nactuator = m->nactuator; mjtSensor type = (mjtSensor)m->sensor_type[i]; int objtype = m->sensor_objtype[i]; int objid = m->sensor_objid[i]; @@ -1303,7 +1303,7 @@ static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sens break; case mjSENS_ACTUATORFRC: // actuator force - sensordata[0] = d->actuator_force[objid]; + sensordata[0] = d->actuator_force[m->actuator_outadr[objid]]; break; case mjSENS_JOINTACTFRC: // actuator force at joint @@ -1312,9 +1312,9 @@ static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sens case mjSENS_TENDONACTFRC: // actuator force at tendon frc = 0.0; - for (int j=0; j < nu; j++) { + for (int j=0; j < nactuator; j++) { if (m->actuator_trntype[j] == mjTRN_TENDON && m->actuator_trnid[2*j] == objid) { - frc += d->actuator_force[j]; + frc += d->actuator_force[m->actuator_outadr[j]]; } } sensordata[0] = frc; diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 096f7651..1da3d0a4 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -212,7 +212,7 @@ static void setFixed(mjModel* m, mjData* d) { // set jnt_actuatorid and tendon_actuatorid mju_fillInt(m->jnt_actuatorid, -1, m->njnt); mju_fillInt(m->tendon_actuatorid, -1, m->ntendon); - for (int i=0; i < m->nu; i++) { + for (int i=0; i < m->nactuator; i++) { // skip actuator with no damping and no armature if (m->actuator_damping[i] == 0 && mju_isZero(m->actuator_dampingpoly+mjNPOLY*i, mjNPOLY) && @@ -312,7 +312,7 @@ static void setFixed(mjModel* m, mjData* d) { // ----- apply compiler AUTO tree sleep policy // actuators: trees with any actuated joint, site, body, or tendon do not auto-sleep - for (int i=0; i < m->nu; i++) { + for (int i=0; i < m->nactuator; i++) { int bodyid = -1; int tid = m->actuator_trnid[2*i]; switch ((mjtTrn)m->actuator_trntype[i]) { @@ -868,7 +868,7 @@ static void set0(mjModel* m, mjData* d) { // copy fields mju_copy(m->flexedge_length0, d->flexedge_length, m->nflexedge); mju_copy(m->tendon_length0, d->ten_length, m->ntendon); - mju_copy(m->actuator_length0, d->actuator_length, m->nu); + mju_copy(m->actuator_length0, d->actuator_length, m->nout); // compute body_invweight0 m->body_invweight0[0] = m->body_invweight0[1] = 0.0; @@ -1002,8 +1002,8 @@ static void set0(mjModel* m, mjData* d) { m->tendon_invweight0[i] = mju_dot(tmp, tmp+nv, nv); } - // compute actuator_acc0 - for (int i=0; i < m->nu; i++) { + // compute actuator_acc0, one per force output (moment row) + for (int i=0; i < m->nout; i++) { mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + i, d->moment_rowadr + i, d->moment_colind); mj_solveM(m, d, tmp, moment, 1); @@ -1011,7 +1011,7 @@ static void set0(mjModel* m, mjData* d) { } } else { mju_zero(m->tendon_invweight0, m->ntendon); - mju_zero(m->actuator_acc0, m->nu); + mju_zero(m->actuator_acc0, m->nout); } // compute missing eq_data for body constraints @@ -1101,7 +1101,7 @@ static void set0(mjModel* m, mjData* d) { } // compute actuator damping from dampratio - for (int i=0; i < m->nu; i++) { + for (int i=0; i < m->nactuator; i++) { // get bias, gain parameters mjtNum* biasprm = m->actuator_biasprm + i*mjNBIAS; mjtNum* gainprm = m->actuator_gainprm + i*mjNGAIN; @@ -1119,8 +1119,8 @@ static void set0(mjModel* m, mjData* d) { // === interpret biasprm[2] > 0 as dampratio for position-like actuators // "reflected" inertia (inversely scaled by transmission squared) - int rownnz = d->moment_rownnz[i]; - int rowadr = d->moment_rowadr[i]; + int rownnz = d->moment_rownnz[m->actuator_outadr[i]]; + int rowadr = d->moment_rowadr[m->actuator_outadr[i]]; mjtNum* transmission = d->actuator_moment + rowadr; mjtNum mass = 0; for (int j=0; j < rownnz; j++) { @@ -1363,6 +1363,7 @@ void mj_setConst(mjModel* m, mjData* d) { static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, const mjLROpt* opt) { int nv = m->nv; + int out = m->actuator_outadr[index]; // reduce velocity mju_scl(d->qvel, d->qvel, mju_exp(-m->opt.timestep/mjMAX(0.01, opt->timeconst)), nv); @@ -1373,8 +1374,8 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, // dense actuator_moment row mj_markStack(d); mjtNum* moment = mjSTACKALLOC(d, nv, mjtNum); - mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + index, - d->moment_rowadr + index, d->moment_colind); + mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + out, + d->moment_rowadr + out, d->moment_colind); // set force to generate desired acceleration mj_solveM(m, d, d->qfrc_applied, moment, 1); @@ -1393,7 +1394,7 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, mj_freeStack(d); // return actuator length - return d->actuator_length[index]; + return d->actuator_length[out]; } @@ -1401,9 +1402,10 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, int mj_setLengthRange(mjModel* m, mjData* d, int index, const mjLROpt* opt, char* error, int error_sz) { // check index - if (index < 0 || index >= m->nu) { + if (index < 0 || index >= m->nactuator) { mjERROR("invalid actuator index"); } + int out = m->actuator_outadr[index]; // skip depending on mode and type int ismuscle = (m->actuator_gaintype[index] == mjGAIN_MUSCLE || @@ -1417,7 +1419,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, } // use existing length range if available - if (opt->useexisting && (m->actuator_lengthrange[2*index] < m->actuator_lengthrange[2*index+1])) { + if (opt->useexisting && (m->actuator_lengthrange[2*out] < m->actuator_lengthrange[2*out+1])) { return 1; } @@ -1432,8 +1434,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, // make sure joint is limited if (m->jnt_limited[threadid]) { // copy range - m->actuator_lengthrange[2*index] = m->jnt_range[2*threadid]; - m->actuator_lengthrange[2*index+1] = m->jnt_range[2*threadid+1]; + m->actuator_lengthrange[2*out] = m->jnt_range[2*threadid]; + m->actuator_lengthrange[2*out+1] = m->jnt_range[2*threadid+1]; // skip optimization return 1; @@ -1445,8 +1447,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, // make sure tendon is limited if (m->tendon_limited[threadid]) { // copy range - m->actuator_lengthrange[2*index] = m->tendon_range[2*threadid]; - m->actuator_lengthrange[2*index+1] = m->tendon_range[2*threadid+1]; + m->actuator_lengthrange[2*out] = m->tendon_range[2*threadid]; + m->actuator_lengthrange[2*out+1] = m->tendon_range[2*threadid+1]; // skip optimization return 1; @@ -1491,12 +1493,12 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, } // check range - mjtNum dif = m->actuator_lengthrange[2*index+1] - m->actuator_lengthrange[2*index]; + mjtNum dif = m->actuator_lengthrange[2*out+1] - m->actuator_lengthrange[2*out]; if (dif <= 0) { snprintf(error, error_sz, "Invalid lengthrange (%g, %g) in actuator %d", - m->actuator_lengthrange[2*index], - m->actuator_lengthrange[2*index+1], index); + m->actuator_lengthrange[2*out], + m->actuator_lengthrange[2*out+1], index); return 0; } @@ -1506,8 +1508,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, "Lengthrange computation did not converge in actuator %d:\n" " eval (%g, %g)\n range (%g, %g)", index, lmin[0], lmax[0], - m->actuator_lengthrange[2*index], - m->actuator_lengthrange[2*index+1]); + m->actuator_lengthrange[2*out], + m->actuator_lengthrange[2*out+1]); return 0; } @@ -1517,8 +1519,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, "Lengthrange computation did not converge in actuator %d:\n" " eval (%g, %g)\n range (%g, %g)", index, lmin[1], lmax[1], - m->actuator_lengthrange[2*index], - m->actuator_lengthrange[2*index+1]); + m->actuator_lengthrange[2*out], + m->actuator_lengthrange[2*out+1]); return 0; } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index b999c7b5..75fb7fce 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -885,7 +885,7 @@ void mju_camIntrinsics(const mjModel* m, int camid, // read delayed ctrl value for actuator at given time mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int interp) { // validate actuator id - if (id < 0 || id >= m->nu) { + if (id < 0 || id >= m->nactuator) { mjERROR("invalid actuator id %d", id); return 0; } @@ -893,7 +893,7 @@ mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int i // no delay: return current ctrl value int nsample = m->actuator_history[2*id]; if (nsample == 0) { - return d->ctrl[id]; + return d->ctrl[m->actuator_ctrladr[id]]; } // resolve interpolation order: use model's interp if argument is -1 @@ -938,7 +938,7 @@ const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum ti void mj_initCtrlHistory(const mjModel* m, mjData* d, int id, const mjtNum* times, const mjtNum* values) { // validate actuator id - if (id < 0 || id >= m->nu) { + if (id < 0 || id >= m->nactuator) { mjERROR("invalid actuator id %d", id); return; } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 6350da14..2a1db45d 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1116,7 +1116,7 @@ int mjv_isCatenary(const mjModel* m, const mjData* d, int i, mjtNum* length) { // no actuator if (draw_catenary) { - for (int j=0; j < m->nu; j++) { + for (int j=0; j < m->nactuator; j++) { if (m->actuator_trntype[j] == mjTRN_TENDON && m->actuator_trnid[2*j] == i) { draw_catenary = 0; break; @@ -1271,7 +1271,7 @@ static void addSliderCrankGeoms(const mjModel* m, mjData* d, const mjvOption* vo } const float scl = m->stat.meansize; - for (int i=0; i < m->nu; i++) { + for (int i=0; i < m->nactuator; i++) { if (m->actuator_trntype[i] == mjTRN_SLIDERCRANK) { // get data int j = m->actuator_trnid[2*i]; // crank @@ -2077,7 +2077,7 @@ static void addActuatorGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } const float scl = m->stat.meansize; - for (int i=0; i < m->nu; i++) { + for (int i=0; i < m->nactuator; i++) { if (!vopt->actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) { continue; } @@ -2088,9 +2088,9 @@ static void addActuatorGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // determine extended range mjtNum rng[3] = {-1, 0, +1}; mjtNum rmin = -1, rmax = 1, act = 0; - if (m->actuator_ctrllimited[i]) { - rmin = m->actuator_ctrlrange[2*i]; - rmax = m->actuator_ctrlrange[2*i+1]; + if (m->actuator_ctrllimited[m->actuator_ctrladr[i]]) { + rmin = m->actuator_ctrlrange[2*m->actuator_ctrladr[i]]; + rmax = m->actuator_ctrlrange[2*m->actuator_ctrladr[i]+1]; } else if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_actlimited[i]) { rmin = m->actuator_actrange[2*i]; rmax = m->actuator_actrange[2*i+1]; @@ -2121,7 +2121,7 @@ static void addActuatorGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_dyntype[i]) { act = mju_clip(d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1], rng[0], rng[2]); } else { - act = mju_clip(d->ctrl[i], rng[0], rng[2]); + act = mju_clip(d->ctrl[m->actuator_ctrladr[i]], rng[0], rng[2]); } // compute interpolants diff --git a/src/user/user_model.cc b/src/user/user_model.cc index d71ca7c9..5ad63b23 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -330,6 +330,8 @@ void mjCModel::SaveDofOffsets(bool computesize) { int qposadr = 0; int dofadr = 0; int actadr = 0; + int ctrladr = 0; + int outadr = 0; int mocapadr = 0; for (auto joint : joints_) { @@ -347,6 +349,12 @@ void mjCModel::SaveDofOffsets(bool computesize) { } actuator->actadr_ = actuator->actdim_ ? actadr : -1; actadr += actuator->actdim_; + + // input and output blocks; all actuator types are currently 1x1 + actuator->ctrladr_ = ctrladr; + ctrladr += actuator->ctrlnum_; + actuator->outadr_ = outadr; + outadr += actuator->outnum_; } for (mjCBody* body : bodies_) { @@ -361,7 +369,9 @@ void mjCModel::SaveDofOffsets(bool computesize) { nq = qposadr; nv = dofadr; na = actadr; - nu = (int)actuators_.size(); + nu = ctrladr; + nactuator = (int)actuators_.size(); + nout = outadr; nmocap = mocapadr; } } @@ -1192,6 +1202,8 @@ void mjCModel::Clear() { nq = 0; nv = 0; nu = 0; + nactuator = 0; + nout = 0; na = 0; nflexnode = 0; nflexvert = 0; @@ -2166,7 +2178,7 @@ void mjCModel::SetSizes() { ntuple = (int)tuples_.size(); nkey = (int)keys_.size(); nplugin = (int)plugins_.size(); - nq = nv = ntree = nu = na = nmocap = 0; + nq = nv = ntree = nu = nactuator = nout = na = nmocap = 0; // nq, nv, ntree for (int i=0; i < njnt; i++) { @@ -2195,9 +2207,11 @@ void mjCModel::SetSizes() { } } - // nu, na + // nu, nactuator, nout, na; all actuator types are currently 1x1 for (int i=0; i < actuators_.size(); i++) { - nu++; + nactuator++; + nu += actuators_[i]->ctrlnum_; + nout += actuators_[i]->outnum_; na += actuators_[i]->actdim; } @@ -2373,7 +2387,7 @@ void mjCModel::SetSizes() { for (int i=0; i < nexclude; i++) nnames += (int)excludes_[i]->name.length() + 1; for (int i=0; i < neq; i++) nnames += (int)equalities_[i]->name.length() + 1; for (int i=0; i < ntendon; i++) nnames += (int)tendons_[i]->name.length() + 1; - for (int i=0; i < nu; i++) nnames += (int)actuators_[i]->name.length() + 1; + for (int i=0; i < nactuator; i++) nnames += (int)actuators_[i]->name.length() + 1; for (int i=0; i < nsensor; i++) nnames += (int)sensors_[i]->name.length() + 1; for (int i=0; i < nnumeric; i++) nnames += (int)numerics_[i]->name.length() + 1; for (int i=0; i < ntext; i++) nnames += (int)texts_[i]->name.length() + 1; @@ -2465,7 +2479,7 @@ void* LRfunc(void* arg) { LRThreadArg* larg = (LRThreadArg*)arg; for (int i=larg->start; i < larg->start+larg->num; i++) { - if (i < larg->m->nu) { + if (i < larg->m->nactuator) { if (!mj_setLengthRange(larg->m, larg->data, i, larg->LRopt, larg->error, larg->error_sz)) { return nullptr; } @@ -2488,7 +2502,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) { // count actuators that need computation int cnt = 0; - for (int i=0; i < m->nu; i++) { + for (int i=0; i < m->nactuator; i++) { // skip depending on mode and type int ismuscle = (m->actuator_gaintype[i] == mjGAIN_MUSCLE || m->actuator_biastype[i] == mjBIAS_MUSCLE); @@ -2515,7 +2529,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) { // single thread if (!compiler.usethread || cnt < 2 || nthread < 2) { char err[200]; - for (int i=0; i < m->nu; i++) { + for (int i=0; i < m->nactuator; i++) { if (!mj_setLengthRange(m, data, i, &compiler.LRopt, err, 200)) { throw mjCError(0, "%s", err); } @@ -2535,8 +2549,8 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) { } // number of actuators per thread - int num = m->nu / nthread; - while (num*nthread < m->nu) { + int num = m->nactuator / nthread; + while (num*nthread < m->nactuator) { num++; } @@ -3178,7 +3192,7 @@ void mjCModel::CopyPlugins(mjModel* m) { { // set actuator_plugin to the plugin instance ID std::vector > plugin_to_actuators(nplugin); - for (int i = 0; i < nu; ++i) { + for (int i = 0; i < nactuator; ++i) { if (actuators_[i]->plugin.active) { int actuator_plugin = static_cast(actuators_[i]->plugin.element)->id; m->actuator_plugin[i] = actuator_plugin; @@ -3287,11 +3301,11 @@ int mjCModel::CountTendonDofs(const mjModel* m, int id) { } int mjCModel::CountNJmom(const mjModel* m) { - int nu = m->nu; + int nactuator = m->nactuator; int nv = m->nv; int count = 0; - for (int i = 0; i < nu; i++) { + for (int i = 0; i < nactuator; i++) { // extract info int id = m->actuator_trnid[2 * i]; @@ -3363,6 +3377,7 @@ int mjCModel::CountNJten(const mjModel* m) { } // copy objects outside kinematic tree +// NOLINTBEGIN(readability/fn_size) void mjCModel::CopyObjects(mjModel* m) { mjtSize adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr, oct_adr; mjtSize stiffness_adr, bending_adr; @@ -3905,7 +3920,9 @@ void mjCModel::CopyObjects(mjModel* m) { // actuators adr = 0; int delay_adr = 0; - for (int i=0; i < nu; i++) { + int ctrladr = 0; + int outadr = 0; + for (int i=0; i < nactuator; i++) { // get pointer mjCActuator* pac = actuators_[i]; @@ -3923,6 +3940,16 @@ void mjCModel::CopyObjects(mjModel* m) { adr += m->actuator_actnum[i]; m->actuator_group[i] = pac->group; + // input and output blocks; all actuator types are currently 1x1 + m->actuator_ctrladr[i] = ctrladr; + m->actuator_ctrlnum[i] = pac->ctrlnum_; + pac->ctrladr_ = ctrladr; + ctrladr += pac->ctrlnum_; + m->actuator_outadr[i] = outadr; + m->actuator_outnum[i] = pac->outnum_; + pac->outadr_ = outadr; + outadr += pac->outnum_; + // historyadr m->actuator_delay[i] = (mjtNum)pac->delay; m->actuator_history[2*i] = pac->nsample; @@ -3934,23 +3961,32 @@ void mjCModel::CopyObjects(mjModel* m) { m->actuator_historyadr[i] = -1; } - m->actuator_ctrllimited[i] = (mjtBool)pac->is_ctrllimited(); - m->actuator_forcelimited[i] = (mjtBool)pac->is_forcelimited(); m->actuator_actlimited[i] = (mjtBool)pac->is_actlimited(); m->actuator_actearly[i] = pac->actearly; m->actuator_cranklength[i] = (mjtNum)pac->cranklength; - mjuu_copyvec(m->actuator_gear + 6*i, pac->gear, 6); m->actuator_damping[i] = (mjtNum)pac->damping[0]; mjuu_copyvec(m->actuator_dampingpoly + mjNPOLY*i, pac->damping + 1, mjNPOLY); m->actuator_armature[i] = (mjtNum)pac->armature; mjuu_copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN); mjuu_copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN); mjuu_copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS); - mjuu_copyvec(m->actuator_ctrlrange + 2*i, pac->ctrlrange, 2); - mjuu_copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2); mjuu_copyvec(m->actuator_actrange + 2*i, pac->actrange, 2); - mjuu_copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2); mjuu_copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator); + + // per-input arrays, at the actuator's ctrl block + for (int j = m->actuator_ctrladr[i]; + j < m->actuator_ctrladr[i] + m->actuator_ctrlnum[i]; j++) { + m->actuator_ctrllimited[j] = (mjtBool)pac->is_ctrllimited(); + mjuu_copyvec(m->actuator_ctrlrange + 2 * j, pac->ctrlrange, 2); + } + + // per-output arrays, at the actuator's output block + for (int j=m->actuator_outadr[i]; j < m->actuator_outadr[i]+m->actuator_outnum[i]; j++) { + m->actuator_forcelimited[j] = (mjtBool)pac->is_forcelimited(); + mjuu_copyvec(m->actuator_forcerange + 2*j, pac->forcerange, 2); + mjuu_copyvec(m->actuator_gear + 6*j, pac->gear, 6); + mjuu_copyvec(m->actuator_lengthrange + 2*j, pac->lengthrange, 2); + } } // sensors @@ -4084,6 +4120,7 @@ void mjCModel::CopyObjects(mjModel* m) { mjuu_copyvec(body_pos0.data(), m->body_pos, 3*nbody); mjuu_copyvec(body_quat0.data(), m->body_quat, 4*nbody); } +// NOLINTEND(readability/fn_size) @@ -4309,7 +4346,7 @@ void mjCModel::StoreKeyframes(mjCModel* dest) { } if (!compiled) { - nq = nv = na = nu = nmocap = 0; + nq = nv = na = nu = nactuator = nout = nmocap = 0; } } @@ -4958,7 +4995,7 @@ void mjCModel::ExpandAllKeyframes() { for (auto* key : keys_) { ExpandKeyframe(key, qpos0.data(), body_pos0.data(), body_quat0.data()); } - nq = nv = na = nu = nmocap = 0; + nq = nv = na = nu = nactuator = nout = nmocap = 0; } @@ -5239,7 +5276,8 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // create low-level model mj_makeModel(&m, - nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, noct, njnt, ntree, nM, nB, nC, + nq, nv, nu, nactuator, nout, na, + nbody, nbvh, nbvhstatic, nbvhdynamic, noct, njnt, ntree, nM, nB, nC, nD, ngeom, nsite, ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem, nflexelemdata, nflexstiffness, nflexbending, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord, nJfe, nJfv, nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, @@ -5603,7 +5641,8 @@ bool mjCModel::CopyBack(const mjModel* m) { } // make sure sizes match - if (nq != m->nq || nv != m->nv || nu != m->nu || na != m->na || + if (nq != m->nq || nv != m->nv || nu != m->nu || nactuator != m->nactuator || + nout != m->nout || na != m->na || nbody != m->nbody ||njnt != m->njnt || ngeom != m->ngeom || nsite != m->nsite || ncam != m->ncam || nlight != m->nlight || nmesh != m->nmesh || nskin != m->nskin || nhfield != m->nhfield || @@ -5838,17 +5877,17 @@ bool mjCModel::CopyBack(const mjModel* m) { // actuators mjCActuator* pa; - for (int i=0; i < nu; i++) { + for (int i=0; i < nactuator; i++) { pa = actuators_[i]; mjuu_copyvec(pa->dynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN); mjuu_copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN); mjuu_copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS); - mjuu_copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*i, 2); - mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2); + mjuu_copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*m->actuator_ctrladr[i], 2); + mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*m->actuator_outadr[i], 2); mjuu_copyvec(pa->actrange, m->actuator_actrange+2*i, 2); - mjuu_copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2); - mjuu_copyvec(pa->gear, m->actuator_gear+6*i, 6); + mjuu_copyvec(pa->lengthrange, m->actuator_lengthrange+2*m->actuator_outadr[i], 2); + mjuu_copyvec(pa->gear, m->actuator_gear+6*m->actuator_outadr[i], 6); pa->damping[0] = (double)m->actuator_damping[i]; mjuu_copyvec(pa->damping + 1, m->actuator_dampingpoly + mjNPOLY*i, mjNPOLY); pa->armature = (double)m->actuator_armature[i]; diff --git a/src/user/user_model.h b/src/user/user_model.h index e72773c2..d00269dc 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -84,7 +84,9 @@ class mjCModel_ : public mjsElement { // sizes computed by Compile mjtSize nq; // number of generalized coordinates = dim(qpos) mjtSize nv; // number of degrees of freedom = dim(qvel) - mjtSize nu; // number of actuators/controls + mjtSize nu; // number of scalar controls = dim(ctrl) + mjtSize nactuator; // number of actuators + mjtSize nout; // number of force outputs = dim(actuator_force) mjtSize na; // number of activation variables mjtSize ntree; // number of trees mjtSize nbvh; // number of total boundary volume hierarchies diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index a61f35be..68d74528 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -6908,6 +6908,12 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { // no previous state when an actuator is created actadr_ = -1; actdim_ = -1; + + // input and output blocks, set by mjCModel; all actuator types are currently 1x1 + ctrladr_ = -1; + ctrlnum_ = 1; + outadr_ = -1; + outnum_ = 1; } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index ed40733c..42f51f17 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1810,6 +1810,10 @@ class mjCActuator_ : public mjCBase { // variable used for temporarily storing the state of the actuator int actadr_; // address of dof in data->act int actdim_; // number of dofs in data->act + int ctrladr_; // address of first control in data->ctrl + int ctrlnum_; // number of controls + int outadr_; // address of first force output + int outnum_; // number of force outputs, from trntype std::map> act_; // act at the previous step std::map ctrl_; // ctrl at the previous step diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 191a22f4..52d5cd0a 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1021,6 +1021,8 @@ public unsafe struct mjModel_ { public UInt64 nq; public UInt64 nv; public UInt64 nu; + public UInt64 nactuator; + public UInt64 nout; public UInt64 na; public UInt64 nbody; public UInt64 nbvh; @@ -1480,33 +1482,37 @@ public unsafe struct mjModel_ { public int* actuator_dyntype; public int* actuator_gaintype; public int* actuator_biastype; - public int* actuator_trnid; - public double* actuator_damping; - public double* actuator_dampingpoly; - public double* actuator_armature; + public int* actuator_ctrladr; + public int* actuator_ctrlnum; + public int* actuator_outadr; + public int* actuator_outnum; public int* actuator_actadr; public int* actuator_actnum; - public int* actuator_group; - public int* actuator_history; - public int* actuator_historyadr; - public double* actuator_delay; - public byte* actuator_ctrllimited; - public byte* actuator_forcelimited; - public byte* actuator_actlimited; + public int* actuator_trnid; + public double* actuator_cranklength; public double* actuator_dynprm; public double* actuator_gainprm; public double* actuator_biasprm; - public byte* actuator_actearly; - public double* actuator_ctrlrange; - public double* actuator_forcerange; + public byte* actuator_actlimited; public double* actuator_actrange; + public byte* actuator_actearly; + public int* actuator_history; + public int* actuator_historyadr; + public double* actuator_delay; + public double* actuator_damping; + public double* actuator_dampingpoly; + public double* actuator_armature; + public int* actuator_group; + public double* actuator_user; + public int* actuator_plugin; + public byte* actuator_ctrllimited; + public double* actuator_ctrlrange; public double* actuator_gear; - public double* actuator_cranklength; + public byte* actuator_forcelimited; + public double* actuator_forcerange; public double* actuator_acc0; public double* actuator_length0; public double* actuator_lengthrange; - public double* actuator_user; - public int* actuator_plugin; public int* sensor_type; public int* sensor_datatype; public int* sensor_needstage; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 62317c1e..f6a323b6 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -4785,7 +4785,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("actuator_biasprm", &MjModel::actuator_biasprm) .property("actuator_biastype", &MjModel::actuator_biastype) .property("actuator_cranklength", &MjModel::actuator_cranklength) + .property("actuator_ctrladr", &MjModel::actuator_ctrladr) .property("actuator_ctrllimited", &MjModel::actuator_ctrllimited) + .property("actuator_ctrlnum", &MjModel::actuator_ctrlnum) .property("actuator_ctrlrange", &MjModel::actuator_ctrlrange) .property("actuator_damping", &MjModel::actuator_damping) .property("actuator_dampingpoly", &MjModel::actuator_dampingpoly) @@ -4802,6 +4804,8 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("actuator_historyadr", &MjModel::actuator_historyadr) .property("actuator_length0", &MjModel::actuator_length0) .property("actuator_lengthrange", &MjModel::actuator_lengthrange) + .property("actuator_outadr", &MjModel::actuator_outadr) + .property("actuator_outnum", &MjModel::actuator_outnum) .property("actuator_plugin", &MjModel::actuator_plugin) .property("actuator_trnid", &MjModel::actuator_trnid) .property("actuator_trntype", &MjModel::actuator_trntype) @@ -5091,6 +5095,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("nJten", &MjModel::nJten, &MjModel::set_nJten, reference()) .property("nM", &MjModel::nM, &MjModel::set_nM, reference()) .property("na", &MjModel::na, &MjModel::set_na, reference()) + .property("nactuator", &MjModel::nactuator, &MjModel::set_nactuator, reference()) .property("name_actuatoradr", &MjModel::name_actuatoradr) .property("name_bodyadr", &MjModel::name_bodyadr) .property("name_camadr", &MjModel::name_camadr) @@ -5165,6 +5170,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("nnumeric", &MjModel::nnumeric, &MjModel::set_nnumeric, reference()) .property("nnumericdata", &MjModel::nnumericdata, &MjModel::set_nnumericdata, reference()) .property("noct", &MjModel::noct, &MjModel::set_noct, reference()) + .property("nout", &MjModel::nout, &MjModel::set_nout, reference()) .property("npair", &MjModel::npair, &MjModel::set_npair, reference()) .property("npaths", &MjModel::npaths, &MjModel::set_npaths, reference()) .property("nplugin", &MjModel::nplugin, &MjModel::set_nplugin, reference()) diff --git a/wasm/codegen/generated/bindings.h b/wasm/codegen/generated/bindings.h index ff6e32e0..72a8b635 100644 --- a/wasm/codegen/generated/bindings.h +++ b/wasm/codegen/generated/bindings.h @@ -3723,6 +3723,18 @@ struct MjModel { void set_nu(int value) { ptr_->nu = static_cast(value); } + int nactuator() const { + return static_cast(ptr_->nactuator); + } + void set_nactuator(int value) { + ptr_->nactuator = static_cast(value); + } + int nout() const { + return static_cast(ptr_->nout); + } + void set_nout(int value) { + ptr_->nout = static_cast(value); + } int na() const { return static_cast(ptr_->na); } @@ -5353,97 +5365,109 @@ struct MjModel { return emscripten::val(emscripten::typed_memory_view(ptr_->nwrap, ptr_->wrap_prm)); } emscripten::val actuator_trntype() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_trntype)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_trntype)); } emscripten::val actuator_dyntype() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_dyntype)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_dyntype)); } emscripten::val actuator_gaintype() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_gaintype)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_gaintype)); } emscripten::val actuator_biastype() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_biastype)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_biastype)); } - emscripten::val actuator_trnid() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * 2, ptr_->actuator_trnid)); + emscripten::val actuator_ctrladr() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_ctrladr)); } - emscripten::val actuator_damping() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_damping)); + emscripten::val actuator_ctrlnum() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_ctrlnum)); } - emscripten::val actuator_dampingpoly() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * mjNPOLY, ptr_->actuator_dampingpoly)); + emscripten::val actuator_outadr() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_outadr)); } - emscripten::val actuator_armature() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_armature)); + emscripten::val actuator_outnum() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_outnum)); } emscripten::val actuator_actadr() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_actadr)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_actadr)); } emscripten::val actuator_actnum() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_actnum)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_actnum)); } - emscripten::val actuator_group() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_group)); + emscripten::val actuator_trnid() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator * 2, ptr_->actuator_trnid)); + } + emscripten::val actuator_cranklength() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_cranklength)); + } + emscripten::val actuator_dynprm() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator * mjNDYN, ptr_->actuator_dynprm)); + } + emscripten::val actuator_gainprm() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator * mjNGAIN, ptr_->actuator_gainprm)); + } + emscripten::val actuator_biasprm() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator * mjNBIAS, ptr_->actuator_biasprm)); + } + emscripten::val actuator_actlimited() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_actlimited)); + } + emscripten::val actuator_actrange() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator * 2, ptr_->actuator_actrange)); + } + emscripten::val actuator_actearly() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_actearly)); } emscripten::val actuator_history() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * 2, ptr_->actuator_history)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator * 2, ptr_->actuator_history)); } emscripten::val actuator_historyadr() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_historyadr)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_historyadr)); } emscripten::val actuator_delay() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_delay)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_delay)); + } + emscripten::val actuator_damping() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_damping)); + } + emscripten::val actuator_dampingpoly() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator * mjNPOLY, ptr_->actuator_dampingpoly)); + } + emscripten::val actuator_armature() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_armature)); + } + emscripten::val actuator_group() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_group)); + } + emscripten::val actuator_user() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator * ptr_->nuser_actuator, ptr_->actuator_user)); + } + emscripten::val actuator_plugin() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_plugin)); } emscripten::val actuator_ctrllimited() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_ctrllimited)); } - emscripten::val actuator_forcelimited() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_forcelimited)); - } - emscripten::val actuator_actlimited() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_actlimited)); - } - emscripten::val actuator_dynprm() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * mjNDYN, ptr_->actuator_dynprm)); - } - emscripten::val actuator_gainprm() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * mjNGAIN, ptr_->actuator_gainprm)); - } - emscripten::val actuator_biasprm() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * mjNBIAS, ptr_->actuator_biasprm)); - } - emscripten::val actuator_actearly() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_actearly)); - } emscripten::val actuator_ctrlrange() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nu * 2, ptr_->actuator_ctrlrange)); } - emscripten::val actuator_forcerange() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * 2, ptr_->actuator_forcerange)); - } - emscripten::val actuator_actrange() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * 2, ptr_->actuator_actrange)); - } emscripten::val actuator_gear() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * 6, ptr_->actuator_gear)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nout * 6, ptr_->actuator_gear)); } - emscripten::val actuator_cranklength() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_cranklength)); + emscripten::val actuator_forcelimited() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nout, ptr_->actuator_forcelimited)); + } + emscripten::val actuator_forcerange() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nout * 2, ptr_->actuator_forcerange)); } emscripten::val actuator_acc0() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_acc0)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nout, ptr_->actuator_acc0)); } emscripten::val actuator_length0() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_length0)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nout, ptr_->actuator_length0)); } emscripten::val actuator_lengthrange() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * 2, ptr_->actuator_lengthrange)); - } - emscripten::val actuator_user() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu * ptr_->nuser_actuator, ptr_->actuator_user)); - } - emscripten::val actuator_plugin() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_plugin)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nout * 2, ptr_->actuator_lengthrange)); } emscripten::val sensor_type() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor, ptr_->sensor_type)); @@ -5617,7 +5641,7 @@ struct MjModel { return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->name_tendonadr)); } emscripten::val name_actuatoradr() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->name_actuatoradr)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->name_actuatoradr)); } emscripten::val name_sensoradr() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor, ptr_->name_sensoradr)); @@ -6870,13 +6894,13 @@ struct MjData { return emscripten::val(emscripten::typed_memory_view(model->nwrap * 6, ptr_->wrap_xpos)); } emscripten::val actuator_length() const { - return emscripten::val(emscripten::typed_memory_view(model->nu, ptr_->actuator_length)); + return emscripten::val(emscripten::typed_memory_view(model->nout, ptr_->actuator_length)); } emscripten::val moment_rownnz() const { - return emscripten::val(emscripten::typed_memory_view(model->nu, ptr_->moment_rownnz)); + return emscripten::val(emscripten::typed_memory_view(model->nout, ptr_->moment_rownnz)); } emscripten::val moment_rowadr() const { - return emscripten::val(emscripten::typed_memory_view(model->nu, ptr_->moment_rowadr)); + return emscripten::val(emscripten::typed_memory_view(model->nout, ptr_->moment_rowadr)); } emscripten::val moment_colind() const { return emscripten::val(emscripten::typed_memory_view(model->nJmom, ptr_->moment_colind)); @@ -6921,7 +6945,7 @@ struct MjData { return emscripten::val(emscripten::typed_memory_view(model->ntendon, ptr_->ten_velocity)); } emscripten::val actuator_velocity() const { - return emscripten::val(emscripten::typed_memory_view(model->nu, ptr_->actuator_velocity)); + return emscripten::val(emscripten::typed_memory_view(model->nout, ptr_->actuator_velocity)); } emscripten::val cvel() const { return emscripten::val(emscripten::typed_memory_view(model->nbody * 6, ptr_->cvel)); @@ -6966,7 +6990,7 @@ struct MjData { return emscripten::val(emscripten::typed_memory_view(model->nD, ptr_->qLU)); } emscripten::val actuator_force() const { - return emscripten::val(emscripten::typed_memory_view(model->nu, ptr_->actuator_force)); + return emscripten::val(emscripten::typed_memory_view(model->nout, ptr_->actuator_force)); } emscripten::val qfrc_actuator() const { return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->qfrc_actuator));