Dynamically allocate contact and efc_ arrays on a new memory arena.
- Add private function `mj_arenaAlloc`. This is used internally to allocate memory from the arena. - Add private function `mj_nefc` to count constraints. This function returns a tight upper bound on `d->nefc`. The number of counted constraints can be slightly bigger than exact `d->nefc` in the case of constraints with empty Jacobian, as when placing a frictional tendon between two world sites. - Add new `memory` attribute to the `size` XML element for specification of arena memory size. This attribute is mutually exclusive with `nstack` and `njmax` specifications, which are now deprecated (but left around for the time being for legacy compatibility). - Move `d->stack` to the end of the new arena space. The stack now grows in reverse from the end. PiperOrigin-RevId: 479341539 Change-Id: Ie019c202e0908577ffc6f833a37920858116f667
This commit is contained in:
Saran Tunyasuvunakool
committed by
Copybara-Service
Copybara-Service
parent
4d85a464cc
commit
58fd72f53d
+48
-39
@@ -15,6 +15,7 @@
|
||||
#ifndef MUJOCO_MJDATA_H_
|
||||
#define MUJOCO_MJDATA_H_
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <mujoco/mjtnum.h>
|
||||
@@ -124,15 +125,19 @@ typedef struct mjSolverStat_ mjSolverStat;
|
||||
|
||||
struct mjData_ {
|
||||
// constant sizes
|
||||
int nstack; // number of mjtNums that can fit in stack
|
||||
int nstack; // number of mjtNums that can fit in the arena+stack space
|
||||
int nbuffer; // size of main buffer in bytes
|
||||
int nplugin; // number of plugin instances
|
||||
|
||||
// stack pointer
|
||||
int pstack; // first available mjtNum address in stack
|
||||
size_t pstack; // first available mjtNum address in stack
|
||||
|
||||
// arena pointer
|
||||
size_t parena; // first available byte in arena
|
||||
|
||||
// memory utilization stats
|
||||
int maxuse_stack; // maximum stack allocation
|
||||
size_t maxuse_arena; // maximum arena allocation
|
||||
int maxuse_con; // maximum number of contacts
|
||||
int maxuse_efc; // maximum number of scalar constraints
|
||||
|
||||
@@ -157,8 +162,8 @@ struct mjData_ {
|
||||
//-------------------------------- end of info header
|
||||
|
||||
// buffers
|
||||
void* buffer; // main buffer; all pointers point in it (nbuffer bytes)
|
||||
mjtNum* stack; // stack buffer (nstack mjtNums)
|
||||
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
|
||||
|
||||
@@ -240,36 +245,6 @@ struct mjData_ {
|
||||
mjtNum* qLDiagInv; // 1/diag(D) (nv x 1)
|
||||
mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1)
|
||||
|
||||
// computed by mj_fwdPosition/mj_collision
|
||||
mjContact* contact; // list of all detected contacts (nconmax x 1)
|
||||
|
||||
// computed by mj_fwdPosition/mj_makeConstraint
|
||||
int* efc_type; // constraint type (mjtConstraint) (njmax x 1)
|
||||
int* efc_id; // id of object of specified type (njmax x 1)
|
||||
int* efc_J_rownnz; // number of non-zeros in Jacobian row (njmax x 1)
|
||||
int* efc_J_rowadr; // row start address in colind array (njmax x 1)
|
||||
int* efc_J_rowsuper; // number of subsequent rows in supernode (njmax x 1)
|
||||
int* efc_J_colind; // column indices in Jacobian (njmax x nv)
|
||||
int* efc_JT_rownnz; // number of non-zeros in Jacobian row T (nv x 1)
|
||||
int* efc_JT_rowadr; // row start address in colind array T (nv x 1)
|
||||
int* efc_JT_rowsuper; // number of subsequent rows in supernode T (nv x 1)
|
||||
int* efc_JT_colind; // column indices in Jacobian T (nv x njmax)
|
||||
mjtNum* efc_J; // constraint Jacobian (njmax x nv)
|
||||
mjtNum* efc_JT; // constraint Jacobian transposed (nv x njmax)
|
||||
mjtNum* efc_pos; // constraint position (equality, contact) (njmax x 1)
|
||||
mjtNum* efc_margin; // inclusion margin (contact) (njmax x 1)
|
||||
mjtNum* efc_frictionloss; // frictionloss (friction) (njmax x 1)
|
||||
mjtNum* efc_diagApprox; // approximation to diagonal of A (njmax x 1)
|
||||
mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (njmax x 4)
|
||||
mjtNum* efc_D; // constraint mass (njmax x 1)
|
||||
mjtNum* efc_R; // inverse constraint mass (njmax x 1)
|
||||
|
||||
// computed by mj_fwdPosition/mj_projectConstraint
|
||||
int* efc_AR_rownnz; // number of non-zeros in AR (njmax x 1)
|
||||
int* efc_AR_rowadr; // row start address in colind array (njmax x 1)
|
||||
int* efc_AR_colind; // column indices in sparse AR (njmax x njmax)
|
||||
mjtNum* efc_AR; // J*inv(M)*J' + R (njmax x njmax)
|
||||
|
||||
//-------------------------------- POSITION, VELOCITY dependent
|
||||
|
||||
// computed by mj_fwdVelocity
|
||||
@@ -287,8 +262,8 @@ struct mjData_ {
|
||||
mjtNum* qfrc_passive; // passive force (nv x 1)
|
||||
|
||||
// computed by mj_fwdVelocity/mj_referenceConstraint
|
||||
mjtNum* efc_vel; // velocity in constraint space: J*qvel (njmax x 1)
|
||||
mjtNum* efc_aref; // reference pseudo-acceleration (njmax x 1)
|
||||
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)
|
||||
@@ -320,9 +295,6 @@ struct mjData_ {
|
||||
mjtNum* qacc_smooth; // unconstrained acceleration (nv x 1)
|
||||
|
||||
// computed by mj_fwdConstraint/mj_inverse
|
||||
mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (njmax x 1)
|
||||
mjtNum* efc_force; // constraint force in constraint space (njmax x 1)
|
||||
int* efc_state; // constraint state (mjtConstraintState) (njmax x 1)
|
||||
mjtNum* qfrc_constraint; // constraint force (nv x 1)
|
||||
|
||||
// computed by mj_inverse
|
||||
@@ -333,6 +305,43 @@ struct mjData_ {
|
||||
mjtNum* cacc; // com-based acceleration (nbody x 6)
|
||||
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
|
||||
|
||||
// computed by mj_collision
|
||||
mjContact* contact; // list of all detected contacts (ncon x 1)
|
||||
|
||||
// computed by mj_makeConstraint
|
||||
int* efc_type; // constraint type (mjtConstraint) (nefc x 1)
|
||||
int* efc_id; // id of object of specified type (nefc x 1)
|
||||
int* efc_J_rownnz; // number of non-zeros in Jacobian row (nefc x 1)
|
||||
int* efc_J_rowadr; // row start address in colind array (nefc x 1)
|
||||
int* efc_J_rowsuper; // number of subsequent rows in supernode (nefc x 1)
|
||||
int* efc_J_colind; // column indices in Jacobian (nefc x nv)
|
||||
int* efc_JT_rownnz; // number of non-zeros in Jacobian row T (nv x 1)
|
||||
int* efc_JT_rowadr; // row start address in colind array T (nv x 1)
|
||||
int* efc_JT_rowsuper; // number of subsequent rows in supernode T (nv x 1)
|
||||
int* efc_JT_colind; // column indices in Jacobian T (nv x nefc)
|
||||
mjtNum* efc_J; // constraint Jacobian (nefc x nv)
|
||||
mjtNum* efc_JT; // constraint Jacobian transposed (nv x nefc)
|
||||
mjtNum* efc_pos; // constraint position (equality, contact) (nefc x 1)
|
||||
mjtNum* efc_margin; // inclusion margin (contact) (nefc x 1)
|
||||
mjtNum* efc_frictionloss; // frictionloss (friction) (nefc x 1)
|
||||
mjtNum* efc_diagApprox; // approximation to diagonal of A (nefc x 1)
|
||||
mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (nefc x 4)
|
||||
mjtNum* efc_D; // constraint mass (nefc x 1)
|
||||
mjtNum* efc_R; // inverse constraint mass (nefc x 1)
|
||||
|
||||
// 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;
|
||||
|
||||
|
||||
+50
-31
@@ -434,8 +434,7 @@
|
||||
|
||||
// define symbols needed in MJDATA_POINTERS (corresponding to number of columns)
|
||||
#define MJDATA_POINTERS_PREAMBLE( m ) \
|
||||
int nv = m->nv; \
|
||||
int njmax = m->njmax;
|
||||
int nv = m->nv;
|
||||
|
||||
|
||||
// pointer fields of mjData
|
||||
@@ -490,38 +489,12 @@
|
||||
X( mjtNum, qLD, nM, 1 ) \
|
||||
X( mjtNum, qLDiagInv, nv, 1 ) \
|
||||
X( mjtNum, qLDiagSqrtInv, nv, 1 ) \
|
||||
X( mjContact, contact, nconmax, 1 ) \
|
||||
X( int, efc_type, njmax, 1 ) \
|
||||
X( int, efc_id, njmax, 1 ) \
|
||||
X( int, efc_J_rownnz, njmax, 1 ) \
|
||||
X( int, efc_J_rowadr, njmax, 1 ) \
|
||||
X( int, efc_J_rowsuper, njmax, 1 ) \
|
||||
X( int, efc_J_colind, njmax, MJ_M(nv) ) \
|
||||
X( int, efc_JT_rownnz, nv, 1 ) \
|
||||
X( int, efc_JT_rowadr, nv, 1 ) \
|
||||
X( int, efc_JT_rowsuper, nv, 1 ) \
|
||||
X( int, efc_JT_colind, nv, MJ_M(njmax) ) \
|
||||
X( mjtNum, efc_J, njmax, MJ_M(nv) ) \
|
||||
X( mjtNum, efc_JT, nv, MJ_M(njmax) ) \
|
||||
X( mjtNum, efc_pos, njmax, 1 ) \
|
||||
X( mjtNum, efc_margin, njmax, 1 ) \
|
||||
X( mjtNum, efc_frictionloss, njmax, 1 ) \
|
||||
X( mjtNum, efc_diagApprox, njmax, 1 ) \
|
||||
X( mjtNum, efc_KBIP, njmax, 4 ) \
|
||||
X( mjtNum, efc_D, njmax, 1 ) \
|
||||
X( mjtNum, efc_R, njmax, 1 ) \
|
||||
X( int, efc_AR_rownnz, njmax, 1 ) \
|
||||
X( int, efc_AR_rowadr, njmax, 1 ) \
|
||||
X( int, efc_AR_colind, njmax, MJ_M(njmax) ) \
|
||||
X( mjtNum, efc_AR, njmax, MJ_M(njmax) ) \
|
||||
X( mjtNum, ten_velocity, ntendon, 1 ) \
|
||||
X( mjtNum, actuator_velocity, nu, 1 ) \
|
||||
X( mjtNum, cvel, nbody, 6 ) \
|
||||
X( mjtNum, cdof_dot, nv, 6 ) \
|
||||
X( mjtNum, qfrc_bias, nv, 1 ) \
|
||||
X( mjtNum, qfrc_passive, nv, 1 ) \
|
||||
X( mjtNum, efc_vel, njmax, 1 ) \
|
||||
X( mjtNum, efc_aref, njmax, 1 ) \
|
||||
X( mjtNum, subtree_linvel, nbody, 3 ) \
|
||||
X( mjtNum, subtree_angmom, nbody, 3 ) \
|
||||
X( mjtNum, qH, nM, 1 ) \
|
||||
@@ -535,9 +508,6 @@
|
||||
X( mjtNum, qfrc_actuator, nv, 1 ) \
|
||||
X( mjtNum, qfrc_smooth, nv, 1 ) \
|
||||
X( mjtNum, qacc_smooth, nv, 1 ) \
|
||||
X( mjtNum, efc_b, njmax, 1 ) \
|
||||
X( mjtNum, efc_force, njmax, 1 ) \
|
||||
X( int, efc_state, njmax, 1 ) \
|
||||
X( mjtNum, qfrc_constraint, nv, 1 ) \
|
||||
X( mjtNum, qfrc_inverse, nv, 1 ) \
|
||||
X( mjtNum, cacc, nbody, 6 ) \
|
||||
@@ -545,6 +515,55 @@
|
||||
X( mjtNum, cfrc_ext, nbody, 6 )
|
||||
|
||||
|
||||
// macro for annotating that an array size in an X macro is a member of mjData
|
||||
// by default this macro does nothing, but users can redefine it as necessary
|
||||
#define MJ_D(n) n
|
||||
|
||||
// array of contacts
|
||||
#define MJDATA_ARENA_POINTERS_CONTACT \
|
||||
X( mjContact, contact, MJ_D(ncon), 1 )
|
||||
|
||||
// array fields of mjData that are used in the primal problem
|
||||
#define MJDATA_ARENA_POINTERS_PRIMAL \
|
||||
X( int, efc_type, MJ_D(nefc), 1 ) \
|
||||
X( int, efc_id, MJ_D(nefc), 1 ) \
|
||||
X( int, efc_J_rownnz, MJ_D(nefc), 1 ) \
|
||||
X( int, efc_J_rowadr, MJ_D(nefc), 1 ) \
|
||||
X( int, efc_J_rowsuper, MJ_D(nefc), 1 ) \
|
||||
X( int, efc_J_colind, MJ_D(nefc), MJ_M(nv) ) \
|
||||
X( int, efc_JT_rownnz, MJ_M(nv), 1 ) \
|
||||
X( int, efc_JT_rowadr, MJ_M(nv), 1 ) \
|
||||
X( int, efc_JT_rowsuper, MJ_M(nv), 1 ) \
|
||||
X( int, efc_JT_colind, MJ_M(nv), MJ_D(nefc) ) \
|
||||
X( mjtNum, efc_J, MJ_D(nefc), MJ_M(nv) ) \
|
||||
X( mjtNum, efc_JT, MJ_M(nv), MJ_D(nefc) ) \
|
||||
X( mjtNum, efc_pos, MJ_D(nefc), 1 ) \
|
||||
X( mjtNum, efc_margin, MJ_D(nefc), 1 ) \
|
||||
X( mjtNum, efc_frictionloss, MJ_D(nefc), 1 ) \
|
||||
X( mjtNum, efc_diagApprox, MJ_D(nefc), 1 ) \
|
||||
X( mjtNum, efc_KBIP, MJ_D(nefc), 4 ) \
|
||||
X( mjtNum, efc_D, MJ_D(nefc), 1 ) \
|
||||
X( mjtNum, efc_R, MJ_D(nefc), 1 ) \
|
||||
X( mjtNum, efc_vel, MJ_D(nefc), 1 ) \
|
||||
X( mjtNum, efc_aref, MJ_D(nefc), 1 ) \
|
||||
X( mjtNum, efc_b, MJ_D(nefc), 1 ) \
|
||||
X( mjtNum, efc_force, MJ_D(nefc), 1 ) \
|
||||
X( int, efc_state, MJ_D(nefc), 1 ) \
|
||||
|
||||
// array fields of mjData that are used in the dual problem
|
||||
#define MJDATA_ARENA_POINTERS_DUAL \
|
||||
X( int, efc_AR_rownnz, MJ_D(nefc), 1 ) \
|
||||
X( int, efc_AR_rowadr, MJ_D(nefc), 1 ) \
|
||||
X( int, efc_AR_colind, MJ_D(nefc), MJ_D(nefc) ) \
|
||||
X( mjtNum, efc_AR, MJ_D(nefc), MJ_D(nefc) )
|
||||
|
||||
// array fields of mjData that live in d->arena
|
||||
#define MJDATA_ARENA_POINTERS \
|
||||
MJDATA_ARENA_POINTERS_CONTACT \
|
||||
MJDATA_ARENA_POINTERS_PRIMAL \
|
||||
MJDATA_ARENA_POINTERS_DUAL
|
||||
|
||||
|
||||
// scalar fields of mjData
|
||||
#define MJDATA_SCALAR \
|
||||
X( int, nstack ) \
|
||||
|
||||
Reference in New Issue
Block a user