From f0f535ed04db91f0f3e285f718aff1c09325fb1b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 7 Aug 2023 03:06:26 -0700 Subject: [PATCH] Improve order and clarify declarations of arena variables in `mjdata.h` PiperOrigin-RevId: 554419319 Change-Id: I83b83c56be4b74060f4a108c35ce481b8f354c5e --- doc/includes/references.h | 36 ++++++------ include/mujoco/mjdata.h | 36 ++++++------ introspect/structs.py | 84 ++++++++++++++-------------- unity/Runtime/Bindings/MjBindings.cs | 10 ++-- 4 files changed, 87 insertions(+), 79 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index b05bbba9..735ab693 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -167,13 +167,13 @@ struct mjData_ { mjtNum time; // simulation time mjtNum energy[2]; // potential, kinetic energy - //-------------------------------- end of info header + //-------------------- end of info header // buffers void* buffer; // main buffer; all pointers point in it (nbuffer bytes) void* arena; // arena+stack buffer (nstack*sizeof(mjtNum) bytes) - //-------------------------------- main inputs and outputs of the computation + //-------------------- main inputs and outputs of the computation // state mjtNum* qpos; // position (nq x 1) @@ -205,7 +205,7 @@ struct mjData_ { int* plugin; // copy of m->plugin, required for deletion (nplugin x 1) uintptr_t* plugin_data; // pointer to plugin-managed data structure (nplugin x 1) - //-------------------------------- POSITION dependent + //-------------------- POSITION dependent // computed by mj_fwdPosition/mj_kinematics mjtNum* xpos; // Cartesian position of body frame (nbody x 3) @@ -256,7 +256,7 @@ struct mjData_ { // computed by mj_collisionTree mjtByte* bvh_active; // volume has been added to collisions (nbvh x 1) - //-------------------------------- POSITION, VELOCITY dependent + //-------------------- POSITION, VELOCITY dependent // computed by mj_fwdVelocity mjtNum* ten_velocity; // tendon velocities (ntendon x 1) @@ -272,10 +272,6 @@ struct mjData_ { // computed by mj_fwdVelocity/mj_passive mjtNum* qfrc_passive; // passive force (nv x 1) - // computed by mj_fwdVelocity/mj_referenceConstraint - mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1) - mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1) - // computed by mj_sensorVel/mj_subtreeVel if needed mjtNum* subtree_linvel; // linear velocity of subtree com (nbody x 3) mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3) @@ -298,7 +294,7 @@ struct mjData_ { // computed by mj_implicit/mju_factorLUSparse mjtNum* qLU; // sparse LU of (qM - dt*qDeriv) (nD x 1) - //-------------------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent + //-------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent // computed by mj_fwdActuation mjtNum* actuator_force; // actuator force in actuation space (nu x 1) @@ -320,7 +316,7 @@ struct mjData_ { mjtNum* cfrc_int; // com-based interaction force with parent (nbody x 6) mjtNum* cfrc_ext; // com-based external force on body (nbody x 6) - //-------------------------------- ARENA-ALLOCATED ARRAYS + //-------------------- arena-allocated: POSITION dependent // computed by mj_collision mjContact* contact; // list of all detected contacts (ncon x 1) @@ -346,16 +342,24 @@ struct mjData_ { mjtNum* efc_D; // constraint mass (nefc x 1) mjtNum* efc_R; // inverse constraint mass (nefc x 1) + // computed by mj_projectConstraint (dual solver) + int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1) + int* efc_AR_rowadr; // row start address in colind array (nefc x 1) + int* efc_AR_colind; // column indices in sparse AR (nefc x nefc) + mjtNum* efc_AR; // J*inv(M)*J' + R (nefc x nefc) + + //-------------------- arena-allocated: POSITION, VELOCITY dependent + + // computed by mj_fwdVelocity/mj_referenceConstraint + mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1) + mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1) + + //-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent + // computed by mj_fwdConstraint/mj_inverse mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) mjtNum* efc_force; // constraint force in constraint space (nefc x 1) int* efc_state; // constraint state (mjtConstraintState) (nefc x 1) - - // computed by mj_projectConstraint - int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1) - int* efc_AR_rowadr; // row start address in colind array (nefc x 1) - int* efc_AR_colind; // column indices in sparse AR (nefc x nefc) - mjtNum* efc_AR; // J*inv(M)*J' + R (nefc x nefc) }; typedef struct mjData_ mjData; typedef enum mjtDisableBit_ { // disable default feature bitflags diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index defd4b19..012ec7e7 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -194,13 +194,13 @@ struct mjData_ { mjtNum time; // simulation time mjtNum energy[2]; // potential, kinetic energy - //-------------------------------- end of info header + //-------------------- end of info header // buffers void* buffer; // main buffer; all pointers point in it (nbuffer bytes) void* arena; // arena+stack buffer (nstack*sizeof(mjtNum) bytes) - //-------------------------------- main inputs and outputs of the computation + //-------------------- main inputs and outputs of the computation // state mjtNum* qpos; // position (nq x 1) @@ -232,7 +232,7 @@ struct mjData_ { int* plugin; // copy of m->plugin, required for deletion (nplugin x 1) uintptr_t* plugin_data; // pointer to plugin-managed data structure (nplugin x 1) - //-------------------------------- POSITION dependent + //-------------------- POSITION dependent // computed by mj_fwdPosition/mj_kinematics mjtNum* xpos; // Cartesian position of body frame (nbody x 3) @@ -283,7 +283,7 @@ struct mjData_ { // computed by mj_collisionTree mjtByte* bvh_active; // volume has been added to collisions (nbvh x 1) - //-------------------------------- POSITION, VELOCITY dependent + //-------------------- POSITION, VELOCITY dependent // computed by mj_fwdVelocity mjtNum* ten_velocity; // tendon velocities (ntendon x 1) @@ -299,10 +299,6 @@ struct mjData_ { // computed by mj_fwdVelocity/mj_passive mjtNum* qfrc_passive; // passive force (nv x 1) - // computed by mj_fwdVelocity/mj_referenceConstraint - mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1) - mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1) - // computed by mj_sensorVel/mj_subtreeVel if needed mjtNum* subtree_linvel; // linear velocity of subtree com (nbody x 3) mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3) @@ -325,7 +321,7 @@ struct mjData_ { // computed by mj_implicit/mju_factorLUSparse mjtNum* qLU; // sparse LU of (qM - dt*qDeriv) (nD x 1) - //-------------------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent + //-------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent // computed by mj_fwdActuation mjtNum* actuator_force; // actuator force in actuation space (nu x 1) @@ -347,7 +343,7 @@ struct mjData_ { mjtNum* cfrc_int; // com-based interaction force with parent (nbody x 6) mjtNum* cfrc_ext; // com-based external force on body (nbody x 6) - //-------------------------------- ARENA-ALLOCATED ARRAYS + //-------------------- arena-allocated: POSITION dependent // computed by mj_collision mjContact* contact; // list of all detected contacts (ncon x 1) @@ -373,16 +369,24 @@ struct mjData_ { mjtNum* efc_D; // constraint mass (nefc x 1) mjtNum* efc_R; // inverse constraint mass (nefc x 1) + // computed by mj_projectConstraint (dual solver) + int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1) + int* efc_AR_rowadr; // row start address in colind array (nefc x 1) + int* efc_AR_colind; // column indices in sparse AR (nefc x nefc) + mjtNum* efc_AR; // J*inv(M)*J' + R (nefc x nefc) + + //-------------------- arena-allocated: POSITION, VELOCITY dependent + + // computed by mj_fwdVelocity/mj_referenceConstraint + mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1) + mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1) + + //-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent + // computed by mj_fwdConstraint/mj_inverse mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) mjtNum* efc_force; // constraint force in constraint space (nefc x 1) int* efc_state; // constraint state (mjtConstraintState) (nefc x 1) - - // computed by mj_projectConstraint - int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1) - int* efc_AR_rowadr; // row start address in colind array (nefc x 1) - int* efc_AR_colind; // column indices in sparse AR (nefc x nefc) - mjtNum* efc_AR; // J*inv(M)*J' + R (nefc x nefc) }; typedef struct mjData_ mjData; diff --git a/introspect/structs.py b/introspect/structs.py index 300b0f79..170e98e4 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -3997,20 +3997,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='passive force (nv x 1)', # pylint: disable=line-too-long ), - StructFieldDecl( - name='efc_vel', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='velocity in constraint space: J*qvel (nefc x 1)', # pylint: disable=line-too-long - ), - StructFieldDecl( - name='efc_aref', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='reference pseudo-acceleration (nefc x 1)', # pylint: disable=line-too-long - ), StructFieldDecl( name='subtree_linvel', type=PointerType( @@ -4298,6 +4284,48 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='inverse constraint mass (nefc x 1)', # pylint: disable=line-too-long ), + StructFieldDecl( + name='efc_AR_rownnz', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='number of non-zeros in AR (nefc x 1)', # pylint: disable=line-too-long + ), + StructFieldDecl( + name='efc_AR_rowadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='row start address in colind array (nefc x 1)', # pylint: disable=line-too-long + ), + StructFieldDecl( + name='efc_AR_colind', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='column indices in sparse AR (nefc x nefc)', # pylint: disable=line-too-long + ), + StructFieldDecl( + name='efc_AR', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc="J*inv(M)*J' + R (nefc x nefc)", # pylint: disable=line-too-long + ), + StructFieldDecl( + name='efc_vel', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='velocity in constraint space: J*qvel (nefc x 1)', # pylint: disable=line-too-long + ), + StructFieldDecl( + name='efc_aref', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='reference pseudo-acceleration (nefc x 1)', # pylint: disable=line-too-long + ), StructFieldDecl( name='efc_b', type=PointerType( @@ -4319,34 +4347,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='constraint state (mjtConstraintState) (nefc x 1)', # pylint: disable=line-too-long ), - StructFieldDecl( - name='efc_AR_rownnz', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='number of non-zeros in AR (nefc x 1)', # pylint: disable=line-too-long - ), - StructFieldDecl( - name='efc_AR_rowadr', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='row start address in colind array (nefc x 1)', # pylint: disable=line-too-long - ), - StructFieldDecl( - name='efc_AR_colind', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='column indices in sparse AR (nefc x nefc)', # pylint: disable=line-too-long - ), - StructFieldDecl( - name='efc_AR', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc="J*inv(M)*J' + R (nefc x nefc)", # pylint: disable=line-too-long - ), ), )), ('mjvPerturb', diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index bb8e041b..60fb6a49 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1664,8 +1664,6 @@ public unsafe struct mjData_ { public double* cdof_dot; public double* qfrc_bias; public double* qfrc_passive; - public double* efc_vel; - public double* efc_aref; public double* subtree_linvel; public double* subtree_angmom; public double* qH; @@ -1707,13 +1705,15 @@ public unsafe struct mjData_ { public double* efc_KBIP; public double* efc_D; public double* efc_R; - public double* efc_b; - public double* efc_force; - public int* efc_state; public int* efc_AR_rownnz; public int* efc_AR_rowadr; public int* efc_AR_colind; public double* efc_AR; + public double* efc_vel; + public double* efc_aref; + public double* efc_b; + public double* efc_force; + public int* efc_state; } [StructLayout(LayoutKind.Sequential)]