diff --git a/doc/includes/references.h b/doc/includes/references.h index 52323950..324ad35a 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -111,6 +111,10 @@ typedef struct mjData_ { int nl; // number of limit constraints int nefc; // number of constraints int nJ; // number of non-zeros in constraint Jacobian + int efm_active; // implicit effective metric M+K: 0 inactive, 1 active, 2 active + preconditioner exact + int nefmK; // number of non-zeros in effective-stiffness CSR + int nefmdof; // number of rows in effective-metric factor + int nefmL; // number of non-zeros in the effective-metric factor int nY; // number of non-zeros in constraint inverse inertia square root int nA; // number of non-zeros in constraint inverse inertia matrix int nisland; // number of detected constraint islands @@ -200,6 +204,7 @@ typedef struct mjData_ { // computed by mj_fwdPosition/mj_flex mjtNum* flexvert_xpos; // Cartesian flex vertex positions (nflexvert x 3) mjtNum* flexelem_aabb; // flex element bounding boxes (center, size) (nflexelem x 6) + mjtNum* flexelem_krot; // corotated element stiffness (implicit only) (nflexstiffness x 1) mjtNum* flexedge_J; // flex edge Jacobian (nJfe x 1) mjtNum* flexedge_length; // flex edge lengths (nflexedge x 1) mjtNum* flexvert_J; // flex vertex Jacobian (nJfv x 2) @@ -369,6 +374,18 @@ typedef struct mjData_ { mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1) mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1) + // computed by mj_fwdPosition/mj_invPosition when the implicit effective metric M+K is active + mjtNum* efm_c; // smooth-force shift h*K*qvel (nv x 1) + int* efm_K_rownnz; // effective-stiffness CSR row nonzeros (nv x 1) + int* efm_K_rowadr; // effective-stiffness CSR row addresses (nv x 1) + int* efm_K_colind; // effective-stiffness CSR column indices (nefmK x 1) + mjtNum* efm_K_val; // effective-stiffness CSR values (nefmK x 1) + int* efm_dofid; // factor row -> dof address (nefmdof x 1) + int* efm_L_rownnz; // factor row nonzeros (nefmdof x 1) + int* efm_L_rowadr; // factor row addresses (nefmdof x 1) + int* efm_L_colind; // factor column indices (nefmL x 1) + mjtNum* efm_L; // Cholesky factor of diag(M)+K, covered dofs (nefmL x 1) + //-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent // computed by mj_fwdConstraint/mj_inverse @@ -584,6 +601,8 @@ typedef struct mjModel_ { mjtSize nflexelemdata; // number of element vertex ids in all flexes mjtSize nflexstiffness; // number of stiffness parameters in all flexes mjtSize nflexbending; // number of bending parameters in all flexes + mjtSize nefm0dof; // number of dofs covered by the constant metric factor + mjtSize nefm0L; // number of non-zeros in the constant metric factor mjtSize nflexelemedge; // number of element edge ids in all flexes mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes mjtSize nflexevpair; // number of element-vertex pairs in all flexes @@ -902,6 +921,11 @@ typedef struct mjModel_ { mjtNum* flex_size; // vertex bounding box half sizes in qpos0 (nflex x 3) mjtNum* flex_stiffness; // finite element stiffness matrix (nflexstiffness x 1) mjtNum* flex_bending; // bending stiffness (nflexbending x 1) + int* efm0_dofid; // constant metric factor row->dof address (nefm0dof x 1) + int* efm0_L_rownnz; // constant metric factor row nonzeros (nefm0dof x 1) + int* efm0_L_rowadr; // constant metric factor row addresses (nefm0dof x 1) + int* efm0_L_colind; // constant metric factor column indices (nefm0L x 1) + mjtNum* efm0_L; // factor of M + (dt^2+dt*d)*K_bend (nefm0L x 1) mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 15afe8b2..621b1129 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -135,6 +135,10 @@ typedef struct mjData_ { int nl; // number of limit constraints int nefc; // number of constraints int nJ; // number of non-zeros in constraint Jacobian + int efm_active; // implicit effective metric M+K: 0 inactive, 1 active, 2 active + preconditioner exact + int nefmK; // number of non-zeros in effective-stiffness CSR + int nefmdof; // number of rows in effective-metric factor + int nefmL; // number of non-zeros in the effective-metric factor int nY; // number of non-zeros in constraint inverse inertia square root int nA; // number of non-zeros in constraint inverse inertia matrix int nisland; // number of detected constraint islands @@ -224,6 +228,7 @@ typedef struct mjData_ { // computed by mj_fwdPosition/mj_flex mjtNum* flexvert_xpos; // Cartesian flex vertex positions (nflexvert x 3) mjtNum* flexelem_aabb; // flex element bounding boxes (center, size) (nflexelem x 6) + mjtNum* flexelem_krot; // corotated element stiffness (implicit only) (nflexstiffness x 1) mjtNum* flexedge_J; // flex edge Jacobian (nJfe x 1) mjtNum* flexedge_length; // flex edge lengths (nflexedge x 1) mjtNum* flexvert_J; // flex vertex Jacobian (nJfv x 2) @@ -393,6 +398,18 @@ typedef struct mjData_ { mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1) mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1) + // computed by mj_fwdPosition/mj_invPosition when the implicit effective metric M+K is active + mjtNum* efm_c; // smooth-force shift h*K*qvel (nv x 1) + int* efm_K_rownnz; // effective-stiffness CSR row nonzeros (nv x 1) + int* efm_K_rowadr; // effective-stiffness CSR row addresses (nv x 1) + int* efm_K_colind; // effective-stiffness CSR column indices (nefmK x 1) + mjtNum* efm_K_val; // effective-stiffness CSR values (nefmK x 1) + int* efm_dofid; // factor row -> dof address (nefmdof x 1) + int* efm_L_rownnz; // factor row nonzeros (nefmdof x 1) + int* efm_L_rowadr; // factor row addresses (nefmdof x 1) + int* efm_L_colind; // factor column indices (nefmL x 1) + mjtNum* efm_L; // Cholesky factor of diag(M)+K, covered dofs (nefmL x 1) + //-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent // computed by mj_fwdConstraint/mj_inverse diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index a27b6bf0..48eadb4a 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -272,6 +272,8 @@ typedef struct mjModel_ { mjtSize nflexelemdata; // number of element vertex ids in all flexes mjtSize nflexstiffness; // number of stiffness parameters in all flexes mjtSize nflexbending; // number of bending parameters in all flexes + mjtSize nefm0dof; // number of dofs covered by the constant metric factor + mjtSize nefm0L; // number of non-zeros in the constant metric factor mjtSize nflexelemedge; // number of element edge ids in all flexes mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes mjtSize nflexevpair; // number of element-vertex pairs in all flexes @@ -590,6 +592,11 @@ typedef struct mjModel_ { mjtNum* flex_size; // vertex bounding box half sizes in qpos0 (nflex x 3) mjtNum* flex_stiffness; // finite element stiffness matrix (nflexstiffness x 1) mjtNum* flex_bending; // bending stiffness (nflexbending x 1) + int* efm0_dofid; // constant metric factor row->dof address (nefm0dof x 1) + int* efm0_L_rownnz; // constant metric factor row nonzeros (nefm0dof x 1) + int* efm0_L_rowadr; // constant metric factor row addresses (nefm0dof x 1) + int* efm0_L_colind; // constant metric factor column indices (nefm0L x 1) + mjtNum* efm0_L; // factor of M + (dt^2+dt*d)*K_bend (nefm0L x 1) mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 845c9ffd..fc78487a 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -189,6 +189,8 @@ X( nflexelemdata ) \ X( nflexstiffness ) \ X( nflexbending ) \ + X( nefm0dof ) \ + X( nefm0L ) \ X( nflexelemedge ) \ X( nflexshelldata ) \ X( nflexevpair ) \ @@ -501,6 +503,11 @@ X ( mjtNum, flex_size, nflex, 3 ) \ X ( mjtNum, flex_stiffness, nflexstiffness, 1 ) \ X ( mjtNum, flex_bending, nflexbending, 1 ) \ + X ( int, efm0_dofid, nefm0dof, 1 ) \ + X ( int, efm0_L_rownnz, nefm0dof, 1 ) \ + X ( int, efm0_L_rowadr, nefm0dof, 1 ) \ + X ( int, efm0_L_colind, nefm0L, 1 ) \ + X ( mjtNum, efm0_L, nefm0L, 1 ) \ X ( mjtNum, flex_damping, nflex, 1 ) \ X ( mjtNum, flex_edgestiffness, nflex, 1 ) \ X ( mjtNum, flex_edgedamping, nflex, 1 ) \ @@ -866,6 +873,7 @@ X ( mjtNum, cinert, nbody, 10 ) \ X ( mjtNum, flexvert_xpos, nflexvert, 3 ) \ X ( mjtNum, flexelem_aabb, nflexelem, 6 ) \ + X ( mjtNum, flexelem_krot, nflexstiffness, 1 ) \ X ( mjtNum, flexedge_J, nJfe, 1 ) \ X ( mjtNum, flexedge_length, nflexedge, 1 ) \ X ( mjtNum, flexvert_J, nJfv, 2 ) \ @@ -995,11 +1003,25 @@ X ( mjtNum, ifrc_constraint, MJ_D(nidof), 1 ) // array fields of mjData that live in d->arena +#define MJDATA_ARENA_POINTERS_EFM \ + X ( mjtNum, efm_c, MJ_M(nv), 1 ) \ + X ( int, efm_K_rownnz, MJ_M(nv), 1 ) \ + X ( int, efm_K_rowadr, MJ_M(nv), 1 ) \ + X ( int, efm_K_colind, MJ_D(nefmK), 1 ) \ + X ( mjtNum, efm_K_val, MJ_D(nefmK), 1 ) \ + X ( int, efm_dofid, MJ_D(nefmdof), 1 ) \ + X ( int, efm_L_rownnz, MJ_D(nefmdof), 1 ) \ + X ( int, efm_L_rowadr, MJ_D(nefmdof), 1 ) \ + X ( int, efm_L_colind, MJ_D(nefmL), 1 ) \ + X ( mjtNum, efm_L, MJ_D(nefmL), 1 ) + + #define MJDATA_ARENA_POINTERS \ MJDATA_ARENA_POINTERS_CONTACT \ MJDATA_ARENA_POINTERS_SOLVER \ MJDATA_ARENA_POINTERS_DUAL \ - MJDATA_ARENA_POINTERS_ISLAND + MJDATA_ARENA_POINTERS_ISLAND \ + MJDATA_ARENA_POINTERS_EFM // scalar fields of mjData @@ -1021,6 +1043,10 @@ X( int, nl ) \ X( int, nefc ) \ X( int, nJ ) \ + X( int, efm_active ) \ + X( int, nefmK ) \ + X( int, nefmdof ) \ + X( int, nefmL ) \ X( int, nY ) \ X( int, nA ) \ X( int, nisland ) \ diff --git a/model/flex/bunny.xml b/model/flex/bunny.xml index 0d10cad9..3160530f 100644 --- a/model/flex/bunny.xml +++ b/model/flex/bunny.xml @@ -16,7 +16,7 @@ -