chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implement sleeping in engine

Browse Source 
PiperOrigin-RevId: 829361787
Change-Id: I6f64d8e25c4248cf32c18cd94d37ff5def78946e
This commit is contained in:
Yuval Tassa
2025-11-07 03:32:03 -08:00
committed by Copybara-Service
parent 1e0226d360
commit 769f37b653
55 changed files with 3602 additions and 677 deletions
Download Patch File Download Diff File Expand all files Collapse all files
STYLEGUIDE.md
+10 -10
View File
@@ -86,7 +86,7 @@ are required.
[attached K&R braces](https://en.wikipedia.org/wiki/Indentation_style#Variant:_mandatory_braces),
including for one-line blocks:
```C
```c
// transpose matrix
void mju_transpose(mjtNum* res, const mjtNum* mat, int nr, int nc) {
for (int i=0; i < nr; i++) {
@@ -97,15 +97,14 @@ including for one-line blocks:
}
```
- Brace-less single line statements are allowed outside of `engine/` code, for
similar, repeated blocks, that do not contain flow control statements (`return`,
`continue`, etc.). For an example of this exception, inspect the [`mjCModel`
destructor](https://github.com/google-deepmind/mujoco/search?q=repo%3Adeepmind%2Fmujoco+filename%3Auser_model.cc).
- Brace-less single line statements are allowed with the exception of `return`
and `break` statements. For an example of this exception, inspect the
[`mjCModel` destructor](https://github.com/google-deepmind/mujoco/search?q=repo%3Adeepmind%2Fmujoco+filename%3Auser_model.cc).
- Unattached braces are allowed in `if/else` blocks, when inserting a comment
before the `else`:
- Unattached braces are allowed in `if/else` blocks, when inserting an
explanatory comment above the `else`:
```C
```c
// rotate vector by quaternion
void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) {
// null quat: copy vec
@@ -129,7 +128,7 @@ readability. For example below, note the lack of spaces around the
multiplication operator, and the aligning spaces in the second and fourth
assignments:
```C
```c
// time-derivative of quaternion, given 3D rotational velocity
void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]) {
res[0] = 0.5*(-vel[0]*quat[1] - vel[1]*quat[2] - vel[2]*quat[3]);
@@ -145,7 +144,8 @@ assignments:
variable initialisation in `for` loops. For example, inspect the
`mju_transpose` implementation above.
- Two blank lines are required between function implementations in source files.
- Two blank lines are required between function implementations in source
files.
#### Variable declarations
simulate/simulate.cc
+6 -3
View File
@@ -408,10 +408,12 @@ void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) {
}
sqrt_nnz = mju_sqrt(sqrt_nnz);
// get sizes: nv, nbody, nefc, sqrt(nnz), ncont, iter
// get sizes: nv, nbody, nefc, sqrt(nnz), ncon, iter
int nv = mjENABLED(mjENBL_SLEEP) ? d->nv_awake : m->nv;
int nbody = mjENABLED(mjENBL_SLEEP) ? d->nbody_awake : m->nbody;
float sdata[6] = {
static_cast<float>(m->nv),
static_cast<float>(m->nbody),
static_cast<float>(nv),
static_cast<float>(nbody),
static_cast<float>(d->nefc),
static_cast<float>(sqrt_nnz),
static_cast<float>(d->ncon),
@@ -708,6 +710,7 @@ void MakePhysicsSection(mj::Simulate* sim) {
{mjITEM_EDITNUM, "Noslip Tol", 2, &(opt->noslip_tolerance), "1 0 1"},
{mjITEM_EDITINT, "CCD Iter", 2, &(opt->ccd_iterations), "1 0 1000"},
{mjITEM_EDITNUM, "CCD Tol", 2, &(opt->ccd_tolerance), "1 0 1"},
{mjITEM_EDITNUM, "Sleep Tol", 2, &(opt->sleep_tolerance), "1 0 1"},
{mjITEM_EDITINT, "SDF Iter", 2, &(opt->sdf_iterations), "1 1 20"},
{mjITEM_EDITINT, "SDF Init", 2, &(opt->sdf_initpoints), "1 1 100"},
{mjITEM_SEPARATOR, "Physical Parameters", mjPRESERVE},
src/engine/engine_collision_driver.c
+32 -7
View File
@@ -156,14 +156,25 @@ static int mj_filterSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum m
}
// filter body pair: 1- discard, 0- proceed
static int filterBodyPair(int weldbody1, int weldparent1, int weldbody2,
int weldparent2, int dsbl_filterparent) {
// filter body pair; 1: discard, 0: proceed
static int filterBodyPair(int weldbody1, int weldparent1, int asleep1,
int weldbody2, int weldparent2, int asleep2,
int dsbl_filterparent) {
// same weldbody check
if (weldbody1 == weldbody2) {
return 1;
}
// both asleep check
if (asleep1 && asleep2) {
return 1;
}
// asleep and static check
if ((asleep1 && !weldbody2) || (asleep2 && !weldbody1)) {
return 1;
}
// weldparent check
if ((!dsbl_filterparent && weldbody1 != 0 && weldbody2 != 0) &&
(weldbody1 == weldparent2 || weldbody2 == weldparent1)) {
@@ -1126,6 +1137,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
int npair = 0, nbody = m->nbody, ngeom = m->ngeom;
int nvert = m->nflexvert, nflex = m->nflex, nbodyflex = m->nbody + m->nflex;
int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT);
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < nbody;
mjtNum cov[9], cen[3], eigval[3], frame[9], quat[4];
// init with pairs involving always-colliding bodies
@@ -1138,7 +1150,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
// b1 is world body with geoms, or world-welded body with plane
if ((b1 == 0 && m->body_geomnum[b1] > 0) ||
(m->body_weldid[b1] == 0 && hasPlane(m, b1))) {
// add b1:body pairs that are not welded together
// add b1:b2 pairs that are not welded together
for (int b2=0; b2 < nbody; b2++) {
// cannot collide
if (!canCollide(m, b2)) {
@@ -1148,7 +1160,8 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
// welded together
int weld2 = m->body_weldid[b2];
int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
if (filterBodyPair(0, 0, weld2, parent_weld2, dsbl_filterparent)) {
int asleep2 = sleep_filter ? d->body_awake[b2] == mjS_ASLEEP : 0;
if (filterBodyPair(0, 0, 1, weld2, parent_weld2, asleep2, dsbl_filterparent)) {
continue;
}
@@ -1230,14 +1243,17 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
int bf1 = bfid[sappair[i] >> 16];
int bf2 = bfid[sappair[i] & 0xFFFF];
// body pair: prune based on weld filter
// body pair: prune based on sleep filter and weld filter
if (bf1 < nbody && bf2 < nbody) {
int asleep1 = sleep_filter ? d->body_awake[bf1] == mjS_ASLEEP : 0;
int asleep2 = sleep_filter ? d->body_awake[bf2] == mjS_ASLEEP : 0;
int weld1 = m->body_weldid[bf1];
int weld2 = m->body_weldid[bf2];
int parent_weld1 = m->body_weldid[m->body_parentid[weld1]];
int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
if (filterBodyPair(weld1, parent_weld1, weld2, parent_weld2,
if (filterBodyPair(weld1, parent_weld1, asleep1,
weld2, parent_weld2, asleep2,
dsbl_filterparent)) {
continue;
}
@@ -1421,6 +1437,15 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
if (ipair >= 0) {
g1 = m->pair_geom1[ipair];
g2 = m->pair_geom2[ipair];
// sleep filtering for explicit pairs
if (mjENABLED(mjENBL_SLEEP)) {
int b1 = m->geom_bodyid[g1];
int b2 = m->geom_bodyid[g2];
if (d->body_awake[b1] != mjS_AWAKE && d->body_awake[b2] != mjS_AWAKE) {
return;
}
}
}
// order geoms by type
src/engine/engine_core_constraint.c
+100 -25
View File
@@ -26,6 +26,7 @@
#include "engine/engine_core_util.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_memory.h"
#include "engine/engine_sleep.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
@@ -377,6 +378,9 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
return;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
mj_markStack(d);
// allocate space
@@ -392,10 +396,16 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// find active equality constraints
for (int i=0; i < m->neq; i++) {
// skip inactive
if (!d->eq_active[i]) {
continue;
}
// skip sleeping
if (sleep_filter && mj_sleepState(m, d, mjOBJ_EQUALITY, i) == mjS_ASLEEP) {
continue;
}
// get constraint data
data = m->eq_data + mjNEQDATA*i;
id[0] = m->eq_obj1id[i];
@@ -649,6 +659,9 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
return;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
mj_markStack(d);
// allocate Jacobian
@@ -656,7 +669,16 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
// find frictional dofs
for (int i=0; i < nv; i++) {
if (m->dof_frictionloss[i] > 0) {
// no friction loss: skip
if (!m->dof_frictionloss[i]) {
continue;
}
// sleeping tree: skip
if (sleep_filter && mj_sleepState(m, d, mjOBJ_DOF, i) == mjS_ASLEEP) {
continue;
}
// prepare Jacobian: sparse or dense
if (issparse) {
jac[0] = 1;
@@ -670,7 +692,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
1, mjCNSTR_FRICTION_DOF, i,
issparse ? 1 : 0,
issparse ? &i : NULL);
}
}
// find frictional tendons
@@ -696,7 +718,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
// joint and tendon limits
void mj_instantiateLimit(const mjModel* m, mjData* d) {
int side, nv = m->nv, issparse = mj_isSparse(m);
int nv = m->nv, issparse = mj_isSparse(m);
mjtNum margin, value, dist, angleAxis[3];
mjtNum *jac;
@@ -705,6 +727,9 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
return;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
mj_markStack(d);
// allocate Jacobian
@@ -712,7 +737,16 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// find joint limits
for (int i=0; i < m->njnt; i++) {
if (m->jnt_limited[i]) {
// no limit: skip
if (!m->jnt_limited[i]) {
continue;
}
// sleeping tree: skip
if (sleep_filter && mj_sleepState(m, d, mjOBJ_JOINT, i) == mjS_ASLEEP) {
continue;
}
// get margin
margin = m->jnt_margin[i];
@@ -722,7 +756,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
value = d->qpos[m->jnt_qposadr[i]];
// process lower and upper limits
for (side=-1; side <= 1; side+=2) {
for (int side=-1; side <= 1; side+=2) {
// compute distance (negative: penetration)
dist = side * (m->jnt_range[2*i+(side+1)/2] - value);
@@ -765,7 +799,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
if (issparse) {
// prepare dof index array
int chain[3] = {
m->jnt_dofadr[i],
m->jnt_dofadr[i] + 0,
m->jnt_dofadr[i] + 1,
m->jnt_dofadr[i] + 2
};
@@ -791,7 +825,6 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
}
}
}
}
// find tendon limits
for (int i=0; i < m->ntendon; i++) {
@@ -801,7 +834,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
margin = m->tendon_margin[i];
// process lower and upper limits
for (side=-1; side <= 1; side+=2) {
for (int side=-1; side <= 1; side+=2) {
// compute distance (negative: penetration)
dist = side * (m->tendon_range[2*i+(side+1)/2] - value);
@@ -913,6 +946,7 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di
}
}
// frictionless and frictional contacts
void mj_instantiateContact(const mjModel* m, mjData* d) {
int ispyramid = mj_isPyramidal(m), issparse = mj_isSparse(m), ncon = d->ncon;
@@ -1560,6 +1594,9 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
return 0;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
mj_markStack(d);
if (nnz) {
@@ -1569,7 +1606,16 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
// find active equality constraints
for (int i=0; i < neq; i++) {
if (d->eq_active[i]) {
// skip inactive
if (!d->eq_active[i]) {
continue;
}
// skip sleeping
if (sleep_filter && mj_sleepState(m, d, mjOBJ_EQUALITY, i) == mjS_ASLEEP) {
continue;
}
id[0] = m->eq_obj1id[i];
id[1] = m->eq_obj2id[i];
size = 0;
@@ -1673,7 +1719,6 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
ne += mj_addConstraintCount(m, size, NV);
nnze += (m->eq_type[i] == mjEQ_FLEX) ? NV : size*NV;
}
}
if (nnz) {
*nnz += nnze;
@@ -1693,12 +1738,23 @@ static int mj_nf(const mjModel* m, const mjData* d, int *nnz) {
return 0;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
for (int i=0; i < nv; i++) {
if (m->dof_frictionloss[i] > 0) {
// no friction loss: skip
if (!m->dof_frictionloss[i]) {
continue;
}
// sleeping tree: skip
if (sleep_filter && !d->tree_awake[m->dof_treeid[i]]) {
continue;
}
nf += mj_addConstraintCount(m, 1, 1);
if (nnz) *nnz += 1;
}
}
for (int i=0; i < ntendon; i++) {
if (m->tendon_frictionloss[i] > 0) {
@@ -1723,15 +1779,22 @@ static int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
return 0;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
for (int i=0; i < m->njnt; i++) {
if (!m->jnt_limited[i]) {
continue;
}
// sleeping tree: skip
if (sleep_filter && !d->tree_awake[m->dof_treeid[m->jnt_dofadr[i]]]) {
continue;
}
margin = m->jnt_margin[i];
// slider and hinge joint limits can be bilateral, check both sides
// SLIDE and HINGE joint limits can be bilateral, check both sides
if (m->jnt_type[i] == mjJNT_SLIDE || m->jnt_type[i] == mjJNT_HINGE) {
value = d->qpos[m->jnt_qposadr[i]];
for (side=-1; side <= 1; side+=2) {
@@ -1742,6 +1805,8 @@ static int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
}
}
}
// BALL joint limits are always unilateral
else if (m->jnt_type[i] == mjJNT_BALL) {
mjtNum angleAxis[3];
int adr = m->jnt_qposadr[i];
@@ -1757,21 +1822,14 @@ static int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
}
}
// tendon limits
for (int i=0; i < ntendon; i++) {
if (m->tendon_limited[i]) {
value = d->ten_length[i];
margin = m->tendon_margin[i];
// tendon limits can be bilateral, check both sides
for (side=-1; side <= 1; side+=2) {
dist = side * (m->tendon_range[2*i+(side+1)/2] - value);
if (dist < margin) {
int count = tendonLimit(m, d->ten_length, i);
for (int j = 0; j < count; j++) {
nl += mj_addConstraintCount(m, 1, d->ten_J_rownnz[i]);
if (nnz) *nnz += d->ten_J_rownnz[i];
}
}
}
}
return nl;
}
@@ -1786,6 +1844,9 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
return 0;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
mj_markStack(d);
int *chain = mjSTACKALLOC(d, m->nv, int);
@@ -1804,6 +1865,21 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
continue;
}
// check for contact with sleeping tree; SHOULD NOT OCCUR
if (sleep_filter) {
int g1 = con->geom[0];
int g2 = con->geom[1];
if (g1 >= 0 && g2 >= 0) {
int b1 = m->body_weldid[m->geom_bodyid[g1]];
int b2 = m->body_weldid[m->geom_bodyid[g2]];
int asleep1 = d->body_awake[b1] == mjS_ASLEEP;
int asleep2 = d->body_awake[b2] == mjS_ASLEEP;
if (asleep1 || asleep2) {
mjERROR("contact %d involves sleeping geom %d", i, asleep1 ? g1 : g2);
}
}
}
// compute NV only if nnz requested
int NV = 0;
if (nnz) {
@@ -1908,8 +1984,7 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
d->tendon_efcadr[i] = -1;
}
// reset nefc for the instantiation functions,
// and instantiate all elements of Jacobian
// reset nefc for the instantiation functions, instantiate all elements of Jacobian
d->nefc = 0;
mj_instantiateEquality(m, d);
mj_instantiateFriction(m, d);
src/engine/engine_core_smooth.c
+291 -87
View File
@@ -25,17 +25,19 @@
#include "engine/engine_crossplatform.h"
#include "engine/engine_macro.h"
#include "engine/engine_memory.h"
#include "engine/engine_sleep.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_sparse.h"
#include "engine/engine_util_spatial.h"
//--------------------------- position -------------------------------------------------------------
// forward kinematics
void mj_kinematics(const mjModel* m, mjData* d) {
int nbody = m->nbody, nsite = m->nsite, ngeom = m->ngeom;
// forward kinematics part 1: bodies
void mj_kinematics1(const mjModel* m, mjData* d) {
int nbody = m->nbody;
// set world position and orientation
mju_zero3(d->xpos);
@@ -46,8 +48,15 @@ void mj_kinematics(const mjModel* m, mjData* d) {
d->xmat[0] = d->xmat[4] = d->xmat[8] = 1;
d->ximat[0] = d->ximat[4] = d->ximat[8] = 1;
int sleep_filter = mjENABLED(mjENBL_SLEEP);
// compute global cartesian positions and orientations of all bodies
for (int i=1; i < nbody; i++) {
// skip static bodies
if (sleep_filter) {
if (d->body_awake[i] == mjS_STATIC) continue;
}
mjtNum xpos[3], xquat[4];
int jntadr = m->body_jntadr[i];
int jntnum = m->body_jntnum[i];
@@ -147,53 +156,124 @@ void mj_kinematics(const mjModel* m, mjData* d) {
}
}
// assign xquat and xpos, construct xmat
// normalize quaternion
mju_normalize4(xquat);
// sleeping body, check for mismatch
if (sleep_filter && jntnum && d->body_awake[i] == mjS_ASLEEP) {
// compare new and existing xpos and xquat
const mjtNum* pos = d->xpos+3*i;
const mjtNum* xq = d->xquat+4*i;
int match = xpos[0] == pos[0] && xpos[1] == pos[1] && xpos[2] == pos[2] &&
xquat[0] == xq[0] && xquat[1] == xq[1] && xquat[2] == xq[2] && xquat[3] == xq[3];
// match: continue to next body
if (match) {
continue;
}
// mismatch: mark the tree for waking later (in mj_wake)
else {
d->tree_awake[m->body_treeid[i]] = 1;
}
}
// assign xquat and xpos, construct xmat
mju_copy4(d->xquat+4*i, xquat);
mju_copy3(d->xpos+3*i, xpos);
mju_quat2Mat(d->xmat+9*i, xquat);
}
}
// forward kinematics part 2: body inertias, geoms and sites
void mj_kinematics2(const mjModel* m, mjData* d) {
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
// compute/copy Cartesian positions and orientations of body inertial frames
for (int i=1; i < nbody; i++) {
for (int b=1; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
mj_local2Global(d, d->xipos+3*i, d->ximat+9*i,
m->body_ipos+3*i, m->body_iquat+4*i,
i, m->body_sameframe[i]);
}
// compute/copy Cartesian positions and orientations of geoms
for (int i=0; i < ngeom; i++) {
mj_local2Global(d, d->geom_xpos+3*i, d->geom_xmat+9*i,
m->geom_pos+3*i, m->geom_quat+4*i,
m->geom_bodyid[i], m->geom_sameframe[i]);
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
// skip geom in sleeping or static body
if (sleep_filter && d->body_awake[i] != mjS_AWAKE) continue;
int start = m->body_geomadr[i];
int end = start + m->body_geomnum[i];
for (int g=start; g < end; g++) {
mj_local2Global(d, d->geom_xpos+3*g, d->geom_xmat+9*g,
m->geom_pos+3*g, m->geom_quat+4*g,
m->geom_bodyid[g], m->geom_sameframe[g]);
}
}
// compute/copy Cartesian positions and orientations of sites
int nsite = m->nsite;
for (int i=0; i < nsite; i++) {
int bodyid = m->site_bodyid[i];
// skip site in sleeping or static body
if (sleep_filter && d->body_awake[bodyid] != mjS_AWAKE) continue;
mj_local2Global(d, d->site_xpos+3*i, d->site_xmat+9*i,
m->site_pos+3*i, m->site_quat+4*i,
m->site_bodyid[i], m->site_sameframe[i]);
bodyid, m->site_sameframe[i]);
}
}
// forward kinematics
void mj_kinematics(const mjModel* m, mjData* d) {
mj_kinematics1(m, d);
if (mj_wake(m, d)) {
mj_updateSleep(m, d);
}
mj_kinematics2(m, d);
}
// map inertias and motion dofs to global frame centered at subtree-CoM
void mj_comPos(const mjModel* m, mjData* d) {
int nbody = m->nbody, njnt = m->njnt;
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
int nparent = sleep_filter ? d->nparent_awake : m->nbody;
// subtree_com: initialize with body moment
for (int i=0; i < nbody; i++) {
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
mju_scl3(d->subtree_com+3*i, d->xipos+3*i, m->body_mass[i]);
}
// subtree_com: accumulate to parent in backward pass
for (int i=nbody-1; i > 0; i--) {
int j = m->body_parentid[i];
mju_addTo3(d->subtree_com+3*j, d->subtree_com+3*i);
for (int b=nparent-1; b >= 0; b--) {
int i = sleep_filter ? d->parent_awake_ind[b] : b;
if (!i) continue;
// accumulate moment to parent, rescale if sleeping
int parent = m->body_parentid[i];
if (sleep_filter && d->body_awake[i] == mjS_ASLEEP) {
mjtNum child_moment[3];
mju_scl3(child_moment, d->subtree_com+3*i, m->body_subtreemass[i]);
mju_addTo3(d->subtree_com+3*parent, child_moment);
} else {
mju_addTo3(d->subtree_com+3*parent, d->subtree_com+3*i);
}
}
// subtree_com: normalize
for (int i=0; i < nbody; i++) {
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
if (m->body_subtreemass[i] < mjMINVAL) {
mju_copy3(d->subtree_com+3*i, d->xipos+3*i);
} else {
@@ -205,22 +285,30 @@ void mj_comPos(const mjModel* m, mjData* d) {
mju_zero(d->cinert, 10);
// map inertias to frame centered at subtree_com
for (int i=1; i < nbody; i++) {
for (int b=1; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
mjtNum offset[3];
mju_sub3(offset, d->xipos+3*i, d->subtree_com+3*m->body_rootid[i]);
mju_inertCom(d->cinert+10*i, m->body_inertia+3*i, d->ximat+9*i,
offset, m->body_mass[i]);
mju_inertCom(d->cinert+10*i, m->body_inertia+3*i, d->ximat+9*i, offset, m->body_mass[i]);
}
// map motion dofs to global frame centered at subtree_com
for (int j=0; j < njnt; j++) {
// get dof address, body index
for (int b=1; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
int jntnum = m->body_jntnum[i];
if (!jntnum) continue;
int start = m->body_jntadr[i];
int end = start + jntnum;
for (int j=start; j < end; j++) {
// get cdof address
int da = 6*m->jnt_dofadr[j];
int bi = m->jnt_bodyid[j];
// compute com-anchor vector
mjtNum offset[3], axis[3];
mju_sub3(offset, d->subtree_com+3*m->body_rootid[bi], d->xanchor+3*j);
mju_sub3(offset, d->subtree_com+3*m->body_rootid[i], d->xanchor+3*j);
// create motion dof
int skip = 0;
@@ -228,22 +316,22 @@ void mj_comPos(const mjModel* m, mjData* d) {
case mjJNT_FREE:
// translation components: x, y, z in global frame
mju_zero(d->cdof+da, 18);
for (int i=0; i < 3; i++) {
d->cdof[da+3+7*i] = 1;
}
d->cdof[da+3+7*0] = 1;
d->cdof[da+3+7*1] = 1;
d->cdof[da+3+7*2] = 1;
// rotation components: same as ball
skip = 18;
mjFALLTHROUGH;
case mjJNT_BALL:
for (int i=0; i < 3; i++) {
for (int k=0; k < 3; k++) {
// I_3 rotation in child frame (assume no subsequent rotations)
axis[0] = d->xmat[9*bi+i+0];
axis[1] = d->xmat[9*bi+i+3];
axis[2] = d->xmat[9*bi+i+6];
axis[0] = d->xmat[9*i + k + 0];
axis[1] = d->xmat[9*i + k + 3];
axis[2] = d->xmat[9*i + k + 6];
mju_dofCom(d->cdof+da+skip+6*i, axis, offset);
mju_dofCom(d->cdof+da+skip+6*k, axis, offset);
}
break;
@@ -257,22 +345,31 @@ void mj_comPos(const mjModel* m, mjData* d) {
}
}
}
}
// compute camera and light positions and orientations
void mj_camlight(const mjModel* m, mjData* d) {
mjtNum pos[3], matT[9];
int ncam = m->ncam, nlight = m->nlight;
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
// compute Cartesian positions and orientations of cameras
for (int i=0; i < m->ncam; i++) {
// default processing for fixed mode
mj_local2Global(d, d->cam_xpos+3*i, d->cam_xmat+9*i,
m->cam_pos+3*i, m->cam_quat+4*i, m->cam_bodyid[i], 0);
for (int i=0; i < ncam; i++) {
// get camera body id and target body id
int id = m->cam_bodyid[i];
int id1 = m->cam_targetbodyid[i];
// skip camera if both body and target body are asleep or static
if (sleep_filter && d->body_awake[id] != mjS_AWAKE) {
if (id1 < 0 || d->body_awake[id1] != mjS_AWAKE) {
continue;
}
}
// default processing for fixed mode
mj_local2Global(d, d->cam_xpos+3*i, d->cam_xmat+9*i,
m->cam_pos+3*i, m->cam_quat+4*i, id, 0);
// adjust for mode
switch ((mjtCamLight) m->cam_mode[i]) {
case mjCAMLIGHT_FIXED:
@@ -297,6 +394,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
case mjCAMLIGHT_TARGETBODYCOM:
// only if target body is specified
if (id1 >= 0) {
mjtNum pos[3];
// get position to look at
if (m->cam_mode[i] == mjCAMLIGHT_TARGETBODY) {
mju_copy3(pos, d->xpos+3*id1);
@@ -305,6 +403,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
}
// zaxis = -desired camera direction, in global frame
mjtNum matT[9];
mju_sub3(matT+6, d->cam_xpos+3*i, pos);
mju_normalize3(matT+6);
@@ -326,15 +425,22 @@ void mj_camlight(const mjModel* m, mjData* d) {
}
// compute Cartesian positions and directions of lights
for (int i=0; i < m->nlight; i++) {
// default processing for fixed mode
mj_local2Global(d, d->light_xpos+3*i, 0, m->light_pos+3*i, 0, m->light_bodyid[i], 0);
mju_rotVecQuat(d->light_xdir+3*i, m->light_dir+3*i, d->xquat+4*m->light_bodyid[i]);
for (int i=0; i < nlight; i++) {
// get light body id and target body id
int id = m->light_bodyid[i];
int id1 = m->light_targetbodyid[i];
// skip light if both body and target body are asleep or static
if (sleep_filter && d->body_awake[id] != mjS_AWAKE) {
if (id1 < 0 || d->body_awake[id1] != mjS_AWAKE) {
continue;
}
}
// default processing for fixed mode
mj_local2Global(d, d->light_xpos+3*i, 0, m->light_pos+3*i, 0, id, 0);
mju_rotVecQuat(d->light_xdir+3*i, m->light_dir+3*i, d->xquat+4*id);
// adjust for mode
switch ((mjtCamLight) m->light_mode[i]) {
case mjCAMLIGHT_FIXED:
@@ -360,14 +466,15 @@ void mj_camlight(const mjModel* m, mjData* d) {
// only if target body is specified
if (id1 >= 0) {
// get position to look at
mjtNum lookat[3];
if (m->light_mode[i] == mjCAMLIGHT_TARGETBODY) {
mju_copy3(pos, d->xpos+3*id1);
mju_copy3(lookat, d->xpos+3*id1);
} else {
mju_copy3(pos, d->subtree_com+3*id1);
mju_copy3(lookat, d->subtree_com+3*id1);
}
// set dir
mju_sub3(d->light_xdir+3*i, pos, d->light_xpos+3*i);
mju_sub3(d->light_xdir+3*i, lookat, d->light_xpos+3*i);
}
}
@@ -665,9 +772,17 @@ void mj_tendon(const mjModel* m, mjData* d) {
mju_zero(J, nten*nv);
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
// loop over tendons
int wrapcount = 0;
for (int i=0; i < nten; i++) {
// skip sleeping tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) == mjS_ASLEEP) {
continue;
}
// initialize tendon path
int adr = m->tendon_adr[i];
d->ten_wrapadr[i] = wrapcount;
@@ -994,11 +1109,19 @@ void mj_transmission(const mjModel* m, mjData* d) {
// define stack variables required for site transmission, don't allocate
mjtNum *jacref = NULL, *moment_tmp = NULL;
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];
// skip sleeping actuator
if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
rownnz[i] = 0;
continue;
}
// extract info
int id = m->actuator_trnid[2*i];
mjtNum* gear = m->actuator_gear+6*i;
@@ -1457,9 +1580,16 @@ void mj_tendonArmature(const mjModel* m, mjData* d) {
const int* M_rowadr = m->M_rowadr;
const int* M_colind = m->M_colind;
for (int k=0; k < ntendon; k++) {
mjtNum armature = m->tendon_armature[k];
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < nv;
for (int k=0; k < ntendon; k++) {
// skip sleeping tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, k) == mjS_ASLEEP) {
continue;
}
mjtNum armature = m->tendon_armature[k];
if (!armature) {
continue;
}
@@ -1512,8 +1642,6 @@ void mj_tendonArmature(const mjModel* m, mjData* d) {
// composite rigid body inertia algorithm
void mj_crb(const mjModel* m, mjData* d) {
int nv = m->nv, nbody = m->nbody;
// outputs
mjtNum* crb = d->crb;
mjtNum* M = d->M;
@@ -1523,6 +1651,9 @@ void mj_crb(const mjModel* m, mjData* d) {
const mjtNum* cdof = d->cdof;
const mjtNum* dof_M0 = m->dof_M0;
const mjtNum* dof_armature = m->dof_armature;
const int* body_awake_ind = d->body_awake_ind;
const int* parent_awake_ind = d->parent_awake_ind;
const int* dof_awake_ind = d->dof_awake_ind;
const int* rownnz = m->M_rownnz;
const int* rowadr = m->M_rowadr;
const int* body_parentid = m->body_parentid;
@@ -1530,21 +1661,38 @@ void mj_crb(const mjModel* m, mjData* d) {
const int* dof_simplenum = m->dof_simplenum;
const int* dof_bodyid = m->dof_bodyid;
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
int nparent = sleep_filter ? d->nparent_awake : m->nbody;
int nv = sleep_filter ? d->nv_awake : m->nv;
// crb = cinert
if (!sleep_filter) {
mju_copy(crb, cinert, 10*nbody);
} else {
mju_copyRows(crb, cinert, body_awake_ind, nbody, 10);
}
// backward pass over bodies, accumulate composite inertias
for (int i=nbody - 1; i > 0; i--) {
if (body_parentid[i]) {
for (int b = nparent - 1; b >= 0; b--) {
int i = sleep_filter ? parent_awake_ind[b] : b;
if (body_parentid[i] > 0) {
mju_addTo(crb + 10*body_parentid[i], crb + 10*i, 10);
}
}
// clear M
if (!sleep_filter) {
mju_zero(M, m->nC);
} else {
mju_zeroSparse(M, rownnz, rowadr, dof_awake_ind, nv);
}
// dense forward pass over dofs
for (int i=0; i < nv; i++) {
for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : v;
// simple dof: fixed diagonal inertia
int adr = rowadr[i];
if (dof_simplenum[i]) {
@@ -1573,7 +1721,7 @@ void mj_makeM(const mjModel* m, mjData* d) {
TM_START;
mj_crb(m, d);
mj_tendonArmature(m, d);
mju_scatter(d->qM, d->M, m->mapM2M, m->nC);
mju_scatter(d->qM, d->M, m->mapM2M, m->nC); // TODO(tassa): scatter only awake dofs
TM_END(mjTIMER_POS_INERTIA);
}
@@ -1644,17 +1792,41 @@ void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD
// sparse L'*D*L factorizaton of the inertia matrix M, assumed spd
void mj_factorM(const mjModel* m, mjData* d) {
TM_START;
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
const int* index;
int nv;
// no sleep filtering: copy everything
if (!sleep_filter) {
index = NULL;
nv = m->nv;
mju_copy(d->qLD, d->M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind);
}
// sleep filtering: copy only awake dofs
else {
index = d->dof_awake_ind;
nv = d->nv_awake;
mju_copySparse(d->qLD, d->M, m->M_rownnz, m->M_rowadr, d->dof_awake_ind, d->nv_awake);
}
// factorize
mj_factorI(d->qLD, d->qLDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind, index);
TM_ADD(mjTIMER_POS_INERTIA);
}
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd (with dof skipping)
void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv,
const int* rownnz, const int* rowadr, const int* colind) {
const int* rownnz, const int* rowadr, const int* colind,
const int* index) {
// backward loop over rows
for (int k=nv-1; k >= 0; k--) {
for (int j=nv-1; j >= 0; j--) {
int k = index ? index[j] : j;
// get row k's address, diagonal index, inverse diagonal value
int start = rowadr[k];
int diag = rownnz[k] - 1;
@@ -1787,11 +1959,13 @@ void mj_solveLD_legacy(const mjModel* m, mjtNum* restrict x, int n,
}
// in-place sparse backsubstitution: x = inv(L'*D*L)*x
// in-place sparse backsubstitution: x = inv(L'*D*L)*x (with dof skipping)
void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv, int nv, int n,
const int* rownnz, const int* rowadr, const int* colind) {
const int* rownnz, const int* rowadr, const int* colind, const int* index) {
// x <- L^-T x
for (int i=nv-1; i > 0; i--) {
for (int k = nv - 1; k >= 0; k--) {
int i = index ? index[k] : k;
// skip diagonal rows
if (rownnz[i] == 1) {
continue;
@@ -1825,7 +1999,9 @@ void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv,
}
// x <- D^-1 x
for (int i=0; i < nv; i++) {
for (int k = 0; k < nv; k++) {
int i = index ? index[k] : k;
mjtNum invD_i = qLDiagInv[i];
// one vector
@@ -1842,7 +2018,9 @@ void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv,
}
// x <- L^-1 x
for (int i=1; i < nv; i++) {
for (int k = 0; k < nv; k++) {
int i = index ? index[k] : k;
// skip diagonal rows
if (rownnz[i] == 1) {
continue;
@@ -1874,8 +2052,7 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
if (x != y) {
mju_copy(x, y, n*m->nv);
}
mj_solveLD(x, d->qLD, d->qLDiagInv, m->nv, n,
m->M_rownnz, m->M_rowadr, m->M_colind);
mj_solveLD(x, d->qLD, d->qLDiagInv, m->nv, n, m->M_rownnz, m->M_rowadr, m->M_colind, NULL);
}
@@ -1930,15 +2107,15 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y,
// compute cvel, cdof_dot
void mj_comVel(const mjModel* m, mjData* d) {
int nbody = m->nbody;
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
// set world vel to 0
mju_zero(d->cvel, 6);
// forward pass over bodies
for (int i=1; i < nbody; i++) {
// get body's first dof address
int bda = m->body_dofadr[i];
for (int b=1; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
// cvel = cvel_parent
mjtNum cvel[6];
@@ -1946,6 +2123,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
// cvel = cvel_parent + cdof * qvel, cdofdot = cvel x cdof
int dofnum = m->body_dofnum[i];
int bda = m->body_dofadr[i];
mjtNum cdofdot[36];
for (int j=0; j < dofnum; j++) {
mjtNum tmp[6];
@@ -1966,9 +2144,9 @@ void mj_comVel(const mjModel* m, mjData* d) {
case mjJNT_BALL:
// compute all 3 cdofdots using parent velocity
for (int k=0; k < 3; k++) {
mju_crossMotion(cdofdot+6*(j+k), cvel, d->cdof+6*(bda+j+k));
}
mju_crossMotion(cdofdot+6*(j+0), cvel, d->cdof+6*(bda+j+0));
mju_crossMotion(cdofdot+6*(j+1), cvel, d->cdof+6*(bda+j+1));
mju_crossMotion(cdofdot+6*(j+2), cvel, d->cdof+6*(bda+j+2));
// update velocity
mju_mulDofVec(tmp, d->cdof+6*(bda+j), d->qvel+bda+j, 3);
@@ -1999,13 +2177,16 @@ void mj_comVel(const mjModel* m, mjData* d) {
// subtree linear velocity and angular momentum
void mj_subtreeVel(const mjModel* m, mjData* d) {
int nbody = m->nbody;
mjtNum dx[3], dv[3], dp[3], dL[3];
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
mj_markStack(d);
mjtNum* body_vel = mjSTACKALLOC(d, 6*m->nbody, mjtNum);
// bodywise quantities
for (int i=0; i < nbody; i++) {
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
// compute and save body velocity
mj_objectVelocity(m, d, mjOBJ_BODY, i, body_vel+6*i, 0);
@@ -2013,6 +2194,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
mju_scl3(d->subtree_linvel+3*i, body_vel+6*i+3, m->body_mass[i]);
// body angular momentum
mjtNum dv[3];
mju_mulMatTVec3(dv, d->ximat+9*i, body_vel+6*i);
dv[0] *= m->body_inertia[3*i];
dv[1] *= m->body_inertia[3*i+1];
@@ -2020,8 +2202,10 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
mju_mulMatVec3(d->subtree_angmom+3*i, d->ximat+9*i, dv);
}
// subtree linvel
for (int i=nbody-1; i >= 0; i--) {
// subtree linear velocity
for (int b=nbody-1; b >= 0; b--) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
// non-world: add linear momentum to parent
if (i) {
mju_addTo3(d->subtree_linvel+3*m->body_parentid[i], d->subtree_linvel+3*i);
@@ -2029,14 +2213,17 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
// convert linear momentum to linear velocity
mju_scl3(d->subtree_linvel+3*i, d->subtree_linvel+3*i,
1/mjMAX(mjMINVAL, m->body_subtreemass[i]));
1/mju_max(mjMINVAL, m->body_subtreemass[i]));
}
// subtree angmom
for (int i=nbody-1; i > 0; i--) {
// subtree angular momentum
for (int b=nbody-1; b > 0; b--) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
int parent = m->body_parentid[i];
// momentum wrt body i
mjtNum dx[3], dv[3], dp[3], dL[3];
mju_sub3(dx, d->xipos+3*i, d->subtree_com+3*i);
mju_sub3(dv, body_vel+6*i+3, d->subtree_linvel+3*i);
mju_scl3(dp, dv, m->body_mass[i]);
@@ -2066,8 +2253,11 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
// RNE: compute M(qpos)*qacc + C(qpos,qvel); flg_acc=0 removes inertial term
void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
int nbody = m->nbody, nv = m->nv;
mjtNum tmp[6], tmp1[6];
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
int nparent = sleep_filter ? d->nparent_awake : m->nbody;
int nv = sleep_filter ? d->nv_awake : m->nv;
mj_markStack(d);
mjtNum* loc_cacc = mjSTACKALLOC(d, m->nbody*6, mjtNum);
mjtNum* loc_cfrc_body = mjSTACKALLOC(d, m->nbody*6, mjtNum);
@@ -2079,11 +2269,14 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
}
// forward pass over bodies: accumulate cacc, set cfrc_body
for (int i=1; i < nbody; i++) {
for (int b=1; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
// get body's first dof address
int bda = m->body_dofadr[i];
// cacc = cacc_parent + cdofdot * qvel
mjtNum tmp[6];
mju_mulDofVec(tmp, d->cdof_dot+6*bda, d->qvel+bda, m->body_dofnum[i]);
mju_add(loc_cacc+6*i, loc_cacc+6*m->body_parentid[i], tmp, 6);
@@ -2096,22 +2289,27 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
// cfrc_body = cinert * cacc + cvel x (cinert * cvel)
mju_mulInertVec(loc_cfrc_body+6*i, d->cinert+10*i, loc_cacc+6*i);
mju_mulInertVec(tmp, d->cinert+10*i, d->cvel+6*i);
mjtNum tmp1[6];
mju_crossForce(tmp1, d->cvel+6*i, tmp);
mju_addTo(loc_cfrc_body+6*i, tmp1, 6);
}
// clear world cfrc_body, for style
// clear world cfrc_body
mju_zero(loc_cfrc_body, 6);
// backward pass over bodies: accumulate cfrc_body from children
for (int i=nbody-1; i > 0; i--) {
if (m->body_parentid[i]) {
mju_addTo(loc_cfrc_body+6*m->body_parentid[i], loc_cfrc_body+6*i, 6);
for (int b=nparent-1; b > 0; b--) {
int i = sleep_filter ? d->parent_awake_ind[b] : b;
int j = m->body_parentid[i];
if (j) {
mju_addTo(loc_cfrc_body+6*j, loc_cfrc_body+6*i, 6);
}
}
// result = cdof * cfrc_body
for (int i=0; i < nv; i++) {
for (int v=0; v < nv; v++) {
int i = sleep_filter ? d->dof_awake_ind[v] : v;
result[i] = mju_dot(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i], 6);
}
@@ -2308,12 +2506,18 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// add bias force due to tendon armature
void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) {
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
int ntendon = m->ntendon, nv = m->nv, issparse = mj_isSparse(m);
mjtNum* ten_Jdot = NULL;
mj_markStack(d);
// add bias term due to tendon armature
for (int i=0; i < ntendon; i++) {
// skip sleeping tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) == mjS_ASLEEP) {
continue;
}
mjtNum armature = m->tendon_armature[i];
// no armature: skip
src/engine/engine_core_smooth.h
+10 -4
View File
@@ -24,6 +24,12 @@ extern "C" {
#endif
//-------------------------- position --------------------------------------------------------------
// forward kinematics part 1: bodies
void mj_kinematics1(const mjModel* m, mjData* d);
// forward kinematics part 2: body inertias, geoms and sites
void mj_kinematics2(const mjModel* m, mjData* d);
// forward kinematics
MJAPI void mj_kinematics(const mjModel* m, mjData* d);
@@ -61,9 +67,9 @@ MJAPI void mj_makeM(const mjModel* m, mjData* d);
MJAPI void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M,
mjtNum* qLD, mjtNum* qLDiagInv);
// sparse L'*D*L factorizaton of inertia-like matrix
// sparse L'*D*L factorizaton of inertia-like matrix (only dofs in index, if given)
MJAPI void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv,
const int* rownnz, const int* rowadr, const int* colind);
const int* rownnz, const int* rowadr, const int* colind, const int* index);
// sparse L'*D*L factorizaton of the inertia matrix M, assumed spd
MJAPI void mj_factorM(const mjModel* m, mjData* d);
@@ -72,10 +78,10 @@ MJAPI void mj_factorM(const mjModel* m, mjData* d);
MJAPI void mj_solveLD_legacy(const mjModel* m, mjtNum* x, int n,
const mjtNum* qLD, const mjtNum* qLDiagInv);
// in-place sparse backsubstitution: x = inv(L'*D*L)*x
// in-place sparse backsubstitution (only dofs in index, if given): x = inv(L'*D*L)*x
// handle n vectors at once
MJAPI void mj_solveLD(mjtNum* x, const mjtNum* qLD, const mjtNum* qLDiagInv, int nv, int n,
const int* rownnz, const int* rowadr, const int* colind);
const int* rownnz, const int* rowadr, const int* colind, const int* index);
// sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d
MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n);
src/engine/engine_core_util.c
+24
View File
@@ -726,6 +726,8 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) {
}
//-------------------------- spatial frame utilities -----------------------------------------------
// compute object 6D velocity in object-centered frame, world/local orientation
void mj_objectVelocity(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local) {
@@ -894,6 +896,8 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
}
//-------------------------- miscellaneous utilities -----------------------------------------------
// extract 6D force:torque for one contact, in contact frame
void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]) {
mjContact* con;
@@ -915,6 +919,26 @@ void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]
}
// count the number of length limit violations for tendon i (0, 1 or 2)
int tendonLimit(const mjModel* m, const mjtNum* ten_length, int i) {
if (!m->tendon_limited[i]) {
return 0;
}
int nl = 0;
mjtNum value = ten_length[i];
mjtNum margin = m->tendon_margin[i];
// tendon limits can be bilateral, check both sides
for (int side = -1; side <= 1; side += 2) {
mjtNum dist = side * (m->tendon_range[2 * i + (side + 1) / 2] - value);
if (dist < margin) nl++;
}
return nl;
}
// count warnings, print only the first time
void mj_warning(mjData* d, int warning, int info) {
// check type
src/engine/engine_core_util.h
+6 -3
View File
@@ -114,17 +114,20 @@ MJAPI void mj_objectVelocity(const mjModel* m, const mjData* d,
MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local);
//-------------------------- miscellaneous ---------------------------------------------------------
// map from body local to global Cartesian coordinates
MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
const mjtNum pos[3], const mjtNum quat[4],
int body, mjtByte sameframe);
//-------------------------- miscellaneous ---------------------------------------------------------
// extract 6D force:torque for one contact, in contact frame
MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);
// count the number of length limit violations for tendon i (0, 1 or 2)
int tendonLimit(const mjModel* m, const mjtNum* ten_length, int i);
// high-level warning function: count warnings in mjData, print only the first time
MJAPI void mj_warning(mjData* d, int warning, int info);
src/engine/engine_derivative.c
+71 -12
View File
@@ -21,6 +21,7 @@
#include "engine/engine_crossplatform.h"
#include "engine/engine_memory.h"
#include "engine/engine_passive.h"
#include "engine/engine_sleep.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
@@ -521,12 +522,16 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
// derivative of cvel, cdof_dot w.r.t qvel
static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* Dcdofdot) {
int nv = m->nv, nbody = m->nbody;
int nv = m->nv, nM = m->nM;
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
int* Badr = m->B_rowadr, * Dadr = m->D_rowadr;
mjtNum mat[36], matT[36]; // 6x6 matrices
// forward pass over bodies: accumulate Dcvel, set Dcdofdot
for (int i = 1; i < nbody; i++) {
for (int b=1; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
// Dcvel = Dcvel_parent
copyFromParent(m, d, Dcvel, i);
@@ -534,7 +539,7 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D
int doflast = m->body_dofadr[i] + m->body_dofnum[i];
for (int j = m->body_dofadr[i]; j < doflast; j++) {
// number of dof ancestors of dof j
int Jadr = (j < nv - 1 ? m->dof_Madr[j + 1] : m->nM) - (m->dof_Madr[j] + 1);
int Jadr = (j < nv - 1 ? m->dof_Madr[j + 1] : nM) - (m->dof_Madr[j] + 1);
// Dcvel += D(cdof * qvel), Dcdofdot = D(cvel x cdof)
switch ((mjtJoint) m->jnt_type[m->dof_jntid[j]]) {
@@ -589,7 +594,13 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D
// subtract d qfrc_bias / d qvel from qDeriv
static void mjd_rne_vel(const mjModel* m, mjData* d) {
int nv = m->nv, nbody = m->nbody;
int nM = m->nM;
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
int nparent = sleep_filter ? d->nparent_awake : m->nbody;
int mnv = m->nv;
int nv = sleep_filter ? d->nv_awake : mnv;
const int* Badr = m->B_rowadr;
const int* Dadr = m->D_rowadr;
const int* Bnnz = m->B_rownnz;
@@ -601,19 +612,37 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
mjtNum* Dcvel = mjSTACKALLOC(d, 6*m->nB, mjtNum);
mjtNum* Dcacc = mjSTACKALLOC(d, 6*m->nB, mjtNum);
mjtNum* Dcfrcbody = mjSTACKALLOC(d, 6*m->nB, mjtNum);
mjtNum* row = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* row = mjSTACKALLOC(d, m->nv, mjtNum);
// clear
if (!sleep_filter) {
mju_zero(Dcdofdot, 6*m->nD);
mju_zero(Dcvel, 6*m->nB);
mju_zero(Dcacc, 6*m->nB);
mju_zero(Dcfrcbody, 6*m->nB);
} else {
for (int i = 0; i < nv; i++) {
int dof = d->dof_awake_ind[i];
mju_zero(Dcdofdot + 6*m->D_rowadr[dof], 6*m->D_rownnz[dof]);
}
for (int i = 0; i < nbody; i++) {
int body = d->body_awake_ind[i];
int adr = 6*m->B_rowadr[body];
int nnz = 6*m->B_rownnz[body];
mju_zero(Dcvel + adr, nnz);
mju_zero(Dcacc + adr, nnz);
mju_zero(Dcfrcbody + adr, nnz);
}
}
// compute Dcvel and Dcdofdot
mjd_comVel_vel(m, d, Dcvel, Dcdofdot);
// forward pass over bodies: accumulate Dcacc, set Dcfrcbody
for (int i=1; i < nbody; i++) {
for (int b=1; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
// Dcacc = Dcacc_parent
copyFromParent(m, d, Dcacc, i);
@@ -621,7 +650,7 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
int doflast = m->body_dofadr[i] + m->body_dofnum[i];
for (int j=m->body_dofadr[i]; j < doflast; j++) {
// number of dof ancestors of dof j
int Jadr = (j < nv - 1 ? m->dof_Madr[j + 1] : m->nM) - (m->dof_Madr[j] + 1);
int Jadr = (j < mnv - 1 ? m->dof_Madr[j + 1] : nM) - (m->dof_Madr[j] + 1);
// Dcacc += cdofdot * (D qvel)
mju_addTo(Dcacc + 6*(Badr[i] + Jadr), d->cdof_dot + 6*j, 6);
@@ -655,12 +684,15 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
mju_zero(Dcfrcbody, 6*Bnnz[0]);
// backward pass over bodies: accumulate Dcfrcbody
for (int i=m->nbody-1; i > 0; i--) {
for (int b=nparent-1; b > 0; b--) {
int i = sleep_filter ? d->parent_awake_ind[b] : b;
addToParent(m, d, Dcfrcbody, i);
}
// process all dofs, update qDeriv
for (int j=0; j < nv; j++) {
for (int v=0; v < nv; v++) {
int j = sleep_filter ? d->dof_awake_ind[v] : v;
// get body index
int i = m->dof_bodyid[j];
@@ -797,6 +829,7 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang
// add (d qfrc_actuator / d qvel) to qDeriv
void mjd_actuator_vel(const mjModel* m, mjData* d) {
int nu = m->nu;
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
// disabled: nothing to add
if (mjDISABLED(mjDSBL_ACTUATION)) {
@@ -810,6 +843,11 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
continue;
}
// skip if sleeping
if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
continue;
}
mjtNum bias_vel = 0, gain_vel = 0;
// affine bias
@@ -1401,10 +1439,14 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
return;
}
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
// fluid drag model, either body-level (inertia box) or geom-level (ellipsoid)
if (m->opt.viscosity > 0 || m->opt.density > 0) {
int nbody = m->nbody;
for (int i=1; i < nbody; i++) {
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
if (m->body_mass[i] < mjMINVAL) {
continue;
}
@@ -1430,7 +1472,9 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
// dof damping
int nv = m->nv;
for (int i=0; i < nv; i++) {
int nv_awake = sleep_filter ? d->nv_awake : nv;
for (int j = 0; j < nv_awake; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j;
d->qDeriv[m->D_rowadr[i] + m->D_diag[i]] -= m->dof_damping[i];
}
@@ -1464,6 +1508,15 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
// tendon damping
int ntendon = m->ntendon;
for (int i=0; i < ntendon; i++) {
// skip tendon in one or two sleeping trees
if (sleep_filter) {
int treenum = m->tendon_treenum[i];
int id1 = m->tendon_treeid[2*i];
if (treenum == 1 && !d->tree_awake[id1]) continue;
int id2 = m->tendon_treeid[2*i+1];
if (treenum == 2 && !d->tree_awake[id1] && !d->tree_awake[id2]) continue;
}
mjtNum B = -m->tendon_damping[i];
if (!B) {
@@ -1485,8 +1538,14 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
// analytical derivative of smooth forces w.r.t velocities:
// d->qDeriv = d (qfrc_actuator + qfrc_passive - [qfrc_bias]) / d qvel
void mjd_smooth_vel(const mjModel* m, mjData* d, int flg_bias) {
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
// clear qDeriv
if (!sleep_filter) {
mju_zero(d->qDeriv, m->nD);
} else {
mju_zeroSparse(d->qDeriv, m->D_rownnz, m->D_rowadr, d->dof_awake_ind, d->nv_awake);
}
// qDeriv += d qfrc_actuator / d qvel
mjd_actuator_vel(m, d);
src/engine/engine_forward.c
+136 -28
View File
@@ -29,12 +29,12 @@
#include "engine/engine_derivative.h"
#include "engine/engine_inverse.h"
#include "engine/engine_island.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_memory.h"
#include "engine/engine_passive.h"
#include "engine/engine_plugin.h"
#include "engine/engine_sensor.h"
#include "engine/engine_sleep.h"
#include "engine/engine_solver.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
@@ -51,8 +51,10 @@
// check positions, reset if bad
void mj_checkPos(const mjModel* m, mjData* d) {
for (int i=0; i < m->nq; i++) {
if (mju_isBad(d->qpos[i])) {
int nq = m->nq;
const mjtNum* qpos = d->qpos;
for (int i=0; i < nq; i++) {
if (mju_isBad(qpos[i])) {
mj_warning(d, mjWARN_BADQPOS, i);
if (!mjDISABLED(mjDSBL_AUTORESET)) {
mj_resetData(m, d);
@@ -67,7 +69,12 @@ void mj_checkPos(const mjModel* m, mjData* d) {
// check velocities, reset if bad
void mj_checkVel(const mjModel* m, mjData* d) {
for (int i=0; i < m->nv; i++) {
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
int nv = sleep_filter ? d->nv_awake : m->nv;
for (int j=0; j < nv; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j;
if (mju_isBad(d->qvel[i])) {
mj_warning(d, mjWARN_BADQVEL, i);
if (!mjDISABLED(mjDSBL_AUTORESET)) {
@@ -83,7 +90,12 @@ void mj_checkVel(const mjModel* m, mjData* d) {
// check accelerations, reset if bad
void mj_checkAcc(const mjModel* m, mjData* d) {
for (int i=0; i < m->nv; i++) {
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
int nv = sleep_filter ? d->nv_awake : m->nv;
for (int j=0; j < nv; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j;
if (mju_isBad(d->qacc[i])) {
mj_warning(d, mjWARN_BADQACC, i);
if (!mjDISABLED(mjDSBL_AUTORESET)) {
@@ -135,6 +147,10 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
mj_camlight(m, d);
mj_flex(m, d);
mj_tendon(m, d);
if (mj_wakeTendon(m, d)) {
mj_updateSleep(m, d);
}
TM_END(mjTIMER_POS_KINEMATICS);
// no threadpool: inertia and collision on main thread
@@ -168,6 +184,15 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
mju_taskJoin(&tasks[1]);
}
if (mj_wakeCollision(m, d)) {
mj_updateSleep(m, d);
mj_collision(m, d);
}
if (mj_wakeEquality(m, d)) {
mj_updateSleep(m, d);
}
TM_RESTART;
mj_makeConstraint(m, d);
mj_island(m, d);
@@ -277,6 +302,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// clear actuator_force
mju_zero(force, nu);
int sleep_filter = mjENABLED(mjENBL_SLEEP);
// disabled or no actuation: return
if (nu == 0 || mjDISABLED(mjDSBL_ACTUATION)) {
mju_zero(d->qfrc_actuator, nv);
@@ -305,6 +332,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// act_dot for stateful actuators
for (int i=0; i < nu; i++) {
if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
continue;
}
int act_first = m->actuator_actadr[i];
if (act_first < 0) {
continue;
@@ -371,6 +402,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// force = gain .* [ctrl/act] + bias
for (int i=0; i < nu; i++) {
// skip if sleeping
if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
continue;
}
// skip if disabled
if (mj_actuatorDisabled(m, i)) {
continue;
@@ -548,16 +584,38 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// add up all non-constraint forces, compute qacc_smooth
void mj_fwdAcceleration(const mjModel* m, mjData* d) {
int nv = m->nv;
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
int nv;
const int* index;
// qfrc_smooth = sum of all non-constraint forces
mju_sub(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, nv); // qfrc_bias is negative
// qfrc_smooth = qfrc_passive - qfrc_bias + qfrc_applied + qfrc_actuator
if (!sleep_filter) {
nv = m->nv;
index = NULL;
mju_sub(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, nv);
mju_addTo(d->qfrc_smooth, d->qfrc_applied, nv);
mju_addTo(d->qfrc_smooth, d->qfrc_actuator, nv);
} else {
nv = d->nv_awake;
index = d->dof_awake_ind;
mju_subInd(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, index, nv);
mju_addToInd(d->qfrc_smooth, d->qfrc_applied, index, nv);
mju_addToInd(d->qfrc_smooth, d->qfrc_actuator, index, nv);
}
// qfrc_smooth += project(xfrc_applied)
mj_xfrcAccumulate(m, d, d->qfrc_smooth);
// copy for in-place solve: qacc_smooth = qfrc_smooth
if (!sleep_filter) {
mju_copy(d->qacc_smooth, d->qfrc_smooth, nv);
} else {
mju_copyInd(d->qacc_smooth, d->qfrc_smooth, index, nv);
}
// qacc_smooth = M \ qfrc_smooth
mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1);
mj_solveLD(d->qacc_smooth, d->qLD, d->qLDiagInv, nv, 1,
m->M_rownnz, m->M_rowadr, m->M_colind, index);
}
@@ -746,7 +804,6 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
solve_threaded(m, d, m->opt.solver == mjSOL_NEWTON);
}
// copy back solver outputs (scatter dofs since ni <= nv)
mju_scatter(d->qacc, d->iacc, d->map_idof2dof, nidof);
mju_scatter(d->qfrc_constraint, d->ifrc_constraint, d->map_idof2dof, nidof);
@@ -800,11 +857,27 @@ static void mj_advance(const mjModel* m, mjData* d,
}
}
// put islands to sleep according to velocity tolerance
if (mj_sleep(m, d)) {
// if any trees put to sleep (qvel set to 0), recompute all velocity-dependent quantities
mj_forwardSkip(m, d, mjSTAGE_POS, 0);
// update sleep indices
mj_updateSleep(m, d);
}
// advance velocities
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
if (sleep_filter) {
mju_addToSclInd(d->qvel, qacc, d->dof_awake_ind, m->opt.timestep, d->nv_awake);
} else {
mju_addToScl(d->qvel, qacc, m->opt.timestep, m->nv);
}
// advance positions with qvel if given, d->qvel otherwise (semi-implicit)
mj_integratePos(m, d->qpos, qvel ? qvel : d->qvel, m->opt.timestep);
const int* index = sleep_filter ? d->body_awake_ind : NULL;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
mj_integratePosInd(m, d->qpos, qvel ? qvel : d->qvel, m->opt.timestep, index, nbody);
// advance time
d->time += m->opt.timestep;
@@ -831,15 +904,20 @@ static void mj_advance(const mjModel* m, mjData* d,
// Euler integrator, semi-implicit in velocity, possibly skipping factorisation
void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
TM_START;
int nv = m->nv, nC = m->nC;
mj_markStack(d);
mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* qacc = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* qfrc = mjSTACKALLOC(d, m->nv, mjtNum);
mjtNum* qacc = mjSTACKALLOC(d, m->nv, mjtNum);
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
int nv = sleep_filter ? d->nv_awake : m->nv;
const int* dof_awake_ind = sleep_filter ? d->dof_awake_ind : NULL;
// check for dof damping if disable flag is not set
int dof_damping = 0;
if (!mjDISABLED(mjDSBL_EULERDAMP) && !mjDISABLED(mjDSBL_DAMPER)) {
for (int i=0; i < nv; i++) {
for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : v;
if (m->dof_damping[i] > 0) {
dof_damping = 1;
break;
@@ -849,27 +927,43 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// no damping or disabled: explicit velocity integration
if (!dof_damping) {
if (sleep_filter) {
mju_copyInd(qacc, d->qacc, dof_awake_ind, nv);
} else {
mju_copy(qacc, d->qacc, nv);
}
}
// damping: integrate implicitly
else {
if (!skipfactor) {
// qH = M + h*diag(B)
mju_copy(d->qH, d->M, nC);
for (int i=0; i < nv; i++) {
// qH = M
if (sleep_filter) {
mju_copySparse(d->qH, d->M, m->M_rownnz, m->M_rowadr, dof_awake_ind, d->nv_awake);
} else {
mju_copy(d->qH, d->M, m->nC);
}
// qH += h*diag(B)
for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : v;
d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
}
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind);
}
// solve
if (sleep_filter) {
mju_addInd(qfrc, d->qfrc_smooth, d->qfrc_constraint, dof_awake_ind, nv);
mju_copyInd(qacc, qfrc, dof_awake_ind, nv);
} else {
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
mju_copy(qacc, qfrc, m->nv);
mju_copy(qacc, qfrc, nv);
}
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind);
}
// advance state and time
@@ -995,14 +1089,23 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
// fully implicit in velocity, possibly skipping factorization
void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
TM_START;
int nv = m->nv, nD = m->nD, nC = m->nC;
int nD = m->nD, nC = m->nC;
mj_markStack(d);
mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* qacc = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* qfrc = mjSTACKALLOC(d, m->nv, mjtNum);
mjtNum* qacc = mjSTACKALLOC(d, m->nv, mjtNum);
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
int nv = sleep_filter ? d->nv_awake : m->nv;
const int* dof_awake_ind = sleep_filter ? d->dof_awake_ind : NULL;
// set qfrc = qfrc_smooth + qfrc_constraint
if (sleep_filter) {
mju_addInd(qfrc, d->qfrc_smooth, d->qfrc_constraint, dof_awake_ind, nv);
} else {
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
}
// IMPLICIT
if (m->opt.integrator == mjINT_IMPLICIT) {
@@ -1018,11 +1121,12 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
// factorize qLU
int* scratch = mjSTACKALLOC(d, nv, int);
mju_factorLUSparse(d->qLU, nv, scratch, m->D_rownnz, m->D_rowadr, m->D_colind);
mju_factorLUSparse(d->qLU, nv, scratch, m->D_rownnz, m->D_rowadr, m->D_colind, dof_awake_ind);
}
// solve for qacc: (M - dt*qDeriv) * qacc = qfrc
mju_solveLUSparse(qacc, d->qLU, qfrc, nv, m->D_rownnz, m->D_rowadr, m->D_diag, m->D_colind);
mju_solveLUSparse(qacc, d->qLU, qfrc, nv, m->D_rownnz, m->D_rowadr, m->D_diag, m->D_colind,
dof_awake_ind);
}
// IMPLICITFAST
@@ -1038,13 +1142,17 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
mju_addScl(d->qH, d->M, d->qH, -m->opt.timestep, nC);
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind);
}
// solve for qacc: (M - dt*qDeriv) * qacc = qfrc
if (sleep_filter) {
mju_copyInd(qacc, qfrc, dof_awake_ind, nv);
} else {
mju_copy(qacc, qfrc, nv);
}
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind);
} else {
mjERROR("integrator must be implicit or implicitfast");
src/engine/engine_io.c
+69
View File
@@ -26,6 +26,8 @@
#include <mujoco/mjplugin.h>
#include <mujoco/mjsan.h> // IWYU pragma: keep
#include <mujoco/mjxmacro.h>
#include "engine/engine_core_smooth.h"
#include "engine/engine_forward.h"
#include "engine/engine_init.h"
#include "engine/engine_macro.h"
#include "engine/engine_memory.h"
@@ -1087,6 +1089,11 @@ void mj_makeRawData(mjData** dest, const mjModel* m) {
// clear nplugin (overwritten by _initPlugin)
d->nplugin = 0;
// set awake array sizes to default (all awake)
d->ntree_awake = m->ntree;
d->nbody_awake = d->nparent_awake = m->nbody;
d->nv_awake = m->nv;
// copy pointer if allocated here
if (allocate) {
*dest = d;
@@ -1369,6 +1376,68 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
d->tree_asleep[i] = kAwake;
}
// sleep enabled: handle static bodies and trees marked as mjSLEEP_INIT
if (mjENABLED(mjENBL_SLEEP)) {
// count trees initialized as asleep
int num_asleep_init = 0;
for (int i=0; i < m->ntree; i++) {
num_asleep_init += (m->tree_sleep_policy[i] == mjSLEEP_INIT);
}
// update sleep arrays, treat static bodies as awake
mj_updateSleepInit(m, d, /*flg_staticawake*/ 1);
// partial mj_fwdPosition, functions that update STATIC values
if (!num_asleep_init) {
mj_kinematics(m, d);
mj_comPos(m, d);
mj_camlight(m, d);
mj_tendon(m, d);
}
// if any trees initialized as sleeping call entire mj_forward, put them to sleep
else {
mj_forward(m, d);
// mark asleep-init trees as ready to sleep
for (int i=0; i < m->ntree; i++) {
int init = m->tree_sleep_policy[i] == mjSLEEP_INIT;
d->tree_asleep[i] = init ? -1 : kAwake;
}
int nslept = mj_sleep(m, d);
// raise error if any failed to sleep
if (nslept != num_asleep_init) {
// find root body of the first tree that could not be slept
int root = -1;
for (int i=0; i < m->ntree; i++) {
if (m->tree_sleep_policy[i] == mjSLEEP_INIT && d->tree_asleep[i] < 0) {
root = m->tree_bodyadr[i];
break;
}
}
// free all memory held by d just before aborting
mj_deleteData(d);
// raise error and abort
const char* hasname = mj_id2name(m, mjOBJ_BODY, root);
const char* name = hasname ? hasname : "";
mjERROR("%d trees were marked as sleep='init' but only %d could be slept.\n"
"Body '%s' (id=%d) is the root of the first tree that could not be slept.",
num_asleep_init, nslept, name, root);
}
// clear arrays to avoid MSAN errors upon mid-step wake
mju_zero(d->qacc_smooth, m->nv);
mju_zero(d->qfrc_smooth, m->nv);
// clear arena
mj_clearEfc(d);
}
}
// update sleep arrays and counters
mj_updateSleep(m, d);
src/engine/engine_passive.c
+82 -39
View File
@@ -25,6 +25,7 @@
#include "engine/engine_crossplatform.h"
#include "engine/engine_memory.h"
#include "engine/engine_plugin.h"
#include "engine/engine_sleep.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
@@ -58,25 +59,31 @@ static void inline GradSquaredLengths(mjtNum gradient[6][2][3],
// spring and damper forces
static void mj_springdamper(const mjModel* m, mjData* d) {
int nv = m->nv, njnt = m->njnt, ntendon = m->ntendon;
int nv = m->nv, ntendon = m->ntendon;
int has_spring = !mjDISABLED(mjDSBL_SPRING);
int has_damping = !mjDISABLED(mjDSBL_DAMPER);
int issparse = mj_isSparse(m);
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
// joint-level springs
if (has_spring) {
for (int i=0; i < njnt; i++) {
mjtNum stiffness = m->jnt_stiffness[i];
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
int jnt_start = m->body_jntadr[i];
int jnt_end = jnt_start + m->body_jntnum[i];
for (int j=jnt_start; j < jnt_end; j++) {
mjtNum stiffness = m->jnt_stiffness[j];
// disabled : nothing to do
if (stiffness == 0) {
continue;
}
int padr = m->jnt_qposadr[i];
int dadr = m->jnt_dofadr[i];
int padr = m->jnt_qposadr[j];
int dadr = m->jnt_dofadr[j];
switch ((mjtJoint) m->jnt_type[i]) {
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
// apply force
d->qfrc_spring[dadr+0] = -stiffness*(d->qpos[padr+0] - m->qpos_spring[padr+0]);
@@ -111,10 +118,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
}
}
}
}
// dof-level dampers
if (has_damping) {
for (int i=0; i < m->nv; i++) {
int nv_awake = sleep_filter ? d->nv_awake : nv;
for (int j = 0; j < nv_awake; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j;
mjtNum damping = m->dof_damping[i];
if (damping != 0) {
d->qfrc_damper[i] = -damping*d->qvel[i];
@@ -419,6 +429,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// tendon-level spring-dampers
for (int i=0; i < ntendon; i++) {
// skip sleeping or static tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) {
continue;
}
mjtNum stiffness = m->tendon_stiffness[i] * has_spring;
mjtNum damping = m->tendon_damping[i] * has_damping;
@@ -466,11 +481,14 @@ static int mj_gravcomp(const mjModel* m, mjData* d) {
return 0;
}
int nbody = m->nbody, has_gravcomp = 0;
int has_gravcomp = 0;
mjtNum force[3], torque[3]={0};
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
// apply per-body gravity compensation
for (int i=1; i < nbody; i++) {
for (int b=1; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
if (m->body_gravcomp[i]) {
has_gravcomp = 1;
mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i]));
@@ -484,11 +502,17 @@ static int mj_gravcomp(const mjModel* m, mjData* d) {
// fluid forces
static int mj_fluid(const mjModel* m, mjData* d) {
int has_fluid = m->opt.viscosity > 0 || m->opt.density > 0;
// no fluid forces: early return
if (!m->opt.viscosity && !m->opt.density) {
return 0;
}
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
if (has_fluid) {
int nbody = m->nbody;
for (int i=1; i < nbody; i++) {
if (m->body_mass[i] < mjMINVAL) {
continue;
}
@@ -507,9 +531,8 @@ static int mj_fluid(const mjModel* m, mjData* d) {
mj_inertiaBoxFluidModel(m, d, i);
}
}
}
return has_fluid;
return 1;
}
@@ -597,14 +620,24 @@ int mj_contactPassive(const mjModel* m, mjData* d) {
// all passive forces
void mj_passive(const mjModel* m, mjData* d) {
int nv = m->nv;
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
int nv = sleep_filter ? d->nv_awake : m->nv;
const int* dof_awake_ind = sleep_filter ? d->dof_awake_ind : NULL;
// clear all passive force vectors
// clear passive force vectors for awake dofs
if (sleep_filter) {
mju_zeroInd(d->qfrc_spring, nv, dof_awake_ind);
mju_zeroInd(d->qfrc_damper, nv, dof_awake_ind);
mju_zeroInd(d->qfrc_gravcomp, nv, dof_awake_ind);
mju_zeroInd(d->qfrc_fluid, nv, dof_awake_ind);
mju_zeroInd(d->qfrc_passive, nv, dof_awake_ind);
} else {
mju_zero(d->qfrc_spring, nv);
mju_zero(d->qfrc_damper, nv);
mju_zero(d->qfrc_gravcomp, nv);
mju_zero(d->qfrc_fluid, nv);
mju_zero(d->qfrc_passive, nv);
}
// both spring and damping disabled: skip all passive forces
if (mjDISABLED(mjDSBL_SPRING) && mjDISABLED(mjDSBL_DAMPER)) {
@@ -624,39 +657,49 @@ void mj_passive(const mjModel* m, mjData* d) {
mj_contactPassive(m, d);
// add passive forces into qfrc_passive
if (sleep_filter) {
mju_addInd(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, dof_awake_ind, nv);
} else {
mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv);
}
if (has_fluid) {
if (sleep_filter) {
mju_addToInd(d->qfrc_passive, d->qfrc_fluid, dof_awake_ind, nv);
} else {
mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
}
}
if (has_gravcomp) {
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
// skip if gravcomp added via actuators
if (m->jnt_actgravcomp[i]) {
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
// skip if no joints
int jntnum = m->body_jntnum[i];
if (!jntnum) continue;
// skip if no gravity compensation
if (!m->body_gravcomp[i]) continue;
int start = m->body_jntadr[i];
int end = start + jntnum;
for (int j=start; j < end; j++) {
// skip if gravity compensation added via actuators
if (m->jnt_actgravcomp[j]) {
continue;
}
// get number of dofs for this joint
int dofnum;
switch (m->jnt_type[i]) {
case mjJNT_HINGE:
case mjJNT_SLIDE:
dofnum = 1;
break;
const int jnt_dofnum[4] = {6, 3, 1, 1};
int dofnum = jnt_dofnum[m->jnt_type[j]];
case mjJNT_BALL:
dofnum = 3;
break;
case mjJNT_FREE:
dofnum = 6;
break;
// add gravity compensation force
int dofadr = m->jnt_dofadr[j];
for (int k=0; k < dofnum; k++) {
d->qfrc_passive[dofadr+k] += d->qfrc_gravcomp[dofadr+k];
}
// add gravcomp force
int dofadr = m->jnt_dofadr[i];
for (int j=0; j < dofnum; j++) {
d->qfrc_passive[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
}
}
}
src/engine/engine_sensor.c
+46 -4
View File
@@ -28,6 +28,7 @@
#include "engine/engine_memory.h"
#include "engine/engine_plugin.h"
#include "engine/engine_ray.h"
#include "engine/engine_sleep.h"
#include "engine/engine_sort.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
@@ -394,10 +395,18 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
return;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
// process sensors matching stage
for (int i=0; i < nsensor; i++) {
mjtSensor type = (mjtSensor) m->sensor_type[i];
// skip sleeping sensor
if (sleep_filter && mj_sleepState(m, d, mjOBJ_SENSOR, i) == mjS_ASLEEP) {
continue;
}
// skip sensor plugins -- these are handled after builtin sensor types
if (type == mjSENS_PLUGIN) {
continue;
@@ -698,6 +707,9 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
return;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
// process sensors matching stage
int subtreeVel = 0;
for (int i=0; i < m->nsensor; i++) {
@@ -706,6 +718,11 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
continue;
}
// skip sleeping sensor
if (sleep_filter && mj_sleepState(m, d, mjOBJ_SENSOR, i) == mjS_ASLEEP) {
continue;
}
if (m->sensor_needstage[i] == mjSTAGE_VEL) {
// get sensor info
mjtSensor type = m->sensor_type[i];
@@ -883,9 +900,17 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
return;
}
// sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
// process sensors matching stage
int rnePost = 0;
for (int i=0; i < m->nsensor; i++) {
// skip sleeping sensor
if (sleep_filter && mj_sleepState(m, d, mjOBJ_SENSOR, i) == mjS_ASLEEP) {
continue;
}
// skip sensor plugins -- these are handled after builtin sensor types
if (m->sensor_type[i] == mjSENS_PLUGIN) {
continue;
@@ -1412,13 +1437,24 @@ void mj_energyPos(const mjModel* m, mjData* d) {
}
}
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < m->nbody;
// add joint-level springs
if (!mjDISABLED(mjDSBL_SPRING)) {
for (int i=0; i < m->njnt; i++) {
stiffness = m->jnt_stiffness[i];
padr = m->jnt_qposadr[i];
int nbody = m->nbody;
for (int b=1; b < nbody; b++) {
if (sleep_filter && d->body_awake[b] != mjS_AWAKE) continue;
switch ((mjtJoint) m->jnt_type[i]) {
int jnt_start = m->body_jntadr[b];
int jnt_end = jnt_start + m->body_jntnum[b];
for (int j=jnt_start; j < jnt_end; j++) {
stiffness = m->jnt_stiffness[j];
if (stiffness == 0) {
continue;
}
padr = m->jnt_qposadr[j];
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr);
d->energy[0] += 0.5 * stiffness * mju_dot3(dif, dif);
@@ -1444,10 +1480,16 @@ void mj_energyPos(const mjModel* m, mjData* d) {
}
}
}
}
// add tendon-level springs
if (!mjDISABLED(mjDSBL_SPRING)) {
for (int i=0; i < m->ntendon; i++) {
// skip sleeping or static tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) {
continue;
}
stiffness = m->tendon_stiffness[i];
mjtNum length = d->ten_length[i];
mjtNum displacement = 0;
src/engine/engine_sleep.c
+679
View File
@@ -19,7 +19,13 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_core_util.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
// uncomment to print sleep/wake events
// #define MJ_DEBUG_SLEEP
//-------------------------------- update ----------------------------------------------------------
@@ -97,3 +103,676 @@ void mj_updateSleepInit(const mjModel* m, mjData* d, int flg_staticawake) {
void mj_updateSleep(const mjModel* m, mjData* d) {
mj_updateSleepInit(m, d, /*flg_staticawake*/0);
}
//-------------------------------- utilities -------------------------------------------------------
// return 1 if the weighted infinity norm of vec is smaller than tol, 0 otherwise
static int isSmaller(const mjtNum* vec, const mjtNum* weight, int n, mjtNum tol) {
mjtNum max = 0;
for (int i=0; i < n; i++) {
max = mju_max(max, weight[i] * mju_abs(vec[i]));
if (max >= tol) {
return 0;
}
}
return 1;
}
// return 1 if tree i can sleep, 0 otherwise
static int treeCanSleep(const mjModel* m, const mjData* d, int i, mjtNum tol) {
// check sleep policy
if (m->tree_sleep_policy[i] == mjSLEEP_NEVER ||
m->tree_sleep_policy[i] == mjSLEEP_AUTO_NEVER) {
return 0;
}
// check xfrc_applied
int adr = m->tree_bodyadr[i];
int num = m->tree_bodynum[i];
if (!mju_isZeroByte((const unsigned char*)(d->xfrc_applied+6*adr), 6*num*sizeof(mjtNum))) {
return 0;
}
// check qfrc_applied
adr = m->tree_dofadr[i];
num = m->tree_dofnum[i];
if (!mju_isZeroByte((const unsigned char*)(d->qfrc_applied+adr), num*sizeof(mjtNum))) {
return 0;
}
// check qvel
if (tol) {
return isSmaller(d->qvel+adr, m->dof_length+adr, num, tol);
} else {
return mju_isZeroByte((const unsigned char*)(d->qvel+adr), num*sizeof(mjtNum));
}
}
// return the first tree in the sleep cycle that starts at i, -1 if error
int mj_sleepCycle(const int* tree_asleep, int ntree, int i) {
if (i < 0 || i >= ntree) {
return -1; // index i out of bounds
}
int smallest = i;
int current = i;
int count = 0;
do {
if (count > ntree) {
return -1; // cycle detection failed (too many steps)
}
int next = tree_asleep[current];
if (next < 0 || next >= ntree) {
return -1; // next index out of bounds
}
if (next < smallest) {
smallest = next;
}
current = next;
count++;
} while (current != i);
return smallest;
}
//-------------------------------- wake ------------------------------------------------------------
// wake tree i and its associated cycle, return number of woke trees
int mj_wakeTree(int* tree_asleep, int ntree, int i, int wakeval) {
int nwoke = 0;
// i is invalid; SHOULD NOT OCCUR
if (i < 0 || i >= ntree) {
mjERROR("invalid tree %d", i);
return nwoke;
}
// tree i already awake: set to wakeval if larger than current value
int asleep_val = tree_asleep[i];
if (asleep_val < 0) {
tree_asleep[i] = mjMIN(wakeval, asleep_val);
return nwoke;
}
// tree i asleep: wake up tree and its island cycle
else {
int current = i;
do {
// get the index of the next tree in the cycle
int next = tree_asleep[current];
// next is invalid; SHOULD NOT OCCUR
if (next < 0 || next >= ntree) {
mjERROR("invalid sleep state index %d when waking tree %d", next, i);
return 0;
}
// wake the current tree, increment count, advance to next
tree_asleep[current] = wakeval;
nwoke++;
current = next;
} while (current != i && nwoke < ntree);
// did not come back to tree i, not a cycle; SHOULD NOT OCCUR
if (current != i) {
mjERROR("tree %d is not in a cycle", i);
return 0;
}
}
return nwoke;
}
static int kAwake = -(1+mjMINAWAKE); // tree_asleep value for fully awake tree
// wake sleeping trees due to changes by user, return number of woke trees
int mj_wake(const mjModel* m, mjData* d) {
int ntree = m->ntree, nwoke = 0;
// sleep disabled
if (!mjENABLED(mjENBL_SLEEP)) {
// sleep disabled but some trees still asleep: wake all
if (d->ntree_awake < ntree) {
for (int i=0; i < ntree; i++) d->tree_asleep[i] = kAwake;
}
return ntree - d->ntree_awake;
}
// sweep over trees, wake if required
for (int i=0; i < ntree; i++) {
int asleep = d->tree_asleep[i] >= 0;
// awake: nothing to do
if (!asleep) {
continue;
}
// if qpos mismatch or cannot sleep: wake up
if (d->tree_awake[i] || !treeCanSleep(m, d, i, 0)) {
int woke = mj_wakeTree(d->tree_asleep, ntree, i, kAwake);
if (woke) {
nwoke += woke;
#ifdef MJ_DEBUG_SLEEP
printf("woke tree %d due to perturbation at t=%g\n", i, d->time);
#endif
}
}
}
return nwoke;
}
// wake sleeping trees that touch awake trees, return number of woke trees
int mj_wakeCollision(const mjModel* m, mjData* d) {
int ntree = m->ntree, ncon = d->ncon, nwoke = 0;
if (!mjENABLED(mjENBL_SLEEP)) {
return nwoke;
}
// sweep over contacts, wake trees if required
for (int i=0; i < ncon; i++) {
const mjContact* con = d->contact + i;
// only geom-geom contacts are handled
if (con->geom[0] < 0 || con->geom[1] < 0) {
continue;
}
int b1 = m->geom_bodyid[con->geom[0]];
int b2 = m->geom_bodyid[con->geom[1]];
int tree1 = m->body_treeid[b1];
int tree2 = m->body_treeid[b2];
// contact with static body, nothing to do
if (tree1 < 0 || tree2 < 0) {
continue;
}
int awake1 = d->tree_awake[tree1];
int awake2 = d->tree_awake[tree2];
// both trees awake, nothing to do
if (awake1 && awake2) {
continue;
}
// both trees asleep; SHOULD NOT OCCUR
if (!awake1 && !awake2) {
mjERROR("contact between sleeping bodies %d and %d", b1, b2);
}
// wake sleeping tree
int sleeping_tree = awake1 ? tree2 : tree1;
int wakeval = awake1 ? d->tree_asleep[tree1] : d->tree_asleep[tree2];
nwoke += mj_wakeTree(d->tree_asleep, ntree, sleeping_tree, wakeval);
#ifdef MJ_DEBUG_SLEEP
printf("woke tree %d due to contact at t=%g\n", sleeping_tree, d->time);
#endif
}
return nwoke;
}
// wake sleeping trees with a constrained tendon to a waking tree, return number of woke trees
int mj_wakeTendon(const mjModel* m, mjData* d) {
int ntendon = m->ntendon, nwoke = 0;
if (!mjENABLED(mjENBL_SLEEP)) {
return nwoke;
}
// sweep over tendons, wake trees if required
for (int i=0; i < ntendon; i++) {
if (m->tendon_treenum[i] != 2 || !tendonLimit(m, d->ten_length, i)) {
continue;
}
int tree1 = m->tendon_treeid[2*i];
int tree2 = m->tendon_treeid[2*i + 1];
int awake1 = d->tree_awake[tree1];
int awake2 = d->tree_awake[tree2];
if (awake1 != awake2) {
int sleeping_tree = awake1 ? tree2 : tree1;
int wakeval = awake1 ? d->tree_asleep[tree1] : d->tree_asleep[tree2];
nwoke += mj_wakeTree(d->tree_asleep, m->ntree, sleeping_tree, wakeval);
#ifdef MJ_DEBUG_SLEEP
printf("woke tree %d due to tendon constraint at t=%g\n", sleeping_tree, d->time);
#endif
}
}
return nwoke;
}
// wake sleeping trees with an equality to a waking tree, return number of woke trees
int mj_wakeEquality(const mjModel* m, mjData* d) {
int neq = m->neq, nwoke = 0;
if (!mjENABLED(mjENBL_SLEEP)) {
return nwoke;
}
// sweep over equalities, wake trees if required
for (int i=0; i < neq; i++) {
// skip inactive
if (!d->eq_active[i]) continue;
mjtEq eqtype = m->eq_type[i];
int id1 = m->eq_obj1id[i];
int id2 = m->eq_obj2id[i];
int tree1, tree2;
switch (eqtype) {
case mjEQ_CONNECT:
case mjEQ_WELD:
if (m->eq_objtype[i] == mjOBJ_BODY) {
tree1 = m->body_treeid[id1];
tree2 = m->body_treeid[id2];
} else {
tree1 = m->body_treeid[m->site_bodyid[id1]];
tree2 = m->body_treeid[m->site_bodyid[id2]];
}
break;
case mjEQ_JOINT:
tree1 = id1 >= 0 ? m->body_treeid[m->jnt_bodyid[id1]] : -1;
tree2 = id2 >= 0 ? m->body_treeid[m->jnt_bodyid[id2]] : -1;
break;
case mjEQ_TENDON:
mjERROR("tendon equality does not yet support sleeping");
continue;
case mjEQ_FLEX:
mjERROR("flex equality does not yet support sleeping");
continue;
default:
continue;
}
// get sleep state
mjtSleepState s1 = tree1 >= 0 ? d->tree_awake[tree1] : mjS_STATIC;
mjtSleepState s2 = tree2 >= 0 ? d->tree_awake[tree2] : mjS_STATIC;
// neither is asleep, nothing to do
if (s1 != mjS_ASLEEP && s2 != mjS_ASLEEP) {
continue;
}
// one is static, nothing to do
if (s1 == mjS_STATIC || s2 == mjS_STATIC) {
continue;
}
// equality within the same tree, nothing to do
if (tree1 == tree2) {
continue;
}
// both are asleep, wake if in different islands
if (s1 == mjS_ASLEEP && s2 == mjS_ASLEEP) {
int cycle1 = mj_sleepCycle(d->tree_asleep, m->ntree, tree1);
int cycle2 = mj_sleepCycle(d->tree_asleep, m->ntree, tree2);
if (cycle1 != cycle2) {
int nwoke1 = mj_wakeTree(d->tree_asleep, m->ntree, tree1, kAwake);
int nwoke2 = mj_wakeTree(d->tree_asleep, m->ntree, tree2, kAwake);
#ifdef MJ_DEBUG_SLEEP
printf("woke trees %d, %d due to equality %d at t=%g\n", tree1, tree2, i, d->time);
#endif
nwoke += nwoke1 + nwoke2;
}
continue;
}
// one is asleep and one is awake, wake the sleeping tree
int sleeping_tree = s1 == mjS_ASLEEP ? tree1 : tree2;
nwoke += mj_wakeTree(d->tree_asleep, m->ntree, sleeping_tree, kAwake);
#ifdef MJ_DEBUG_SLEEP
printf("woke tree %d due to equality %d at t=%g\n", sleeping_tree, i, d->time);
#endif
}
return nwoke;
}
//-------------------------------- sleep -----------------------------------------------------------
// put n trees to sleep (create cycle), set their velocity and acceleration to zero
static inline void sleepTrees(const mjModel* m, mjData* d, const int* tree, int n) {
for (int i=0; i < n; i++) {
// create cycle
int current = tree[i];
int next = (i == n - 1) ? tree[0] : tree[i + 1];
if (d->tree_asleep[current] == -1) {
d->tree_asleep[current] = next;
}
// SHOULD NOT OCCUR
else if (d->tree_asleep[current] >= 0) {
mjERROR("trying to sleep tree %d which is already asleep", i);
} else {
mjERROR("trying to sleep tree %d which is not ready to sleep", i);
}
// set tree velocity and acceleration to zero
int adr = m->tree_dofadr[current];
int num = m->tree_dofnum[current];
mju_zero(d->qvel+adr, num);
mju_zero(d->qacc+adr, num);
}
#ifdef MJ_DEBUG_SLEEP
if (n == 1) {
printf("tree %d put to sleep at t=%g\n", tree[0], d->time);
} else if (n > 1) {
printf("trees ");
for (int i = 0; i < n; i++) {
printf("%d%s", tree[i], (i == n - 1) ? "" : ", ");
}
printf(" put to sleep at t=%g\n", d->time);
}
#endif
}
// put trees to sleep according to tolerance, return number of slept trees
int mj_sleep(const mjModel* m, mjData* d) {
int ntree = m->ntree, nisland = d->nisland, nslept = 0;
// sleep disabled: nothing to do
if (!mjENABLED(mjENBL_SLEEP)) {
return nslept;
}
// have constraints but no island structure: can't sleep
if (d->nefc && !nisland) {
return nslept;
}
// sweep over awake trees, increment tree_asleep if under tolerance
for (int i=0; i < ntree; i++) {
// skip sleeping tree
if (d->tree_asleep[i] >= 0) {
continue;
}
// increment tree_asleep if tree can sleep, otherwise wake up
if (treeCanSleep(m, d, i, m->opt.sleep_tolerance)) {
d->tree_asleep[i] += (d->tree_asleep[i] < -1);
} else {
d->tree_asleep[i] = -(1+mjMINAWAKE);
}
}
// sweep over islands, put to sleep if all trees are under tolerance
for (int i=0; i < nisland; i++) {
// check if all trees in the island can sleep
int can_sleep = 1;
int start = d->island_itreeadr[i];
int end = start + d->island_ntree[i];
for (int j=start; j < end; j++) {
int tree_asleep = d->tree_asleep[d->map_itree2tree[j]];
if (tree_asleep < -1) {
can_sleep = 0;
break;
}
// sleeping tree in an island; SHOULD NOT OCCUR
else if (tree_asleep >= 0) {
mjERROR("found sleeping tree %d in island %d", d->map_itree2tree[j], i);
}
}
// put island to sleep
if (can_sleep) {
const int* tree = d->map_itree2tree + start;
int n = d->island_ntree[i];
sleepTrees(m, d, tree, n);
nslept += n;
}
}
// sleep unconstrained trees (with or without island structure)
int start = nisland ? d->island_itreeadr[nisland-1] + d->island_ntree[nisland-1] : 0;
for (int j=start; j < ntree; j++) {
int i = nisland ? d->map_itree2tree[j] : j;
if (d->tree_asleep[i] == -1) {
sleepTrees(m, d, &i, 1);
nslept++;
}
}
return nslept;
}
//-------------------------------- sleep state -----------------------------------------------------
// return sleep state of tendon i
static mjtSleepState mj_tendonSleepState(const mjModel* m, const mjData* d, int i) {
int treenum = m->tendon_treenum[i];
// no trees: tendon is static
if (treenum == 0) {
return mjS_STATIC;
}
// single tree: awake if tree is awake, asleep otherwise
int id1 = m->tendon_treeid[2*i];
if (treenum == 1) {
return d->tree_awake[id1] ? mjS_AWAKE : mjS_ASLEEP;
}
// two trees: asleep only if both are asleep
int id2 = m->tendon_treeid[2*i+1];
if (treenum == 2) {
return (d->tree_awake[id1] || d->tree_awake[id2]) ? mjS_AWAKE : mjS_ASLEEP;
}
return mjS_AWAKE;
}
// return sleep state of actuator i
static mjtSleepState mj_actuatorSleepState(const mjModel* m, const mjData* d, int i) {
mjtSleepState s1, s2;
int trnid = m->actuator_trnid[i*2];
switch ((mjtTrn)m->actuator_trntype[i]) {
case mjTRN_JOINT:
case mjTRN_JOINTINPARENT:
return mj_sleepState(m, d, mjOBJ_JOINT, trnid);
case mjTRN_SLIDERCRANK:
s1 = mj_sleepState(m, d, mjOBJ_SITE, trnid);
s2 = mj_sleepState(m, d, mjOBJ_SITE, m->actuator_trnid[i*2+1]);
return (s1 == mjS_AWAKE || s2 == mjS_AWAKE) ? mjS_AWAKE : mjS_ASLEEP;
case mjTRN_TENDON:
return mj_tendonSleepState(m, d, trnid);
case mjTRN_SITE:
return mj_sleepState(m, d, mjOBJ_SITE, trnid);
case mjTRN_BODY:
return mj_sleepState(m, d, mjOBJ_BODY, trnid);
case mjTRN_UNDEFINED:
return mjS_AWAKE;
}
return mjS_AWAKE;
}
// return sleep state of equality i
static mjtSleepState mj_equalitySleepState(const mjModel* m, const mjData* d, int i) {
mjtEq eqtype = m->eq_type[i];
mjtObj objtype;
switch (eqtype) {
case mjEQ_CONNECT:
case mjEQ_WELD:
objtype = m->eq_objtype[i];
break;
case mjEQ_JOINT:
objtype = mjOBJ_JOINT;
break;
case mjEQ_TENDON:
objtype = mjOBJ_TENDON;
break;
case mjEQ_FLEX:
objtype = mjOBJ_FLEX;
break;
default:
return mjS_AWAKE;
}
int id1 = m->eq_obj1id[i];
int id2 = m->eq_obj2id[i];
mjtSleepState s1 = (id1 >= 0) ? mj_sleepState(m, d, objtype, id1) : mjS_STATIC;
mjtSleepState s2 = (id2 >= 0) ? mj_sleepState(m, d, objtype, id2) : mjS_STATIC;
// return ASLEEP if both objects are asleep or static, AWAKE otherwise
int neither_awake = (s1 != mjS_AWAKE && s2 != mjS_AWAKE);
return neither_awake ? mjS_ASLEEP : mjS_AWAKE;
}
// return sleep state of sensor i
static mjtSleepState mj_sensorSleepState(const mjModel* m, const mjData* d, int i) {
mjtSensor type = m->sensor_type[i];
mjtObj objtype = m->sensor_objtype[i];
int objid = m->sensor_objid[i];
mjtObj reftype = m->sensor_reftype[i];
int refid = m->sensor_refid[i];
// get sleep state of the primary and reference objects
mjtSleepState s_obj = mj_sleepState(m, d, objtype, objid);
mjtSleepState s_ref = mj_sleepState(m, d, reftype, refid);
// special handling for specific sensor types
switch (type) {
// USER and PLUGIN sensors are always awake
case mjSENS_USER:
case mjSENS_PLUGIN:
return mjS_AWAKE;
// sensors that use sites to define a volume are always awake
case mjSENS_INSIDESITE:
case mjSENS_TOUCH:
return mjS_AWAKE;
// contact sensors
case mjSENS_CONTACT:
// site used to define a volume: always awake
if (objtype == mjOBJ_SITE || reftype == mjOBJ_SITE) {
return mjS_AWAKE;
}
// for contact sensors UNKNOWN means undefined, so the AWAKE returned by mj_sleepState is wrong
// if both are UNKNOWN (all contacts), return ASLEEP iff everything is alseep
if (objtype == mjOBJ_UNKNOWN && reftype == mjOBJ_UNKNOWN) {
return d->ntree_awake == 0 ? mjS_ASLEEP : mjS_AWAKE;
}
// if only one is UNKNOWN, return state of other object
if (objtype == mjOBJ_UNKNOWN) {
return s_ref;
} else if (reftype == mjOBJ_UNKNOWN) {
return s_obj;
}
break;
// sensors whose value depends on objects other than the two they are attached to are always awake
case mjSENS_RANGEFINDER:
return mjS_AWAKE;
default:
break;
}
// if either object is awake, return AWAKE
if (s_obj == mjS_AWAKE || s_ref == mjS_AWAKE) {
return mjS_AWAKE;
}
// otherwise return ASLEEP
return mjS_ASLEEP;
}
// return sleep state of object i
mjtSleepState mj_sleepState(const mjModel* m, const mjData* d, mjtObj type, int i) {
const char* typename;
switch (type) {
// simple types
case mjOBJ_BODY:
case mjOBJ_XBODY:
return (mjtSleepState) d->body_awake[i];
case mjOBJ_JOINT:
return (mjtSleepState) d->body_awake[m->jnt_bodyid[i]];
case mjOBJ_SITE:
return (mjtSleepState) d->body_awake[m->site_bodyid[i]];
case mjOBJ_DOF:
return (mjtSleepState) d->body_awake[m->dof_bodyid[i]];
case mjOBJ_GEOM:
return (mjtSleepState) d->body_awake[m->geom_bodyid[i]];
case mjOBJ_CAMERA:
return (mjtSleepState) d->body_awake[m->cam_bodyid[i]];
case mjOBJ_LIGHT:
return (mjtSleepState) d->body_awake[m->light_bodyid[i]];
// complex types
case mjOBJ_EQUALITY:
return mj_equalitySleepState(m, d, i);
case mjOBJ_TENDON:
return mj_tendonSleepState(m, d, i);
case mjOBJ_ACTUATOR:
return mj_actuatorSleepState(m, d, i);
case mjOBJ_SENSOR:
return mj_sensorSleepState(m, d, i);
// always awake
case mjOBJ_FLEX:
case mjOBJ_UNKNOWN:
return mjS_AWAKE;
// unsupported
default:
typename = mju_type2Str(type);
if (typename) {
mjERROR("unsupported object type '%s'", typename);
} else {
mjERROR("unsupported object type %d", type);
}
return mjS_AWAKE;
}
}
#ifdef MJ_DEBUG_SLEEP
#undef MJ_DEBUG_SLEEP
#endif
src/engine/engine_sleep.h
+28 -1
View File
@@ -24,11 +24,38 @@ extern "C" {
#endif
// compute sleeping arrays from tree_asleep, if flg_staticawake is set, treat static bodies as awake
MJAPI void mj_updateSleepInit(const mjModel* m, mjData* d, int flg_staticawake);
void mj_updateSleepInit(const mjModel* m, mjData* d, int flg_staticawake);
// compute {ntree,nbody,nv}_awake, {tree,body}_awake, {body,dof}_awake_ind from tree_asleep
MJAPI void mj_updateSleep(const mjModel* m, mjData* d);
// return the first tree in the sleep cycle that starts at i, -1 if error
int mj_sleepCycle(const int* tree_asleep, int ntree, int i);
// return the first tree in the sleep cycle that starts at i, -1 if error
int mj_sleepCycle(const int* tree_asleep, int ntree, int i);
// wake tree i and its related island cycle, return number of woke trees
MJAPI int mj_wakeTree(int* tree_asleep, int ntree, int i, int wakeval);
// wake trees with nonzero velocity or external forces, return number of woke trees
int mj_wake(const mjModel* m, mjData* d);
// wake sleeping trees that touch awake trees, return number of woke trees
int mj_wakeCollision(const mjModel* m, mjData* d);
// wake sleeping trees with a constrained tendon to a waking tree, return number of woke trees
int mj_wakeTendon(const mjModel* m, mjData* d);
// wake sleeping trees with an equality to a waking tree, return number of woke trees
int mj_wakeEquality(const mjModel* m, mjData* d);
// put trees to sleep according to tolerance, return number of slept trees
int mj_sleep(const mjModel* m, mjData* d);
// return sleep state of object i
mjtSleepState mj_sleepState(const mjModel* m, const mjData* d, mjtObj type, int i);
#ifdef __cplusplus
}
#endif
src/engine/engine_solver.c
+1 -1
View File
@@ -1063,7 +1063,7 @@ static void CGupdateGradient(mjCGContext* ctx, int flg_Newton) {
else {
mju_copy(ctx->Mgrad, ctx->grad, nv);
mj_solveLD(ctx->Mgrad, ctx->qLD, ctx->qLDiagInv, nv, 1,
ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind);
ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, NULL);
}
}
src/engine/engine_support.c
+15 -4
View File
@@ -561,10 +561,14 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
}
// integrate qpos with given qvel
void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt) {
// loop over joints
for (int j=0; j < m->njnt; j++) {
// integrate qpos with given qvel for given body indices
void mj_integratePosInd(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt,
const int* index, int nbody) {
for (int b=1; b < nbody; b++) {
int k = index ? index[b] : b;
int start = m->body_jntadr[k];
int end = start + m->body_jntnum[k];
for (int j=start; j < end; j++) {
// get addresses in qpos and qvel
int padr = m->jnt_qposadr[j];
int vadr = m->jnt_dofadr[j];
@@ -593,6 +597,13 @@ void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum
}
}
}
}
// integrate qpos with given qvel
void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt) {
mj_integratePosInd(m, qpos, qvel, dt, NULL, m->nbody);
}
// normalize all quaternions in qpos-type vector
src/engine/engine_support.h
+4
View File
@@ -89,6 +89,10 @@ MJAPI mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int g
MJAPI void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
const mjtNum* qpos1, const mjtNum* qpos2);
// integrate qpos with given qvel for given body indices
MJAPI void mj_integratePosInd(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt,
const int* index, int nbody);
// integrate position with given velocity
MJAPI void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt);
src/engine/engine_util_blas.c
+77
View File
@@ -273,6 +273,14 @@ void mju_zero(mjtNum* res, int n) {
}
// res = 0, at given indices
void mju_zeroInd(mjtNum* res, int n, const int* ind) {
for (int i = 0; i < n; i++) {
res[ind[i]] = 0;
}
}
// res = val
void mju_fill(mjtNum* res, mjtNum val, int n) {
for (int i=0; i < n; i++) {
@@ -287,6 +295,14 @@ void mju_copy(mjtNum* res, const mjtNum* vec, int n) {
}
// res = vec, at given indices
void mju_copyInd(mjtNum* res, const mjtNum* vec, const int* ind, int n) {
for (int i = 0; i < n; i++) {
res[ind[i]] = vec[ind[i]];
}
}
// sum(vec)
mjtNum mju_sum(const mjtNum* vec, int n) {
mjtNum res = 0;
@@ -397,6 +413,15 @@ void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) {
}
// res = vec1 + vec2, at selected indices
void mju_addInd(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, const int* ind, int n) {
for (int i = 0; i < n; i++) {
int j = ind[i];
res[j] = vec1[j] + vec2[j];
}
}
// res = vec1 - vec2
void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) {
int i = 0;
@@ -439,6 +464,15 @@ void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) {
}
// res = vec1 - vec2, at selected indices
void mju_subInd(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, const int* ind, int n) {
for (int i = 0; i < n; i++) {
int j = ind[i];
res[j] = vec1[j] - vec2[j];
}
}
// res += vec
void mju_addTo(mjtNum* res, const mjtNum* vec, int n) {
int i = 0;
@@ -481,6 +515,15 @@ void mju_addTo(mjtNum* res, const mjtNum* vec, int n) {
}
// res += vec, at selected indices
void mju_addToInd(mjtNum* res, const mjtNum* vec, const int* ind, int n) {
for (int i = 0; i < n; i++) {
int j = ind[i];
res[j] += vec[j];
}
}
// res -= vec
void mju_subFrom(mjtNum* res, const mjtNum* vec, int n) {
int i = 0;
@@ -568,6 +611,18 @@ void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) {
#endif
}
// res += vec*scl, at given indices
void mju_addToSclInd(mjtNum* res, const mjtNum* vec, const int* ind, mjtNum scl, int n) {
for (int i=0; i < n; i++) {
int k = ind[i];
res[k] += vec[k]*scl;
}
}
// res = vec1 + vec2*scl
void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, int n) {
int i = 0;
@@ -706,6 +761,20 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) {
return res;
}
// vector dot-product, at given indices
mjtNum mju_dotInd(const mjtNum* vec1, const mjtNum* vec2, const int* ind, int n) {
mjtNum res = 0;
for (int i = 0; i < n; i++) {
int k = ind[i];
res += vec1[k] * vec2[k];
}
return res;
}
//------------------------------ matrix-vector operations ------------------------------------------
// multiply matrix and vector
@@ -771,6 +840,14 @@ void mju_eye(mjtNum* mat, int n) {
}
// res[ind, :] = mat[ind, :]
void mju_copyRows(mjtNum* res, const mjtNum* mat, const int* ind, int n, int nc) {
for (int i = 0; i < n; i++) {
mju_copy(res + nc*ind[i], mat + nc*ind[i], nc);
}
}
//------------------------------ matrix-matrix operations ------------------------------------------
// multiply matrices, exploit sparsity of mat1
src/engine/engine_util_blas.h
+25 -4
View File
@@ -141,12 +141,18 @@ MJAPI mjtNum mju_normalize4(mjtNum vec[4]);
// res = 0
MJAPI void mju_zero(mjtNum* res, int n);
// res = 0, at given indices
void mju_zeroInd(mjtNum* res, int n, const int* ind);
// res = val
MJAPI void mju_fill(mjtNum* res, mjtNum val, int n);
// res = vec
MJAPI void mju_copy(mjtNum* res, const mjtNum* vec, int n);
// res = vec, at given indices
void mju_copyInd(mjtNum* res, const mjtNum* vec, const int* ind, int n);
// sum(vec)
MJAPI mjtNum mju_sum(const mjtNum* vec, int n);
@@ -159,18 +165,30 @@ MJAPI void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n);
// res = vec1 + vec2
MJAPI void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n);
// res = vec1 + vec2, at given indices
void mju_addInd(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, const int* ind, int n);
// res = vec1 - vec2
MJAPI void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n);
// res = vec1 - vec2, at selected indices
void mju_subInd(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, const int* ind, int n);
// res += vec
MJAPI void mju_addTo(mjtNum* res, const mjtNum* vec, int n);
// res += vec, at selected indices
void mju_addToInd(mjtNum* res, const mjtNum* vec, const int* ind, int n);
// res -= vec
MJAPI void mju_subFrom(mjtNum* res, const mjtNum* vec, int n);
// res += vec*scl
MJAPI void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n);
// res += vec*scl, at given indices
void mju_addToSclInd(mjtNum* res, const mjtNum* vec, const int* ind, mjtNum scl, int n);
// res = vec1 + vec2*scl
MJAPI void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, int n);
@@ -183,16 +201,16 @@ MJAPI mjtNum mju_norm(const mjtNum* res, int n);
// vector dot-product
MJAPI mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n);
// vector dot-product, at given indices
mjtNum mju_dotInd(const mjtNum* vec1, const mjtNum* vec2, const int* ind, int n);
//------------------------------ matrix-vector operations ------------------------------------------
// multiply matrix and vector
MJAPI void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
int nr, int nc);
MJAPI void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc);
// multiply transposed matrix and vector
MJAPI void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
int nr, int nc);
MJAPI void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc);
// multiply square matrix with vectors on both sides: return vec1'*mat*vec2
MJAPI mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec2, int n);
@@ -209,6 +227,9 @@ MJAPI void mju_symmetrize(mjtNum* res, const mjtNum* mat, int n);
// identity matrix
MJAPI void mju_eye(mjtNum* mat, int n);
// copy selected rows: res[ind, :] = mat[ind, :]
void mju_copyRows(mjtNum* res, const mjtNum* mat, const int* ind, int n, int nc);
//------------------------------ matrix-matrix operations ------------------------------------------
// multiply matrices
src/engine/engine_util_solve.c
+31 -14
View File
@@ -612,17 +612,26 @@ void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
// sparse reverse-order LU factorization, no fill-in (assuming tree topology)
// result: LU = L + U; original = (U+I) * L; scratch size is n
void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
const int* rownnz, const int* rowadr, const int* colind) {
const int* rownnz, const int* rowadr, const int* colind,
const int* index) {
int* remaining = scratch;
// set remaining = rownnz
if (index) {
for (int i=0; i < n; i++) {
remaining[i] = rownnz[index[i]];
}
} else {
mju_copyInt(remaining, rownnz, n);
}
// diagonal elements (i,i)
for (int i=n-1; i >= 0; i--) {
for (int r=n-1; r >= 0; r--) {
int i = index ? index[r] : r;
// get address of last remaining element of row i, adjust remaining counter
int ii = rowadr[i] + remaining[i] - 1;
remaining[i]--;
int ii = rowadr[i] + remaining[r] - 1;
remaining[r]--;
// make sure ii is on diagonal
if (colind[ii] != i) {
@@ -635,14 +644,16 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
}
// rows j above i
for (int j=i-1; j >= 0; j--) {
for (int c=r-1; c >= 0; c--) {
int j = index ? index[c] : c;
// get address of last remaining element of row j
int ji = rowadr[j] + remaining[j] - 1;
int ji = rowadr[j] + remaining[c] - 1;
// process row j if (j,i) is non-zero
if (colind[ji] == i) {
// adjust remaining counter
remaining[j]--;
remaining[c]--;
// (j,i) = (j,i) / (i,i)
LU[ji] = LU[ji] / LU[ii];
@@ -650,7 +661,7 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
// (j,k) = (j,k) - (i,k) * (j,i) for k<i; handle incompatible sparsity
int icnt = rowadr[i], jcnt = rowadr[j];
while (jcnt < rowadr[j]+remaining[j]) {
while (jcnt < rowadr[j]+remaining[c]) {
// both non-zero
if (colind[icnt] == colind[jcnt]) {
// update LU, advance counters
@@ -670,7 +681,7 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
}
// make sure both rows fully processed
if (icnt != rowadr[i]+remaining[i] || jcnt != rowadr[j]+remaining[j]) {
if (icnt != rowadr[i]+remaining[r] || jcnt != rowadr[j]+remaining[c]) {
mjERROR("row processing incomplete");
}
}
@@ -678,8 +689,9 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
}
// make sure remaining points to diagonal
for (int i=0; i < n; i++) {
if (remaining[i] < 0 || colind[rowadr[i]+remaining[i]] != i) {
for (int r=0; r < n; r++) {
int i = index ? index[r] : r;
if (remaining[r] < 0 || colind[rowadr[i]+remaining[r]] != i) {
mjERROR("unexpected sparse matrix structure");
}
}
@@ -688,9 +700,12 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
// solve mat*res=vec given LU factorization of mat
void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
const int* rownnz, const int* rowadr, const int* diag, const int* colind) {
const int* rownnz, const int* rowadr, const int* diag, const int* colind,
const int* index) {
// solve (U+I)*res = vec
for (int i=n-1; i >= 0; i--) {
for (int k=n-1; k >= 0; k--) {
int i = index ? index[k] : k;
// init: diagonal of (U+I) is 1
res[i] = vec[i];
@@ -703,7 +718,9 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
}
//------------------ solve L*res(new) = res
for (int i=0; i < n; i++) {
for (int k=0; k < n; k++) {
int i = index ? index[k] : k;
// res[i] -= sum_k<i res[k]*LU(i,k)
int d = diag[i];
int adr = rowadr[i];
src/engine/engine_util_solve.h
+5 -4
View File
@@ -78,14 +78,15 @@ MJAPI void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
// address of diagonal element i in band-dense matrix representation
MJAPI int mju_bandDiag(int i, int ntotal, int nband, int ndense);
// sparse reverse-order LU factorization, no fill-in (assuming tree topology)
// sparse reverse-order LU factorization, assume tree topology (only dofs in index, if given)
// LU = L + U; original = (U+I) * L; scratch is size n
void mju_factorLUSparse(mjtNum *LU, int n, int* scratch,
const int *rownnz, const int *rowadr, const int *colind);
const int *rownnz, const int *rowadr, const int *colind, const int *index);
// solve mat*res=vec given LU factorization of mat
// solve mat*res=vec given LU factorization of mat (only dofs in index, if given)
void mju_solveLUSparse(mjtNum *res, const mjtNum *LU, const mjtNum* vec, int n,
const int *rownnz, const int *rowadr, const int* diag, const int *colind);
const int *rownnz, const int *rowadr, const int* diag, const int *colind,
const int *index);
// eigenvalue decomposition of symmetric 3x3 matrix
MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]);
src/engine/engine_util_sparse.c
+19
View File
@@ -142,6 +142,25 @@ void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc,
}
// res[row, :] = mat[row, :]
void mju_copySparse(mjtNum* res, const mjtNum* mat, const int* rownnz, const int* rowadr,
const int* row, int nrow) {
for (int i=0; i < nrow; i++) {
int r = row[i];
mju_copy(res + rowadr[r], mat + rowadr[r], rownnz[r]);
}
}
// res[row, :] = 0
void mju_zeroSparse(mjtNum* res, const int* rownnz, const int* rowadr, const int* row, int nrow) {
for (int i=0; i < nrow; i++) {
int r = row[i];
mju_zero(res + rowadr[r], rownnz[r]);
}
}
// multiply sparse matrix and dense vector: res = mat * vec.
void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
int nr, const int* rownnz, const int* rowadr,
src/engine/engine_util_sparse.h
+7
View File
@@ -41,6 +41,13 @@ MJAPI int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, const int* rownnz,
const int* rowadr, const int* colind);
// res[row, :] = mat[row, :]
void mju_copySparse(mjtNum* res, const mjtNum* mat, const int* rownnz, const int* rowadr,
const int* row, int nrow);
// res[row, :] = 0
void mju_zeroSparse(mjtNum* res, const int* rownnz, const int* rowadr, const int* row, int nrow);
// multiply sparse matrix and dense vector: res = mat * vec
MJAPI void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
int nr, const int* rownnz, const int* rowadr,
src/engine/engine_vis_visualize.c
+44 -18
View File
@@ -28,6 +28,7 @@
#include "engine/engine_memory.h"
#include "engine/engine_name.h"
#include "engine/engine_plugin.h"
#include "engine/engine_sleep.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
@@ -72,7 +73,7 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) {
// convert HSV to RGB
static void hsv2rgb(float *RGB, float H, float S, float V) {
void hsv2rgb(float *RGB, float H, float S, float V) {
float R, G, B;
if (S <= 0) {
@@ -105,14 +106,32 @@ static void hsv2rgb(float *RGB, float H, float S, float V) {
}
static const float kIslandSaturation = 0.8;
static const float kIslandValue = 0.7;
// assign pseudo-random rgba to constraint island using Halton sequence
static void islandColor(float rgba[4], int h) {
float hue = mju_Halton(h + 1, 2);
float saturation = h >= 0 ? kIslandSaturation : 0;
hsv2rgb(rgba, hue, saturation, kIslandValue);
static void islandColor(float rgba[4], int h, int awake) {
// default to gray R = G = B = 0.7;
float hue = 1.0f;
float saturation = 0.0f;
float value = 0.7f;
// island index given, use Halton sequence to generate pseudo-random color
if (h >= 0) {
// hue in [0, 1]
hue = mju_Halton(h + 1, 7);
// saturation in [0.5, 1.0]
saturation = .5 + .5*mju_Halton(h + 1, 3);
// value in [0.6, 1.0]
value = .6 + .4*mju_Halton(h + 1, 5);
}
// if asleep, decrease saturation and value
if (!awake) {
value *= 0.6;
saturation *= 0.7;
}
hsv2rgb(rgba, hue, saturation, value);
rgba[3] = 1;
}
@@ -572,7 +591,7 @@ static void addContactGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_ISLAND] && efc_adr >= 0) {
// set hue using island's first dof
int h = d->nisland > 0 ? d->island_dofadr[d->efc_island[efc_adr]] : -1;
islandColor(thisgeom->rgba, h);
islandColor(thisgeom->rgba, h, /*awake*/1);
}
// otherwise regular colors (different for included and excluded contacts)
@@ -593,8 +612,7 @@ static void addContactGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const char* geomname = mj_id2name(m, mjOBJ_GEOM, con->geom[k]);
if (geomname) {
mjSNPRINTF(contactlabel[k], "%s", geomname);
}
else {
} else {
mjSNPRINTF(contactlabel[k], "g%d", con->geom[k]);
}
}
@@ -605,16 +623,14 @@ static void addContactGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (flexname) {
if (con->elem[k] >= 0) {
mjSNPRINTF(contactlabel[k], "%s.e%d", flexname, con->elem[k]);
}
else {
} else {
mjSNPRINTF(contactlabel[k], "%s.v%d", flexname, con->vert[k]);
}
}
else {
if (con->elem[k] >= 0) {
mjSNPRINTF(contactlabel[k], "f%d.e%d", con->flex[k], con->elem[k]);
}
else {
} else {
mjSNPRINTF(contactlabel[k], "f%d.v%d", con->flex[k], con->vert[k]);
}
}
@@ -865,9 +881,19 @@ static void addGeomGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
thisgeom->matid = -1;
// set hue using first island dof, -1 if no island
int island = d->nisland ? d->dof_island[m->body_dofadr[weld_id]] : -1;
int dof = m->body_dofadr[weld_id];
int island = d->nisland ? d->dof_island[dof] : -1;
int h = island >= 0 ? d->island_dofadr[island] : -1;
islandColor(thisgeom->rgba, h);
int awake = d->body_awake[m->geom_bodyid[i]];
// if sleep is enabled, color by first tree dof
if (h == -1 && mjENABLED(mjENBL_SLEEP)) {
int tree = m->dof_treeid[dof];
if (!awake) tree = mj_sleepCycle(d->tree_asleep, m->ntree, tree);
h = m->tree_dofadr[tree];
}
islandColor(thisgeom->rgba, h, awake);
}
}
@@ -1107,7 +1133,7 @@ static void addSpatialTendonGeoms(const mjModel* m, mjData* d, const mjvOption*
if (d->nisland && d->tendon_efcadr[i] >= 0) {
h = d->island_dofadr[d->efc_island[d->tendon_efcadr[i]]];
}
islandColor(thisgeom->rgba, h);
islandColor(thisgeom->rgba, h, 1);
}
// vopt->label: only the first segment
src/engine/engine_vis_visualize.h
+3
View File
@@ -70,6 +70,9 @@ void mjv_cameraFrustum(float zver[2], float zhor[2], float zclip[2], const mjMo
int mjv_catenary(const mjtNum x0[3], const mjtNum x1[3], const mjtNum gravity[3], mjtNum length,
mjtNum* catenary, int ncatenary);
// convert HSV to RGB
void hsv2rgb(float *RGB, float H, float S, float V);
#ifdef __cplusplus
}
#endif
src/experimental/studio/app.cc
+7 -2
View File
@@ -641,10 +641,14 @@ void App::UpdateProfilerData() {
sqrt_nnz = mju_sqrt(sqrt_nnz);
dim_dof_.erase(dim_dof_.begin());
dim_dof_.push_back(Model()->nv);
int nv = (Model()->opt.enableflags & mjENBL_SLEEP) ? Data()->nv_awake
: Model()->nv;
dim_dof_.push_back(nv);
dim_body_.erase(dim_body_.begin());
dim_body_.push_back(Model()->nbody);
int nbody = (Model()->opt.enableflags & mjENBL_SLEEP) ? Data()->nbody_awake
: Model()->nbody;
dim_body_.push_back(nbody);
dim_constraint_.erase(dim_constraint_.begin());
dim_constraint_.push_back(Data()->nefc);
@@ -1719,6 +1723,7 @@ void App::PhysicsGui() {
ImGui_Input("Noslip Tol", &opt.noslip_tolerance, {0, 1, 0.01, 0.1, w});
ImGui_Input("CCD Iter", &opt.ccd_iterations, {0, 1000, 1, 100, w});
ImGui_Input("CCD Tol", &opt.ccd_tolerance, {0, 1, 0.01, 0.1, w});
ImGui_Input("Sleep Tol", &opt.sleep_tolerance, {0, 1, 0.01, 0.1, w});
ImGui_Input("SDF Iter", &opt.sdf_iterations, {1, 20, 1, 10, w});
ImGui_Input("SDF Init", &opt.sdf_initpoints, {1, 100, 1, 10, w});
ImGui::TreePop();
src/user/user_model.cc
+25 -2
View File
@@ -4935,6 +4935,8 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// create data
int disableflags = m->opt.disableflags;
m->opt.disableflags |= mjDSBL_CONTACT;
int enableflags = m->opt.enableflags;
m->opt.enableflags &= ~mjENBL_SLEEP;
mj_makeRawData(&d, m);
if (!d) {
// m will be deleted by the catch statement in mjCModel::Compile()
@@ -4980,18 +4982,39 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// delete partial mjData (no plugins), make a complete one
mj_deleteData(d);
d = nullptr;
// if sleep was enabled, check for trees initialized as sleeping
bool asleep_init = false;
if (enableflags & mjENBL_SLEEP) {
for (int i=0; i < m->ntree; i++) {
if (m->tree_sleep_policy[i] == mjSLEEP_INIT) {
asleep_init = true;
break;
}
}
}
// if any trees initialized as sleeping, restore flags before mj_makeData
if (asleep_init) {
m->opt.disableflags = disableflags;
m->opt.enableflags = enableflags;
}
d = mj_makeData(m);
if (!d) {
// m will be deleted by the catch statement in mjCModel::Compile()
throw mjCError(0, "could not create mjData");
}
// test forward simulation
// test forward simulation unless asleep_init is true (potentially expensive)
if (!asleep_init) {
mj_step(m, d);
}
// delete data
// delete data, restore flags
mj_deleteData(d);
m->opt.disableflags = disableflags;
m->opt.enableflags = enableflags;
d = nullptr;
// pass warning back
test/benchmark/factorI_benchmark_test.cc
+1 -1
View File
@@ -59,7 +59,7 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
} else {
mju_copy(d->qLD, M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
}
}
}
test/benchmark/inertia_benchmark_test.cc
+2 -2
View File
@@ -73,9 +73,9 @@ static void BM_solve(benchmark::State& state, SolveType type) {
case SolveType::kCsr:
mju_copy(d->qLD, M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
}
}
}
test/benchmark/solveLD_benchmark_test.cc
+1 -1
View File
@@ -64,7 +64,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv);
} else {
mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
}
}
}
test/engine/engine_core_smooth_test.cc
+4 -4
View File
@@ -742,7 +742,7 @@ TEST_F(CoreSmoothTest, SolveLDs) {
mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
// expect vectors to match up to floating point precision
for (int i=0; i < nv; i++) {
@@ -777,7 +777,7 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
// expect vectors to match up to floating point precision
for (int i=0; i < nv*n; i++) {
@@ -815,7 +815,7 @@ TEST_F(CoreSmoothTest, SolveM2) {
mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
// expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v)
for (int i=0; i < n; i++) {
@@ -854,7 +854,7 @@ TEST_F(CoreSmoothTest, FactorIs) {
vector<mjtNum> qLDiagInv(nv, 0);
mj_factorI(qLD.data(), qLDiagInv.data(), nv,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
// expect outputs to match to floating point precision
EXPECT_THAT(qLD, Pointwise(DoubleNear(1e-12), qLDexpected));
test/engine/engine_derivative_test.cc
+2 -2
View File
@@ -438,7 +438,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
Ac[nv*nv + i*nv + i] = -m->dof_damping[i];
}
mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2*nv,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
// A = [dt*Ac; Ac]
mju_transpose(A, Ac, 2*nv, nv);
@@ -466,7 +466,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz,
d->moment_rowadr, d->moment_colind);
mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu,
m->M_rownnz, m->M_rowadr, m->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
mju_transpose(BcT, Bc, nu, nv);
mju_scl(B, BcT, dt*dt, nu*nv);
mju_scl(B+nu*nv, BcT, dt, nu*nv);
test/engine/engine_sleep_test.cc
+349
View File
@@ -14,8 +14,11 @@
// Tests for engine/engine_sleep.c.
#include <string>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <gtest/gtest-spi.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_sleep.h"
@@ -25,7 +28,10 @@ namespace mujoco {
namespace {
using ::testing::ElementsAre;
using ::testing::IsNull;
using ::testing::HasSubstr;
using ::testing::NotNull;
using ::std::string;
using SleepTest = MujocoTest;
@@ -177,5 +183,348 @@ TEST_F(SleepTest, MjSleepUpdate) {
mj_deleteModel(m);
}
TEST_F(SleepTest, MjWakeTree) {
// one awake tree and two cycles
int asleep[] = {kAwake, 2, 1, 3};
EXPECT_EQ(mj_wakeTree(asleep, 4, 0, kAwake), 0);
EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, 2, 1, 3));
EXPECT_EQ(mj_wakeTree(asleep, 4, 1, kAwake), 2);
EXPECT_THAT(AsVector(asleep, 4),
ElementsAre(kAwake, kAwake, kAwake, 3));
EXPECT_EQ(mj_wakeTree(asleep, 4, 3, kAwake), 1);
EXPECT_THAT(AsVector(asleep, 4),
ElementsAre(kAwake, kAwake, kAwake, kAwake));
}
TEST_F(SleepTest, BadWakeTree) {
EXPECT_FATAL_FAILURE(
([] {
int asleep_bad1[] = {-1, 0};
mj_wakeTree(asleep_bad1, 2, 1, kAwake);
}()),
"invalid sleep state index -1 when waking tree 1");
EXPECT_FATAL_FAILURE(
([] {
int asleep_bad2[] = {-1, 2};
mj_wakeTree(asleep_bad2, 2, 1, kAwake);
}()),
"invalid sleep state index 2 when waking tree 1");
EXPECT_FATAL_FAILURE(
([] {
int asleep_bad3[] = {1, 2, 1};
mj_wakeTree(asleep_bad3, 3, 0, kAwake);
}()),
"tree 0 is not in a cycle");
}
static const char* const kStaticModel = "engine/testdata/sleep/static.xml";
static const char* const kSmoothModel = "engine/testdata/sleep/smooth.xml";
static const char* const kInitModel = "engine/testdata/sleep/init.xml";
static const char* const kInitIslandModel =
"engine/testdata/sleep/init_island.xml";
static const char* const kTendonModel = "engine/testdata/sleep/tendon.xml";
static const char* const kContactModel = "engine/testdata/sleep/contact.xml";
static const char* const kPairModel = "engine/testdata/sleep/contactpair.xml";
static const char* const kSensorModel = "engine/testdata/sleep/sensor.xml";
// roll out some models with sleeping enabled, valuable under ASAN and MSAN
TEST_F(SleepTest, KickTires) {
for (const char* path :
{kStaticModel, kInitModel, kInitIslandModel, kSensorModel, kTendonModel,
kContactModel, kPairModel, kSmoothModel}) {
const std::string xml_path = GetTestDataFilePath(path);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
int duration_id = mj_name2id(m, mjOBJ_NUMERIC, "duration");
ASSERT_GE(duration_id, 0);
mjtNum duration = m->numeric_data[m->numeric_adr[duration_id]];
mjData* d = mj_makeData(m);
while (d->time < duration) {
mj_step(m, d);
}
mj_deleteData(d);
mj_deleteModel(m);
}
}
// Test that sleeping does not affect the simulation of awake trees:
// Roll out kSmoothModel, where all trees go to sleep within `duration` seconds
// in two mjData's, one with sleeping enabled and one without; expect the same
// values (for selected arrays) in awake trees in both.
TEST_F(SleepTest, WakingUnaffectedBySleeping) {
const std::string xml_path = GetTestDataFilePath(kSmoothModel);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
int duration_id = mj_name2id(m, mjOBJ_NUMERIC, "duration");
ASSERT_GE(duration_id, 0);
mjtNum duration = m->numeric_data[m->numeric_adr[duration_id]];
for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) {
m->opt.jacobian = jacobian;
for (mjtIntegrator integrator : // TODO: b/457674312 - Add support for RK4.
{mjINT_EULER, mjINT_IMPLICITFAST, mjINT_IMPLICIT}) {
m->opt.integrator = integrator;
// make data with sleeping enabled
m->opt.enableflags |= mjENBL_SLEEP;
mjData* d_sleep = mj_makeData(m);
// make data with sleeping disabled
m->opt.enableflags &= ~mjENBL_SLEEP;
mjData* d_nosleep = mj_makeData(m);
// disable constraints, contacts
m->opt.disableflags |= mjDSBL_CONSTRAINT | mjDSBL_CONTACT;
ASSERT_EQ(d_sleep->nbody_awake, m->nbody);
int nbody_awake = -1;
while (d_nosleep->time < duration) {
m->opt.enableflags |= mjENBL_SLEEP;
mj_step(m, d_sleep);
m->opt.enableflags &= ~mjENBL_SLEEP;
mj_step(m, d_nosleep);
// if nbody_awake is not changed, skip
if (d_sleep->nbody_awake == nbody_awake) {
continue;
}
// compare xpos
for (int i = 0; i < m->nbody; i++) {
if (d_sleep->body_awake[i] == mjS_ASLEEP) continue;
auto xpos1 = AsVector(d_nosleep->xpos + 3 * i, 3);
auto xpos2 = AsVector(d_sleep->xpos + 3 * i, 3);
EXPECT_EQ(xpos1, xpos2)
<< " xpos[" << i << "] at time " << d_nosleep->time;
}
// compare M and qLD
for (int i = 0; i < d_sleep->nv_awake; i++) {
int j = d_sleep->dof_awake_ind[i];
auto M1 = AsVector(d_nosleep->M + m->M_rowadr[j], m->M_rownnz[j]);
auto M2 = AsVector(d_sleep->M + m->M_rowadr[j], m->M_rownnz[j]);
EXPECT_EQ(M1, M2) << " M[" << j << ",:] at time " << d_nosleep->time;
auto qLD1 = AsVector(d_nosleep->qLD + m->M_rowadr[j], m->M_rownnz[j]);
auto qLD2 = AsVector(d_sleep->qLD + m->M_rowadr[j], m->M_rownnz[j]);
EXPECT_EQ(qLD1, qLD2)
<< " qLD[" << j << ",:] at time " << d_nosleep->time;
}
// compare cvel
for (int i = 0; i < d_sleep->nbody_awake; i++) {
if (d_sleep->body_awake[i] == mjS_ASLEEP) continue;
auto cvel1 = AsVector(d_nosleep->cvel + 6 * i, 6);
auto cvel2 = AsVector(d_sleep->cvel + 6 * i, 6);
EXPECT_EQ(cvel1, cvel2)
<< " cvel[" << i << "] at time " << d_nosleep->time;
}
// compare subtree_angmom, only for dynamic bodies
for (int i = 0; i < d_sleep->nbody_awake; i++) {
if (d_sleep->body_awake[i] != mjS_AWAKE) continue;
auto subtree_angmom1 = AsVector(d_nosleep->subtree_angmom + 3 * i, 3);
auto subtree_angmom2 = AsVector(d_sleep->subtree_angmom + 3 * i, 3);
EXPECT_EQ(subtree_angmom1, subtree_angmom2)
<< " subtree_angmom[" << i << "] at time " << d_nosleep->time;
}
// compare qfrc/qacc arrays
for (int i = 0; i < d_sleep->nv_awake; i++) {
int j = d_sleep->dof_awake_ind[i];
EXPECT_EQ(d_nosleep->qfrc_smooth[j], d_sleep->qfrc_smooth[j])
<< " qfrc_smooth[" << j << "] at time " << d_nosleep->time;
EXPECT_EQ(d_nosleep->qacc_smooth[j], d_sleep->qacc_smooth[j])
<< " qacc_smooth[" << j << "] at time " << d_nosleep->time;
EXPECT_EQ(d_nosleep->qacc[j], d_sleep->qacc[j])
<< " qacc[" << j << "] at time " << d_nosleep->time;
}
nbody_awake = d_sleep->nbody_awake;
}
mj_deleteData(d_sleep);
mj_deleteData(d_nosleep);
}
}
mj_deleteModel(m);
}
// Test that waking does not affect sleeping trees for pos/vel-dependent arrays.
// Roll out models where some trees wake and/or sleep. At kCompare intervals,
// copy the state from the mjData with sleeping enabled to another mjData and
// call mj_forward with sleeping disabled. Expect pos/vel-dependent arrays to be
// unchanged for all trees and frc/acc-dependent arrays to be the same for awake
// trees.
TEST_F(SleepTest, SleepingUnaffectedByWaking) {
for (const char* path : {kInitModel, kInitIslandModel, kTendonModel,
kContactModel, kSensorModel, kSmoothModel}) {
const std::string xml_path = GetTestDataFilePath(path);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
const int kCompare = 10; // number of comparisons per rollout
// TODO: b/457674312 - Add support for RK4.
for (mjtIntegrator integrator :
{mjINT_EULER, mjINT_IMPLICITFAST, mjINT_IMPLICIT}) {
m->opt.integrator = integrator;
// make data with sleeping enabled
m->opt.enableflags |= mjENBL_SLEEP;
mjData* d_sleep = mj_makeData(m);
// make data with sleeping disabled
m->opt.enableflags &= ~mjENBL_SLEEP;
mjData* d_nosleep = mj_makeData(m);
int duration_id = mj_name2id(m, mjOBJ_NUMERIC, "duration");
ASSERT_GE(duration_id, 0);
mjtNum duration = m->numeric_data[m->numeric_adr[duration_id]];
int compare_interval = duration / (m->opt.timestep * kCompare);
int nsteps = 0;
while (d_sleep->time < duration) {
// step d_sleep with sleeping enabled
m->opt.enableflags |= mjENBL_SLEEP;
mj_step(m, d_sleep);
nsteps++;
// every compare_interval steps, compare with d_nosleep
if (nsteps % compare_interval != 0) {
continue;
}
// call mj_forward to update d_sleep
mj_forward(m, d_sleep);
// copy state from d_sleep to d_nosleep
mj_copyData(d_nosleep, m, d_sleep);
// forward d_nosleep with sleeping disabled
m->opt.enableflags &= ~mjENBL_SLEEP;
mj_forward(m, d_nosleep);
// ==== compare arrays for all dofs / bodies / sensors ====
// compare xpos
for (int i = 0; i < m->nbody; i++) {
auto xpos1 = AsVector(d_sleep->xpos + 3 * i, 3);
auto xpos2 = AsVector(d_nosleep->xpos + 3 * i, 3);
EXPECT_EQ(xpos1, xpos2)
<< " xpos[" << i << "] at time " << d_sleep->time;
}
// compare M and qLD
for (int i = 0; i < m->nv; i++) {
auto M1 = AsVector(d_sleep->M + m->M_rowadr[i], m->M_rownnz[i]);
auto M2 = AsVector(d_nosleep->M + m->M_rowadr[i], m->M_rownnz[i]);
EXPECT_EQ(M1, M2) << " M[" << i << ",:] at time " << d_sleep->time;
auto qLD1 = AsVector(d_sleep->qLD + m->M_rowadr[i], m->M_rownnz[i]);
auto qLD2 = AsVector(d_nosleep->qLD + m->M_rowadr[i], m->M_rownnz[i]);
EXPECT_EQ(qLD1, qLD2)
<< " qLD[" << i << ",:] at time " << d_sleep->time;
}
// compare cvel
for (int i = 0; i < m->nbody; i++) {
auto cvel1 = AsVector(d_sleep->cvel + 6 * i, 6);
auto cvel2 = AsVector(d_nosleep->cvel + 6 * i, 6);
EXPECT_EQ(cvel1, cvel2)
<< " cvel[" << i << "] at time " << d_sleep->time;
}
// compare qfrc arrays
for (int i = 0; i < m->nv; i++) {
EXPECT_EQ(d_sleep->qfrc_fluid[i], d_nosleep->qfrc_fluid[i])
<< " qfrc_fluid[" << i << "] at time " << d_sleep->time;
EXPECT_EQ(d_sleep->qfrc_damper[i], d_nosleep->qfrc_damper[i])
<< " qfrc_damper[" << i << "] at time " << d_sleep->time;
EXPECT_EQ(d_sleep->qfrc_spring[i], d_nosleep->qfrc_spring[i])
<< " qfrc_spring[" << i << "] at time " << d_sleep->time;
EXPECT_EQ(d_sleep->qfrc_gravcomp[i], d_nosleep->qfrc_gravcomp[i])
<< " qfrc_gravcomp[" << i << "] at time " << d_sleep->time;
EXPECT_EQ(d_sleep->qfrc_bias[i], d_nosleep->qfrc_bias[i])
<< " qfrc_bias[" << i << "] at time " << d_sleep->time;
}
// compare sensordata
for (int i = 0; i < m->nsensor; i++) {
int dim = m->sensor_dim[i];
int adr = m->sensor_adr[i];
auto data1 = AsVector(d_sleep->sensordata + adr, dim);
auto data2 = AsVector(d_nosleep->sensordata + adr, dim);
EXPECT_EQ(data1, data2)
<< " sensor " << i << " at time " << d_sleep->time;
}
// ==== compare arrays for awake dofs only ====
// compare qacc arrays for awake dofs
for (int j = 0; j < d_sleep->nv_awake; j++) {
int i = d_sleep->dof_awake_ind[j];
EXPECT_EQ(d_sleep->qacc_smooth[i], d_nosleep->qacc_smooth[i])
<< " qacc_smooth[" << i << "] at time " << d_sleep->time;
EXPECT_EQ(d_sleep->qacc[i], d_nosleep->qacc[i])
<< " qacc[" << i << "] at time " << d_sleep->time;
}
}
mj_deleteData(d_nosleep);
mj_deleteData(d_sleep);
}
mj_deleteModel(m);
}
}
static const char* const kEqualityModel = "engine/testdata/sleep/equality.xml";
// Activate equality between sleeping and awake trees, useful under ASAN/MSAN.
TEST_F(SleepTest, Equality) {
const std::string xml_path = GetTestDataFilePath(kEqualityModel);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
while (d->ntree_awake == m->ntree) {
mj_step(m, d);
}
int dd = mj_name2id(m, mjOBJ_EQUALITY, "dyn/dyn");
ASSERT_GE(dd, 0);
mj_step(m, d);
d->eq_active[dd] = 1;
mj_step(m, d);
mj_deleteData(d);
mj_deleteModel(m);
}
static const char* const kInitIslandFailModel =
"engine/testdata/sleep/init_island_fail.xml";
TEST_F(SleepTest, InitIslandFail) {
const std::string xml_path = GetTestDataFilePath(kInitIslandFailModel);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
EXPECT_THAT(m, IsNull());
EXPECT_THAT(error,
HasSubstr("3 trees were marked as sleep='init' but only 0 could "
"be slept.\nBody 'asleep_init0' (id=1) is the root of "
"the first tree that could not be slept."));
}
} // namespace
} // namespace mujoco
test/engine/testdata/sleep/contact.xml
Vendored
+36
View File
@@ -0,0 +1,36 @@
<mujoco>
<custom>
<numeric name="duration" data="1.5"/>
</custom>
<default>
<default class="domino">
<geom type="box" size=".008 .044 .089" rgba=".9 .9 .9 1" mass="0.09"/>
</default>
</default>
<visual>
<global realtime="0.2"/>
</visual>
<option cone="elliptic" impratio="10">
<flag sleep="enable"/>
</option>
<worldbody>
<light pos="0 0 3"/>
<geom type="plane" size="1 1 .01" pos="0 0 1e-6"/>
<body pos="-.1 0 .089" euler="0 7 0">
<freejoint/>
<geom type="box" class="domino"/>
</body>
<replicate count="4" offset=".1 0 0">
<body pos="0 0 .089">
<freejoint/>
<geom type="box" class="domino"/>
</body>
</replicate>
</worldbody>
</mujoco>
test/engine/testdata/sleep/contactpair.xml
Vendored
+39
View File
@@ -0,0 +1,39 @@
<mujoco>
<custom>
<numeric name="duration" data="1.0"/>
</custom>
<default>
<default class="domino">
<geom type="box" size=".008 .044 .089" rgba=".9 .9 .9 1" mass="0.09"/>
</default>
</default>
<visual>
<global realtime="0.2"/>
</visual>
<option cone="elliptic" impratio="10">
<flag sleep="enable"/>
</option>
<worldbody>
<light pos="0 0 3"/>
<geom name="floor" type="plane" size="1 1 .01" pos="0 0 1e-6"/>
<body pos="-.1 0 .089" euler="0 7 0">
<freejoint/>
<geom name="1" type="box" class="domino"/>
</body>
<body pos="0 0 .089">
<freejoint/>
<geom name="2" type="box" class="domino" contype="0" conaffinity="0"/>
</body>
</worldbody>
<contact>
<pair geom1="1" geom2="2"/>
<pair geom1="2" geom2="floor"/>
</contact>
</mujoco>
test/engine/testdata/sleep/equality.xml
Vendored
+60
View File
@@ -0,0 +1,60 @@
<mujoco>
<option>
<flag sleep="enable"/>
</option>
<worldbody>
<light pos="0 -1 4"/>
<geom type="plane" size="1.5 1.5 .01"/>
<body pos=".3 0 .8">
<joint name="1" axis="1 0 0" damping=".1"/>
<geom type="box" size=".03 .1 .1"/>
</body>
<body pos=".7 0 .8">
<joint name="2" axis="1 0 0" damping=".1"/>
<geom type="box" size=".03 .1 .1"/>
</body>
<body pos=".5 0 .5">
<geom type="box" size=".1 .1 .1"/>
<site name="1" pos="-.1 0 0" size=".01" rgba="1 0 0 1"/>
</body>
<body pos="-.3 -.4 .5" euler="10 20 30">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
<site name="2" pos="0 0 .1" size=".01" rgba="1 0 0 1" euler="0 -90 0"/>
<site name="3" pos="0 -.1 0" size=".01" rgba="1 0 0 1" euler="0 0 -90"/>
</body>
<body pos=".7 -.3 .5">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
<site name="4" pos="-.1 0 0" size=".01" rgba="1 0 0 1"/>
</body>
<body pos="-.3 0 .7">
<joint axis="0 1 0" springdamper=".03 1"/>
<geom type="capsule" size=".03" fromto="0 0 0 .2 0 0"/>
<site name="5" pos=".1 0 .03" size=".01" rgba="1 0 0 1"/>
<body pos=".2 0 0">
<joint axis="0 1 0" springdamper=".03 1"/>
<geom type="capsule" size=".03" fromto="0 0 0 0 0 .2"/>
<body pos="0 0 .2">
<joint axis="0 1 0" springdamper=".03 1"/>
<geom type="capsule" size=".03" fromto="0 0 0 -.15 0 0"/>
<site name="6" pos="-.18 0 0" size=".01" rgba="1 0 0 1"/>
</body>
</body>
</body>
</worldbody>
<equality>
<weld active="false" name="stat/dyn" site1="1" site2="2"/>
<weld active="false" name="dyn/dyn" site1="3" site2="4"/>
<connect active="false" name="self" site1="5" site2="6"/>
<joint name="joint" joint1="1" joint2="2" polycoef="0 -2"/>
</equality>
</mujoco>
test/engine/testdata/sleep/init.xml
Vendored
+26
View File
@@ -0,0 +1,26 @@
<mujoco>
<custom>
<numeric name="duration" data="2.0"/>
</custom>
<option>
<flag sleep="enable"/>
</option>
<worldbody>
<light pos="0 -1 3"/>
<geom type="plane" size="1 1 .01"/>
<replicate count="3" offset="0 0 .3">
<body pos="0 0 .15" sleep="init">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
</body>
</replicate>
<body pos="0 0 1.2" euler="10 20 30">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/init_island.xml
Vendored
+26
View File
@@ -0,0 +1,26 @@
<mujoco>
<custom>
<numeric name="duration" data="2.0"/>
</custom>
<option>
<flag sleep="enable"/>
</option>
<worldbody>
<light pos="0 -1 3"/>
<geom type="plane" size="1 1 .01"/>
<replicate count="3" offset="0 0 .18" euler="0 0 20">
<body pos="0 0 .08" sleep="init">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
</body>
</replicate>
<body pos="0 0 1.2" euler="10 20 30">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/init_island_fail.xml
Vendored
+28
View File
@@ -0,0 +1,28 @@
<mujoco>
<custom>
<numeric name="duration" data="2.0"/>
</custom>
<option>
<flag sleep="enable"/>
</option>
<worldbody>
<light pos="0 -1 3"/>
<geom type="plane" size="1 1 .01"/>
<replicate count="3" offset="0 0 .18" euler="0 0 20">
<body name="asleep_init" pos="0 0 .08" sleep="init">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
</body>
</replicate>
<!-- the free objects above are marked as sleep="init", but they are touching this object which
is awake so sleep initialization should fail -->
<body pos="0 0 .6" euler="10 20 30">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/perf/2humanoid100.xml
Vendored
+123
View File
@@ -0,0 +1,123 @@
<mujoco model="2 Humanoids and 100 objects">
<!--
Model designed for a maximally-elaborate island structure.
More horizontal gravity leads to larger, fewer islands.
-->
<option timestep="0.005" cone="elliptic" impratio="10" sleep_tolerance="1e-3">
<flag island="enable" sleep="enable"/>
</option>
<size memory="100M"/>
<default>
<geom condim="6" friction="1 .01 .003"/>
<default class="capsule">
<geom type="capsule" material="capsule" size="0.1 0.05"/>
</default>
<default class="ellipsoid">
<geom type="ellipsoid" material="ellipsoid" size="0.15 0.1 0.07"/>
</default>
<default class="box">
<geom type="box" material="box" size="0.15 0.1 0.05"/>
</default>
<default class="cylinder">
<geom type="cylinder" material="cylinder" size="0.1 0.05"/>
</default>
<default class="sphere">
<geom type="sphere" material="sphere" size="0.1"/>
</default>
<default class="border">
<geom type="capsule" size="0.4" rgba=".4 .4 .4 1"/>
</default>
<default class="borderpost">
<geom type="box" size="0.41 0.41 0.41" rgba=".55 .55 .55 1"/>
</default>
</default>
<asset>
<model file="humanoid.xml"/>
<texture type="skybox" builtin="gradient" width="512" height="512" rgb1=".4 .6 .8" rgb2="0 0 0"/>
<texture name="texgeom" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.6 0.6 0.6" rgb2="0.6 0.6 0.6" markrgb="1 1 1"/>
<texture name="texplane" type="2d" builtin="checker" rgb1=".4 .4 .4" rgb2=".6 .6 .6" width="512" height="512"/>
<material name="MatPlane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true" rgba=".7 .7 .7 1"/>
<material name="capsule" texture="texgeom" texuniform="true" rgba=".4 .9 .6 1"/>
<material name="ellipsoid" texture="texgeom" texuniform="true" rgba=".4 .6 .9 1"/>
<material name="box" texture="texgeom" texuniform="true" rgba=".4 .9 .9 1"/>
<material name="cylinder" texture="texgeom" texuniform="true" rgba=".8 .6 .8 1"/>
<material name="sphere" texture="texgeom" texuniform="true" rgba=".9 .1 .1 1"/>
</asset>
<visual>
<quality shadowsize="4096" offsamples="8"/>
<map znear="0.1" force="0.05"/>
</visual>
<statistic extent="4"/>
<worldbody>
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
<geom name="floor" type="plane" size="3 3 .5" material="MatPlane"/>
<geom class="border" fromto="-3 3 0 3 3 0"/>
<geom class="border" fromto="-3 -3 0 3 -3 0"/>
<geom class="border" fromto="3 3 0 3 -3 0"/>
<geom class="border" fromto="-3 3 0 -3 -3 0"/>
<geom class="borderpost" pos="3 3 0"/>
<geom class="borderpost" pos="-3 3 0"/>
<geom class="borderpost" pos="3 -3 0"/>
<geom class="borderpost" pos="-3 -3 0"/>
<replicate count="4" euler="0 0 90">
<geom type="plane" size=".5 3 .05" zaxis="1 0 0" pos="-3 0 0.4"/>
</replicate>
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="30 40 0">
<freejoint/>
<geom class="capsule"/>
</body>
</replicate>
<attach model="Humanoid" body="torso" prefix="1_"/>
<frame euler="0 0 72">
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="20 40 60">
<freejoint/>
<geom class="ellipsoid"/>
</body>
</replicate>
</frame>
<frame euler="0 0 144">
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="30 70 110">
<freejoint/>
<geom class="box"/>
</body>
</replicate>
</frame>
<frame pos="1 1 0" euler="0 0 144">
<attach model="Humanoid" body="torso" prefix="2_"/>
</frame>
<frame euler="0 0 216">
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="60 30 0">
<freejoint/>
<geom class="cylinder"/>
</body>
</replicate>
</frame>
<frame euler="0 0 288">
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="60 30 0">
<freejoint/>
<geom class="sphere"/>
</body>
</replicate>
</frame>
</worldbody>
</mujoco>
test/engine/testdata/sleep/perf/dominos.xml
Vendored
+77
View File
@@ -0,0 +1,77 @@
<mujoco model="sleeping dominos">
<option cone="elliptic" impratio="10" sleep_tolerance="3e-4">
<flag sleep="enable"/>
</option>
<default>
<default class="domino">
<geom type="box" size=".008 .044 .089" rgba=".9 .9 .9 1" mass="0.09"/>
</default>
</default>
<worldbody>
<light pos=".5 0 3" diffuse="1 1 1"/>
<geom type="plane" size="2 2 .01" pos=".5 0 1e-6"/>
<!-- one domino atilt kicks off the chain reaction -->
<body pos="-.1 0 .089" euler="0 7 0">
<freejoint/>
<geom type="box" class="domino"/>
</body>
<!-- 5 rows of 15 dominoes, arranged in toppling order for consistent ids and colors -->
<!-- 3 rows of 15 dominoes, left to right -->
<replicate count="3" offset="0 1 0">
<frame pos="0 -1 0">
<replicate count="15" offset=".1 0 0">
<body pos="0 0 .089">
<freejoint/>
<geom type="box" class="domino"/>
</body>
</replicate>
</frame>
</replicate>
<!-- 2 rows of 15 dominoes, right to left -->
<replicate count="2" offset="0 1 0">
<frame pos="0 -.5 0">
<replicate count="15" offset="-.1 0 0">
<body pos="1.4 0 .089">
<freejoint/>
<geom type="box" class="domino"/>
</body>
</replicate>
</frame>
</replicate>
<!-- two "U turns", not too close in order to break the island chain -->
<replicate offset="0 -.55 0" count="2">
<frame pos="1.5 .26 0">
<replicate count="5" euler="0 0 45">
<body pos="0 -.23 .089">
<freejoint/>
<geom type="box" class="domino"/>
</body>
</replicate>
</frame>
</replicate>
<!-- two more U turns for the outside rows -->
<replicate offset="0 1.5 0" count="2">
<frame pos="-.1 -.75 0" euler="0 0 180">
<replicate count="5" euler="0 0 45">
<body pos="0 -.23 .089">
<freejoint/>
<geom type="box" class="domino"/>
</body>
</replicate>
</frame>
</replicate>
</worldbody>
<visual>
<global elevation="-25" azimuth="60"/>
</visual>
<statistic center="1.2 0 0"/>
</mujoco>
test/engine/testdata/sleep/perf/humanoid.xml
Vendored
+254
View File
@@ -0,0 +1,254 @@
<mujoco model="Humanoid">
<option timestep="0.005" cone="elliptic" impratio="10" sleep_tolerance="1e-3">
<flag sleep="enable"/>
</option>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
</visual>
<statistic center="0 0 0.7"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<default class="body">
<!-- geoms -->
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" group="1"/>
<default class="thigh">
<geom size=".06"/>
</default>
<default class="shin">
<geom fromto="0 0 0 0 0 -.3" size=".049"/>
</default>
<default class="foot">
<geom size=".027"/>
<default class="foot1">
<geom fromto="-.07 -.01 0 .14 -.03 0"/>
</default>
<default class="foot2">
<geom fromto="-.07 .01 0 .14 .03 0"/>
</default>
</default>
<default class="arm_upper">
<geom size=".04"/>
</default>
<default class="arm_lower">
<geom size=".031"/>
</default>
<default class="hand">
<geom type="sphere" size=".04"/>
</default>
<!-- joints -->
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01" frictionloss="0.0001"/>
<default class="joint_big">
<joint damping="5" stiffness="10"/>
<default class="hip_x">
<joint range="-30 10"/>
</default>
<default class="hip_z">
<joint range="-60 35"/>
</default>
<default class="hip_y">
<joint axis="0 1 0" range="-150 20"/>
</default>
<default class="joint_big_stiff">
<joint stiffness="20"/>
</default>
</default>
<default class="knee">
<joint pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
</default>
<default class="ankle">
<joint range="-50 50"/>
<default class="ankle_y">
<joint pos="0 0 .08" axis="0 1 0" stiffness="6"/>
</default>
<default class="ankle_x">
<joint pos="0 0 .04" stiffness="3"/>
</default>
</default>
<default class="shoulder">
<joint range="-85 60"/>
</default>
<default class="elbow">
<joint range="-100 50" stiffness="0"/>
</default>
</default>
</default>
<worldbody>
<geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>
<light name="spotlight" mode="targetbodycom" target="torso" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 -6 4" cutoff="30"/>
<light name="top" pos="0 0 2" mode="trackcom"/>
<body name="torso" pos="0 0 1.282" childclass="body" sleep="allowed">
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="root"/>
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="waist_upper" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="head" pos="0 0 .19">
<geom name="head" type="sphere" size=".09"/>
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="waist_lower" pos="-.01 0 -.26">
<geom name="waist_lower" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="joint_big_stiff"/>
<joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="joint_big"/>
<body name="pelvis" pos="0 0 -.165">
<joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="joint_big"/>
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="thigh_right" pos="0 -.1 -.04">
<joint name="hip_x_right" axis="1 0 0" class="hip_x"/>
<joint name="hip_z_right" axis="0 0 1" class="hip_z"/>
<joint name="hip_y_right" class="hip_y"/>
<geom name="thigh_right" fromto="0 0 0 0 .01 -.34" class="thigh"/>
<body name="shin_right" pos="0 .01 -.4">
<joint name="knee_right" class="knee"/>
<geom name="shin_right" class="shin"/>
<body name="foot_right" pos="0 0 -.39">
<joint name="ankle_y_right" class="ankle_y"/>
<joint name="ankle_x_right" class="ankle_x" axis="1 0 .5"/>
<geom name="foot1_right" class="foot1"/>
<geom name="foot2_right" class="foot2"/>
</body>
</body>
</body>
<body name="thigh_left" pos="0 .1 -.04">
<joint name="hip_x_left" axis="-1 0 0" class="hip_x"/>
<joint name="hip_z_left" axis="0 0 -1" class="hip_z"/>
<joint name="hip_y_left" class="hip_y"/>
<geom name="thigh_left" fromto="0 0 0 0 -.01 -.34" class="thigh"/>
<body name="shin_left" pos="0 -.01 -.4">
<joint name="knee_left" class="knee"/>
<geom name="shin_left" fromto="0 0 0 0 0 -.3" class="shin"/>
<body name="foot_left" pos="0 0 -.39">
<joint name="ankle_y_left" class="ankle_y"/>
<joint name="ankle_x_left" class="ankle_x" axis="-1 0 -.5"/>
<geom name="foot1_left" class="foot1"/>
<geom name="foot2_left" class="foot2"/>
</body>
</body>
</body>
</body>
</body>
<body name="upper_arm_right" pos="0 -.17 .06">
<joint name="shoulder1_right" axis="2 1 1" class="shoulder"/>
<joint name="shoulder2_right" axis="0 -1 1" class="shoulder"/>
<geom name="upper_arm_right" fromto="0 0 0 .16 -.16 -.16" class="arm_upper"/>
<body name="lower_arm_right" pos=".18 -.18 -.18">
<joint name="elbow_right" axis="0 -1 1" class="elbow"/>
<geom name="lower_arm_right" fromto=".01 .01 .01 .17 .17 .17" class="arm_lower"/>
<body name="hand_right" pos=".18 .18 .18">
<geom name="hand_right" zaxis="1 1 1" class="hand"/>
</body>
</body>
</body>
<body name="upper_arm_left" pos="0 .17 .06">
<joint name="shoulder1_left" axis="-2 1 -1" class="shoulder"/>
<joint name="shoulder2_left" axis="0 -1 -1" class="shoulder"/>
<geom name="upper_arm_left" fromto="0 0 0 .16 .16 -.16" class="arm_upper"/>
<body name="lower_arm_left" pos=".18 .18 -.18">
<joint name="elbow_left" axis="0 -1 -1" class="elbow"/>
<geom name="lower_arm_left" fromto=".01 -.01 .01 .17 -.17 .17" class="arm_lower"/>
<body name="hand_left" pos=".18 -.18 .18">
<geom name="hand_left" zaxis="1 -1 1" class="hand"/>
</body>
</body>
</body>
</body>
</worldbody>
<contact>
<exclude body1="waist_lower" body2="thigh_right"/>
<exclude body1="waist_lower" body2="thigh_left"/>
</contact>
<tendon>
<fixed name="hamstring_right" limited="true" range="-0.3 2">
<joint joint="hip_y_right" coef=".5"/>
<joint joint="knee_right" coef="-.5"/>
</fixed>
<fixed name="hamstring_left" limited="true" range="-0.3 2">
<joint joint="hip_y_left" coef=".5"/>
<joint joint="knee_left" coef="-.5"/>
</fixed>
</tendon>
<actuator>
<motor name="abdomen_z" gear="40" joint="abdomen_z"/>
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
<motor name="abdomen_x" gear="40" joint="abdomen_x"/>
<motor name="hip_x_right" gear="40" joint="hip_x_right"/>
<motor name="hip_z_right" gear="40" joint="hip_z_right"/>
<motor name="hip_y_right" gear="120" joint="hip_y_right"/>
<motor name="knee_right" gear="80" joint="knee_right"/>
<motor name="ankle_y_right" gear="20" joint="ankle_y_right"/>
<motor name="ankle_x_right" gear="20" joint="ankle_x_right"/>
<motor name="hip_x_left" gear="40" joint="hip_x_left"/>
<motor name="hip_z_left" gear="40" joint="hip_z_left"/>
<motor name="hip_y_left" gear="120" joint="hip_y_left"/>
<motor name="knee_left" gear="80" joint="knee_left"/>
<motor name="ankle_y_left" gear="20" joint="ankle_y_left"/>
<motor name="ankle_x_left" gear="20" joint="ankle_x_left"/>
<motor name="shoulder1_right" gear="20" joint="shoulder1_right"/>
<motor name="shoulder2_right" gear="20" joint="shoulder2_right"/>
<motor name="elbow_right" gear="40" joint="elbow_right"/>
<motor name="shoulder1_left" gear="20" joint="shoulder1_left"/>
<motor name="shoulder2_left" gear="20" joint="shoulder2_left"/>
<motor name="elbow_left" gear="40" joint="elbow_left"/>
</actuator>
<keyframe>
<!--
The values below are split into rows for readibility:
torso position
torso orientation
spinal
right leg
left leg
arms
-->
<key name="squat"
qpos="0 0 0.596
0.988015 0 0.154359 0
0 0.4 0
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
0 0 0 0 0 0"/>
<key name="stand_on_left_leg"
qpos="0 0 1.21948
0.971588 -0.179973 0.135318 -0.0729076
-0.0516 -0.202 0.23
-0.24 -0.007 -0.34 -1.76 -0.466 -0.0415
-0.08 -0.01 -0.37 -0.685 -0.35 -0.09
0.109 -0.067 -0.7 -0.05 0.12 0.16"/>
<key name="prone"
qpos="0.4 0 0.0757706
0.7325 0 0.680767 0
0 0.0729 0
0.0077 0.0019 -0.026 -0.351 -0.27 0
0.0077 0.0019 -0.026 -0.351 -0.27 0
0.56 -0.62 -1.752
0.56 -0.62 -1.752"/>
<key name="supine"
qpos="-0.4 0 0.08122
0.722788 0 -0.69107 0
0 -0.25 0
0.0182 0.0142 0.3 0.042 -0.44 -0.02
0.0182 0.0142 0.3 0.042 -0.44 -0.02
0.186 -0.73 -1.73
0.186 -0.73 -1.73"/>
</keyframe>
</mujoco>
test/engine/testdata/sleep/perf/smooth.xml
Vendored
+40
View File
@@ -0,0 +1,40 @@
<mujoco>
<option viscosity="30" integrator="implicit">
<flag gravity="disable" constraint="disable" contact="disable" energy="enable" sleep="enable"/>
</option>
<worldbody>
<replicate offset="0 0 2.5" count="7">
<replicate offset="4 0 0" count="7">
<replicate offset="0 2 0" count="7">
<body sleep="init">
<geom size=".4"/>
<freejoint/>
<body pos="-.4 0 0">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
<joint type="slide" axis="0 0 1" springref="0.2" stiffness="1000" damping="100"/>
</body>
</body>
<body pos="1 0 0" sleep="init">
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
<joint axis="1 0 0" stiffness="300" damping="30"/>
<body pos="0 0 1">
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
<joint axis="1 0 0" springref="45" stiffness="300" damping="30"/>
</body>
</body>
<body pos="2 0 0" sleep="init">
<geom type="box" size=".2 .2 .2"/>
<joint type="ball" stiffness="300" damping="30"/>
<body pos=".2 0 0">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
<joint type="slide" axis="0 0 1" springref="0.2" stiffness="1000" damping="100"/>
</body>
</body>
</replicate>
</replicate>
</replicate>
</worldbody>
</mujoco>
test/engine/testdata/sleep/perf/static.xml
Vendored
+40
View File
@@ -0,0 +1,40 @@
<mujoco>
<size memory="200M"/>
<option timestep="1e-2">
<flag sleep="enable"/>
</option>
<worldbody>
<replicate offset="0 0 2.5" count="7">
<replicate offset="4 0 0" count="7">
<replicate offset="0 2 0" count="7">
<body>
<geom size=".4"/>
<body pos="-.4 0 0">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
</body>
</body>
<geom type="box" size="0.1" fromto=".6 0 0 .6 0 1"/>
<body pos="1 0 0">
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
<body pos="0 0 1">
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
</body>
</body>
<geom type="box" size="0.1" fromto="1.4 0 0 1.4 0 1"/>
<body pos="2 0 0">
<geom type="box" size=".2 .2 .2"/>
<body pos=".2 0 0">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
</body>
</body>
</replicate>
</replicate>
</replicate>
</worldbody>
</mujoco>
test/engine/testdata/sleep/perf/world.xml
Vendored
+40
View File
@@ -0,0 +1,40 @@
<mujoco>
<size memory="200M"/>
<option timestep="1e-2">
<flag sleep="enable"/>
</option>
<worldbody>
<replicate offset="0 0 2.5" count="7">
<replicate offset="4 0 0" count="7">
<replicate offset="0 2 0" count="7">
<frame>
<geom size=".4"/>
<frame pos="-.4 0 0">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
</frame>
</frame>
<geom type="box" size="0.1" fromto=".6 0 0 .6 0 1"/>
<frame pos="1 0 0">
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
<frame pos="0 0 1">
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
</frame>
</frame>
<geom type="box" size="0.1" fromto="1.4 0 0 1.4 0 1"/>
<frame pos="2 0 0">
<geom type="box" size=".2 .2 .2"/>
<frame pos=".2 0 0">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
</frame>
</frame>
</replicate>
</replicate>
</replicate>
</worldbody>
</mujoco>
test/engine/testdata/sleep/sensor.xml
Vendored
+40
View File
@@ -0,0 +1,40 @@
<mujoco>
<custom>
<numeric name="duration" data="2.0"/>
</custom>
<option>
<flag sleep="enable" warmstart="disable"/>
</option>
<worldbody>
<light pos="0 -1 3"/>
<geom name="floor" type="plane" size="1 1 .01"/>
<body name="1" pos="0 0 .1">
<freejoint/>
<geom name="1" type="box" size=".1 .1 .1"/>
</body>
<body name="2" pos="0 0 .3">
<freejoint/>
<geom name="2" type="box" size=".1 .1 .1"/>
<site name="2"/>
</body>
<body name="3" pos="0 0 1.2" euler="10 20 30">
<freejoint/>
<geom name="3" type="box" size=".1 .1 .1"/>
</body>
</worldbody>
<sensor>
<contact reduce="maxforce" data="force"/>
<contact geom1="1" geom2="2"/>
<contact geom1="1" geom2="2" reduce="maxforce" data="force"/>
<contact geom1="1" geom2="floor"/>
<contact geom1="1" reduce="maxforce" data="force"/>
<gyro site="2"/>
<clock/>
</sensor>
</mujoco>
test/engine/testdata/sleep/smooth.xml
Vendored
+54
View File
@@ -0,0 +1,54 @@
<mujoco>
<custom>
<numeric name="duration" data="6.5"/>
</custom>
<option viscosity="60" gravity="0 0 -1">
<flag energy="enable" sleep="enable" constraint="disable" contact="disable"/>
</option>
<worldbody>
<body gravcomp="1">
<geom type="box" size=".2 .2 .2"/>
<freejoint/>
<body pos="-.4 0 0" gravcomp="1">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
<joint type="slide" axis="0 0 1" springref="0.2" springdamper="0.1 1"/>
</body>
</body>
<body pos="1 0 0">
<geom type="box" size=".2 .2 .2"/>
<body>
<joint axis="1 0 0" springdamper="0.1 1"/>
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
<site name="1" pos="0 -.1 .5" size=".03" rgba="1 0 0 1"/>
<body pos="0 0 1">
<joint axis="1 0 0" springref="45" springdamper="0.1 1"/>
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
<site name="2" pos="0 -.1 .5" size=".03" rgba="1 0 0 1"/>
</body>
</body>
</body>
<body pos="2 0 0">
<geom size=".3"/>
<joint type="ball" stiffness="300" damping="30"/>
<body name="angmom" pos=".2 0 0">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
<joint type="slide" axis="0 0 1" springref="0.2" stiffness="1000" damping="100"/>
</body>
</body>
</worldbody>
<tendon>
<spatial width="0.01" armature="1">
<site site="1"/>
<site site="2"/>
</spatial>
</tendon>
<sensor>
<subtreeangmom body="angmom"/>
</sensor>
</mujoco>
test/engine/testdata/sleep/static.xml
Vendored
+35
View File
@@ -0,0 +1,35 @@
<mujoco>
<custom>
<numeric name="duration" data="0.1"/>
</custom>
<option>
<flag sleep="enable"/>
</option>
<worldbody>
<body>
<geom type="box" size=".2 .2 .2"/>
<body pos="-.4 0 0">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
</body>
</body>
<body pos="1 0 0">
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
<body pos="0 0 1">
<geom type="capsule" size="0.1" fromto="0 0 0 0 0 1"/>
<site/>
</body>
</body>
<body pos="2 0 0">
<geom size=".3"/>
<body pos=".2 0 0">
<geom type="cylinder" size="0.1" fromto="0 0 0 0 0 1"/>
<camera/>
<light pos="0 0 1.5"/>
</body>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/tendon.xml
Vendored
+33
View File
@@ -0,0 +1,33 @@
<mujoco>
<custom>
<numeric name="duration" data="2.0"/>
</custom>
<option>
<flag sleep="enable"/>
</option>
<worldbody>
<light pos="0 -1 4"/>
<geom type="plane" size="1.5 1.5 .01" rgba=".5 .5 .5 .5"/>
<body pos=".3 0 1" sleep="init">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
<site name="1" pos="-.1 -.1 -.1" size=".01" rgba="1 0 0 1"/>
</body>
<body pos="-.3 0 1" euler="10 20 30">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
<site name="2" pos=".1 -.1 .1" size=".01" rgba="1 0 0 1"/>
</body>
</worldbody>
<tendon>
<spatial range="0 .6" width="0.01">
<site site="1"/>
<site site="2"/>
</spatial>
</tendon>
</mujoco>
test/engine/testdata/sleep/tendon2.xml
Vendored
+40
View File
@@ -0,0 +1,40 @@
<mujoco>
<option>
<flag sleep="enable"/>
</option>
<worldbody>
<light pos="0 -1 4"/>
<geom type="plane" size="1.5 1.5 .01" rgba=".5 .5 .5 .5"/>
<body pos=".3 0 1" sleep="init">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
<site name="1" pos="-.1 -.1 -.1" size=".01" rgba="1 0 0 1"/>
<site name="2" pos=".1 -.1 -.1" size=".01" rgba="1 0 0 1"/>
</body>
<body pos="-.3 0 1" euler="10 20 30">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
<site name="3" pos=".1 -.1 .1" size=".01" rgba="1 0 0 1"/>
</body>
<body pos=".7 0 1" sleep="init">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
<site name="4" pos="-.1 -.1 -.1" size=".01" rgba="1 0 0 1"/>
</body>
</worldbody>
<tendon>
<spatial range="0 .6" width="0.01">
<site site="1"/>
<site site="3"/>
</spatial>
<spatial range="0 .6" width="0.01">
<site site="2"/>
<site site="4"/>
</spatial>
</tendon>
</mujoco>
test/sample/testspeed_test.sh
+2 -2
View File
@@ -33,7 +33,7 @@ test_model() {
"$model" == */replicate/bunnies.xml ||
"$model" == */replicate/leaves.xml ||
"$model" == */replicate/particle.xml ||
"$model" == */engine/testdata/collision_convex/perf/*
"$model" == */perf/*
]]; then
# these tests can take several minutes under ASAN
return 0
@@ -76,7 +76,7 @@ for model_dir in ${MODEL_DIRS[@]}; do
echo "Skipping $model" >&2
continue
fi
if [[ $(basename $model) == malformed* ]]; then
if [[ $(basename $model) == *_fail.xml ]]; then
echo "Skipping $model" >&2
continue
fi
test/user/user_api_test.cc
+3 -1
View File
@@ -531,9 +531,11 @@ TEST_F(PluginTest, RecompileCompare) {
if (p.path().extension() == ext) {
std::string xml = p.path().string();
// if file is meant to fail, skip it
// if file is meant to fail or model is too slow to load, skip it
if (absl::StrContains(p.path().string(), "malformed_") ||
absl::StrContains(p.path().string(), "_fail") ||
absl::StrContains(p.path().string(), "touch_grid") ||
absl::StrContains(p.path().string(), "perf") ||
absl::StrContains(p.path().string(), "cow")) {
continue;
}
test/xml/xml_native_writer_test.cc
+5 -3
View File
@@ -1378,13 +1378,15 @@ TEST_F(XMLWriterTest, WriteReadCompare) {
if (p.path().extension() == ext) {
std::string xml = p.path().string();
// if file is meant to fail, skip it
if (absl::StrContains(p.path().string(), "malformed_") ||
if ( // if file is meant to fail, skip it
absl::StrContains(p.path().string(), "malformed_") ||
absl::StrContains(p.path().string(), "_fail") ||
// exclude files that are too slow to load
absl::StrContains(p.path().string(), "cow") ||
absl::StrContains(p.path().string(), "gmsh_") ||
absl::StrContains(p.path().string(), "shark_") ||
absl::StrContains(p.path().string(), "spheremesh") ||
absl::StrContains(p.path().string(), "perf") ||
// exclude files that fail the comparison test
absl::StrContains(p.path().string(), "tactile") ||
absl::StrContains(p.path().string(), "makemesh") ||