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

Add constraint island discovery

Browse Source 
PiperOrigin-RevId: 557067599
Change-Id: Ic41e1d0efef02b7a79142518afe49cf9d4e74725
This commit is contained in:
Yuval Tassa
2023-08-15 02:29:26 -07:00
committed by Copybara-Service
parent e4dddea42a
commit 3e034e38b2
40 changed files with 1557 additions and 468 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/CMakeLists.txt
+2
View File
@@ -37,6 +37,8 @@ set(MUJOCO_ENGINE_SRCS
engine_forward.h
engine_inverse.c
engine_inverse.h
engine_island.c
engine_island.h
engine_io.c
engine_io.h
engine_macro.h
src/engine/engine_core_constraint.c
+9 -130
View File
@@ -46,15 +46,18 @@
//-------------------------- utility functions -----------------------------------------------------
// internal function for clearing arena pointers for efc_ arrays in mjData
// clear arena pointers in mjData
static inline void clearEfc(mjData* d) {
#define X(type, name, nr, nc) d->name = NULL;
MJDATA_ARENA_POINTERS
#undef X
d->nefc = 0;
d->contact = d->arena;
d->nisland = 0;
d->contact = (mjContact*) d->arena;
}
// determine type of friction cone
int mj_isPyramidal(const mjModel* m) {
if (m->opt.cone == mjCONE_PYRAMIDAL) {
@@ -1606,16 +1609,15 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
// precount sizes for constraint Jacobian matrices
int *nnz = mj_isSparse(m) ? &(d->nnzJ) : NULL;
int ne_allocated = mj_ne(m, d, nnz);
int nf_allocated = mj_nf(m, d, nnz);
int nefc_allocated = ne_allocated + nf_allocated + mj_nl(m, d, nnz) + mj_nc(m, d, nnz);
if (!mj_isSparse(m)) {
d->nnzJ = nefc_allocated * m->nv;
}
d->nefc = nefc_allocated;
// ========== begin arena allocation
#undef MJ_M
#define MJ_M(n) m->n
#undef MJ_D
@@ -1623,6 +1625,8 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
// move arena pointer to end of contact array
d->parena = d->ncon * sizeof(mjContact);
// poison remaining memory
#ifdef ADDRESS_SANITIZER
ASAN_POISON_MEMORY_REGION(
(char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena);
@@ -1648,6 +1652,7 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
#define MJ_M(n) n
#undef MJ_D
#define MJ_D(n) n
// ========== end arena allocation
// reset nefc for the instantiation functions,
// and instantiate all elements of Jacobian
@@ -2103,129 +2108,3 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
*cost = s;
}
}
//---------------------------- constraint islands --------------------------------------------------
// comparison function for lexicographic edge sorting
quicksortfunc(edgecompare, context, edge0, edge1) {
int* e0 = (int*)edge0;
int* e1 = (int*)edge1;
int v00 = e0[0];
int v10 = e1[0];
if (v00 < v10) {
return -1;
}
if (v00 == v10) {
int v01 = e0[1];
int v11 = e1[1];
if (v01 < v11) {
return -1;
}
if (v01 == v11) {
return 0;
}
}
return 1;
}
// construct sparse matrix from unsorted edge array, return number of nonzeros
int mj_edge2Sparse(int* rownnz, int* rowadr, int* colind, int* edge, int ne, int nr) {
if (!ne) {
return 0;
}
// sort edges
mjQUICKSORT(edge, ne, 2*sizeof(int), edgecompare, NULL);
// construct sparse
int nnz = 0; // number of nonzeros
int e = 0; // current edge
for (int r=0; r < nr; r++) {
// init row
rownnz[r] = 0;
rowadr[r] = nnz;
// copy values while making unique and checking indices
while (e < ne && edge[2*e] == r) {
int v0 = edge[2*e];
int v1 = edge[2*e + 1];
// skip if duplicate
if (rownnz[r] && v0 == edge[2*e - 2] && v1 == edge[2*e - 1]) {
e++;
continue;
}
// check for invalid indices
if (v0 < 0 || v0 >= nr) mju_error("invalid row index %d in edge %d", v0, e);
if (v1 < 0 || v1 >= nr) mju_error("invalid column index %d in edge %d", v1, e);
// copy column index, increment nnz, e, rownnz
colind[nnz++] = edge[2*(e++) + 1];
rownnz[r]++;
}
}
return nnz;
}
// find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix
// arguments:
// island (nr) - island index assigned to vertex, -1 if vertex has no edges
// nr - number of rows/columns of adjacency matrix
// rownnz (nr) - matrix row nonzeros
// rowadr (nr) - matrix row addresses
// colind (nnz) - matrix column indices
// stack (nnz) - stack space
// returns number of islands
int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
int* stack) {
// initialize island count, set ids to -1
int nisland = 0;
for (int i=0; i < nr; i++) island[i] = -1;
// iterate over vertices, discover islands
for (int i=0; i < nr; i++) {
// vertex already in island or singleton with no edges: skip
if (island[i] != -1 || !rownnz[i]) {
continue;
}
// push i onto stack
int nstack = 0;
stack[nstack++] = i;
// DFS traversal of island
while (nstack) {
// pop v from stack
int v = stack[--nstack];
// if v is already assigned, continue
if (island[v] != -1) {
continue;
}
// assign v to current island
island[v] = nisland;
// push adjacent vertices onto stack
memcpy(stack + nstack, colind + rowadr[v], rownnz[v]*sizeof(int));
nstack += rownnz[v];
}
// island is filled: increment nisland
nisland++;
}
return nisland;
}
src/engine/engine_core_constraint.h
+1 -6
View File
@@ -18,6 +18,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjxmacro.h>
#ifdef __cplusplus
extern "C" {
@@ -115,12 +116,6 @@ MJAPI void mj_referenceConstraint(const mjModel* m, mjData* d);
MJAPI void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
mjtNum cost[1], int flg_coneHessian);
// construct sparse matrix from unsorted edge array, return number of nonzeros
MJAPI int mj_edge2Sparse(int* rownnz, int* rowadr, int* colind, int* edge, int ne, int nr);
MJAPI int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
int* scratch);
#ifdef __cplusplus
}
#endif
src/engine/engine_forward.c
+4
View File
@@ -27,6 +27,7 @@
#include "engine/engine_core_smooth.h"
#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_passive.h"
@@ -114,6 +115,9 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
TM_RESTART;
mj_makeConstraint(m, d);
if (mjENABLED(mjENBL_ISLAND)) {
mj_island(m, d);
}
mj_transmission(m, d);
TM_END(mjTIMER_POS_MAKE);
src/engine/engine_io.c
+1
View File
@@ -1321,6 +1321,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
d->nefc = 0;
d->nnzJ = 0;
d->ncon = 0;
d->nisland = 0;
// clear global properties
d->time = 0;
src/engine/engine_island.c
+521
View File
@@ -0,0 +1,521 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "engine/engine_island.h"
#include <stdio.h>
#include <stddef.h>
#include <string.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjxmacro.h>
#include "engine/engine_core_constraint.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_io.h"
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h"
#ifdef MEMORY_SANITIZER
#include <sanitizer/msan_interface.h>
#endif
// clear island-related arena pointers in mjData
static void clearIsland(mjData* d, size_t parena) {
#define X(type, name, nr, nc) d->name = NULL;
MJDATA_ARENA_POINTERS_ISLAND
#undef X
d->nefc = 0;
d->nisland = 0;
d->parena = parena;
// poison remaining memory
#ifdef ADDRESS_SANITIZER
ASAN_POISON_MEMORY_REGION(
(char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena);
#endif
}
// comparison function for lexicographic edge sorting
quicksortfunc(edgecompare, context, edge0, edge1) {
int* e0 = (int*)edge0;
int* e1 = (int*)edge1;
int v00 = e0[0];
int v10 = e1[0];
if (v00 < v10) {
return -1;
}
if (v00 == v10) {
int v01 = e0[1];
int v11 = e1[1];
if (v01 < v11) {
return -1;
}
if (v01 == v11) {
return 0;
}
}
return 1;
}
// construct sparse matrix from non-unique, unsorted edge array, return number of nonzeros
int mj_edge2Sparse(int* rownnz, int* rowadr, int* colind, int* edge, int ne, int nr) {
if (!ne) {
return 0;
}
// sort edges
mjQUICKSORT(edge, ne, 2*sizeof(int), edgecompare, NULL);
// construct sparse
int nnz = 0; // number of nonzeros
int e = 0; // current edge
for (int r=0; r < nr; r++) {
// init row
rownnz[r] = 0;
rowadr[r] = nnz;
// copy values while making unique and checking indices
while (e < ne && edge[2*e] == r) {
int v0 = edge[2*e];
int v1 = edge[2*e + 1];
// skip if duplicate
if (rownnz[r] && v0 == edge[2*e - 2] && v1 == edge[2*e - 1]) {
e++;
continue;
}
// check for invalid indices
if (v0 < 0 || v0 >= nr) mjERROR("invalid row index %d in edge %d", v0, e);
if (v1 < 0 || v1 >= nr) mjERROR("invalid column index %d in edge %d", v1, e);
// copy column index, increment nnz, e, rownnz
colind[nnz++] = edge[2*(e++) + 1];
rownnz[r]++;
}
}
return nnz;
}
// find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix
// arguments:
// island (nr) - island index assigned to vertex, -1 if vertex has no edges
// nr - number of rows/columns of adjacency matrix
// rownnz (nr) - matrix row nonzeros
// rowadr (nr) - matrix row addresses
// colind (nnz) - matrix column indices
// stack (nnz) - stack space
// returns number of islands
int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
int* stack) {
// initialize island count, set ids to -1
int nisland = 0;
for (int i=0; i < nr; i++) island[i] = -1;
// iterate over vertices, discover islands
for (int i=0; i < nr; i++) {
// vertex already in island or singleton with no edges: skip
if (island[i] != -1 || !rownnz[i]) {
continue;
}
// push i onto stack
int nstack = 0;
stack[nstack++] = i;
// DFS traversal of island
while (nstack) {
// pop v from stack
int v = stack[--nstack];
// if v is already assigned, continue
if (island[v] != -1) {
continue;
}
// assign v to current island
island[v] = nisland;
// push adjacent vertices onto stack
memcpy(stack + nstack, colind + rowadr[v], rownnz[v]*sizeof(int));
nstack += rownnz[v];
}
// island is filled: increment nisland
nisland++;
}
return nisland;
}
// return upper bound on number of tree-tree edges
static int countMaxEdge(const mjModel* m, const mjData* d) {
int nedge_max = 0;
nedge_max += 2*d->ncon; // contact: 2 edges
nedge_max += 2*d->ne; // equality: 2 edges
nedge_max += d->nf; // joint friction: 1 edge (always within same tree)
// tendon limits and friction add up to tendon_num edges
for (int i=0; i < m->ntendon; i++) {
if (m->tendon_frictionloss[i]) {
nedge_max += m->tendon_num[i];
}
if (m->tendon_limited[i]) {
nedge_max += m->tendon_num[i];
}
}
return nedge_max;
}
// add tree-tree edge array: check size, add flipped edge if non-self
static int addEdge(int* edge, int nedge, int tree1, int tree2, int nedge_max) {
// handle the static tree
if (tree1 == -1 && tree2 == -1) {
mjERROR("self-edge of the static tree"); // SHOULD NOT OCCUR
return 0;
}
if (tree1 == -1) tree1 = tree2;
if (tree2 == -1) tree2 = tree1;
// previous edge
int p1 = nedge ? edge[2*nedge - 2] : -1;
int p2 = nedge ? edge[2*nedge - 1] : -1;
// === self edge
if (tree1 == tree2) {
// same as previous edge, return
if (nedge && tree1 == p1 && tree1 == p2) {
return nedge;
}
// check size
if (nedge >= nedge_max) {
mjERROR("edge array too small");
return 0;
}
// add tree1-tree1 self-edge
edge[2*nedge + 0] = tree1;
edge[2*nedge + 1] = tree1;
return nedge + 1;
}
// === non-self edge
if (nedge && ((tree1 == p1 && tree2 == p2) || (tree1 == p2 && tree2 == p1))) {
// same as previous edge, return
return nedge;
}
// check size
if (nedge + 2 > nedge_max) {
mjERROR("edge array too small");
return 0;
}
// add tree1-tree2 and tree2-tree1
edge[2*nedge + 0] = tree1;
edge[2*nedge + 1] = tree2;
edge[2*nedge + 2] = tree2;
edge[2*nedge + 3] = tree1;
return nedge + 2;
}
// return id of next tree in Jacobian row i that is different from tree, -1 if not found
// write the index of the found tree to *index if given
// start search from *index if given, otherwise 0
// if J is (dense/sparse) *index is the (column/nonzro) index, respectively
static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *index) {
int tree_next = -1;
int j0 = index ? *index : 0; // start searching at *index if given, otherwise 0
int j; // loop variable, saved to *index
// sparse
if (mj_isSparse(m)) {
int rownnz = d->efc_J_rownnz[i];
int* colind = d->efc_J_colind + d->efc_J_rowadr[i];
// loop over remaining nonzeros, look for different tree
for (j=j0; j < rownnz; j++) {
int tree_j = m->dof_treeid[colind[j]];
if (tree_j != tree) {
// found different tree
tree_next = tree_j;
break;
}
}
}
// dense
else {
int nv = m->nv;
// scan row, look for different tree
for (j=j0; j < nv; j++) {
if (d->efc_J[nv*i + j]) {
int tree_j = m->dof_treeid[j];
if (tree_j != tree) {
// found different tree
tree_next = tree_j;
break;
}
}
}
}
// save last index
if (index) *index = j;
return tree_next;
}
// find tree-tree edges
static int findEdges(const mjModel* m, const mjData* d, int* edge, int nedge_max) {
int nefc = d->nefc;
int efc_type = -1;
int efc_id = -1;
int tree1, tree2;
int nedge = 0;
for (int i=0; i < nefc; i++) {
// row i is still in the same constraint: skip
if (efc_type == d->efc_type[i] && efc_id == d->efc_id[i]) {
continue;
}
efc_type = d->efc_type[i];
efc_id = d->efc_id[i];
// ==== fast handling of special cases
// joint friction
if (efc_type == mjCNSTR_FRICTION_DOF) {
tree1 = m->dof_treeid[efc_id];
nedge = addEdge(edge, nedge, tree1, tree1, nedge_max);
continue;
}
// joint limit
if (efc_type == mjCNSTR_LIMIT_JOINT) {
tree1 = m->dof_treeid[m->jnt_dofadr[efc_id]];
nedge = addEdge(edge, nedge, tree1, tree1, nedge_max);
continue;
}
// contact
if (efc_type == mjCNSTR_CONTACT_FRICTIONLESS ||
efc_type == mjCNSTR_CONTACT_PYRAMIDAL ||
efc_type == mjCNSTR_CONTACT_ELLIPTIC) {
tree1 = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom1]];
tree2 = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom2]];
nedge = addEdge(edge, nedge, tree1, tree2, nedge_max);
continue;
}
// connect or weld constraints
if (efc_type == mjCNSTR_EQUALITY) {
mjtEq eq_type = m->eq_type[efc_id];
if (eq_type == mjEQ_CONNECT || eq_type == mjEQ_WELD) {
tree1 = m->body_treeid[m->eq_obj1id[efc_id]];
tree2 = m->body_treeid[m->eq_obj2id[efc_id]];
nedge = addEdge(edge, nedge, tree1, tree2, nedge_max);
continue;
}
}
// ==== generic case: scan Jacobian
int index = 0;
tree1 = treeNext(m, d, -1, i, &index);
tree2 = treeNext(m, d, tree1, i, &index);
if (tree2 == -1) {
// 1 tree found: add self-edge
nedge = addEdge(edge, nedge, tree1, tree1, nedge_max);
} else {
// 2 trees found: add edge, keep scanning and adding until no more trees
nedge = addEdge(edge, nedge, tree1, tree2, nedge_max);
int tree3 = treeNext(m, d, tree2, i, &index);
while (tree3 > -1 && tree3 != tree2) {
tree1 = tree2;
tree2 = tree3;
nedge = addEdge(edge, nedge, tree1, tree2, nedge_max);
tree3 = treeNext(m, d, tree2, i, &index);
}
}
}
return nedge;
}
// discover islands:
// nisland, island_dofadr, dof_island, dof_islandnext, island_efcadr, efc_island, efc_islandnext
void mj_island(const mjModel* m, mjData* d) {
int nv = m->nv, nefc = d->nefc, ntree=m->ntree;
// no constraints: quick return
if (!nefc) {
d->nisland = 0;
return;
}
mjMARKSTACK;
// allocate edge array
int nedge_max = countMaxEdge(m, d);
int* edge = mj_stackAllocInt(d, 2*nedge_max);
// find tree-tree edges
int nedge = findEdges(m, d, edge, nedge_max);
// TODO: b/295296178 - don't add flipped edges in findEdges, symmetrize in mj_edge2sparse instead
// construct adjacency matrix from edges
int* rownnz = mj_stackAllocInt(d, ntree);
int* rowadr = mj_stackAllocInt(d, ntree);
int* colind = mj_stackAllocInt(d, nedge);
int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, nedge, ntree);
// discover islands
int* tree_island = mj_stackAllocInt(d, ntree); // id of island assigned to tree
int* stack = mj_stackAllocInt(d, nnz);
d->nisland = mj_floodFill(tree_island, ntree, rownnz, rowadr, colind, stack);
// ========== begin arena allocation of MJDATA_ARENA_POINTERS_ISLAND
#undef MJ_M
#define MJ_M(n) m->n
#undef MJ_D
#define MJ_D(n) d->n
size_t parena_old = d->parena;
#define X(type, name, nr, nc) \
d->name = mj_arenaAlloc(d, sizeof(type) * (nr) * (nc), _Alignof(type)); \
if (!d->name) { \
mj_warning(d, mjWARN_CNSTRFULL, d->nstack * sizeof(mjtNum)); \
clearIsland(d, parena_old); \
mjFREESTACK; \
return; \
}
MJDATA_ARENA_POINTERS_ISLAND
#undef X
#undef MJ_M
#define MJ_M(n) n
#undef MJ_D
#define MJ_D(n) n
// ========== end arena allocation
// prepare island_last: id of last element in each island
int* island_last = mj_stackAllocInt(d, d->nisland);
for (int i=0; i < d->nisland; i++) {
island_last[i] = -1;
}
// compute island_dofadr, dof_island, dof_islandnext
int nisland_found = 0;
for (int i=0; i < nv; i++) {
// dof_island
int island = tree_island[m->dof_treeid[i]];;
d->dof_island[i] = island;
// island_dofadr, dof_islandnext
if (island == -1) {
// dof is not in any island (unconstrained)
d->dof_islandnext[i] = -1;
continue;
} else {
int last = island_last[island];
if (last == -1) {
// first dof: set island_dofadr, increment nisland_found
d->island_dofadr[island] = i;
nisland_found++;
} else {
// subsequent dof: point last dof to i
d->dof_islandnext[last] = i;
}
island_last[island] = i;
}
}
// sanity check, SHOULD NOT OCCUR
if (nisland_found != d->nisland) {
mjERROR("not all islands assigned to dofs");
}
// finalize dof_islandnext: mark last dof in each island with -1
for (int i=0; i < d->nisland; i++) {
d->dof_islandnext[island_last[i]] = -1;
}
// reset island_last
for (int i=0; i < d->nisland; i++) {
island_last[i] = -1;
}
// compute island_efcadr, efc_island, efc_islandnext
nisland_found = 0;
for (int i=0; i < nefc; i++) {
// efc_island
int island = tree_island[treeNext(m, d, -1, i, NULL)];
d->efc_island[i] = island;
// island_efcadr, efc_islandnext
if (island == -1) {
mjERROR("constraint %d not in any island", i); // SHOULD NOT OCCUR
} else {
int last = island_last[island];
if (last == -1) {
// first constraint: set island_efcadr, increment nisland_found
d->island_efcadr[island] = i;
nisland_found++;
} else {
// subsequent constraint: point last constraint to i
d->efc_islandnext[last] = i;
}
island_last[island] = i;
}
}
// sanity check, SHOULD NOT OCCUR
if (nisland_found != d->nisland) {
mjERROR("not all islands assigned to constraints");
}
// finalize efc_islandnext: mark last constraint in each island with -1
for (int i=0; i < d->nisland; i++) {
d->efc_islandnext[island_last[i]] = -1;
}
mjFREESTACK;
}
src/engine/engine_island.h
+46
View File
@@ -0,0 +1,46 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MUJOCO_SRC_ENGINE_ENGINE_ISLAND_H_
#define MUJOCO_SRC_ENGINE_ENGINE_ISLAND_H_
#include <mujoco/mjdata.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#ifdef __cplusplus
extern "C" {
#endif
//-------------------------- utility functions -----------------------------------------------------
// construct sparse matrix from non-unique, unsorted edge array, return number of nonzeros
MJAPI int mj_edge2Sparse(int* rownnz, int* rowadr, int* colind, int* edge, int ne, int nr);
// find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix
MJAPI int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
int* scratch);
//-------------------------- top-level API for island construction ---------------------------------
// discover islands:
// nisland, island_dofadr, dof_island, dof_islandnext, island_efcadr, efc_island, efc_islandnext
MJAPI void mj_island(const mjModel* m, mjData* d);
#ifdef __cplusplus
}
#endif
#endif // MUJOCO_SRC_ENGINE_ENGINE_ISLAND_H_
src/engine/engine_print.c
+42 -2
View File
@@ -507,10 +507,11 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
fprintf(fp, " %s\n", m->names + m->name_tendonadr[i]);
object_class = &m->ntendon;
MJMODEL_POINTERS
fprintf(fp, " path \n");
fprintf(fp, " path\n");
fprintf(fp, " type objid prm\n");
for (int j=0; j < m->tendon_num[i]; j++) {
int k = m->tendon_adr[i]+j;
fprintf(fp, " %d %d ", m->wrap_type[k], m->wrap_objid[k]);
fprintf(fp, " %d %d ", m->wrap_type[k], m->wrap_objid[k]);
fprintf(fp, float_format, m->wrap_prm[k]);
fprintf(fp, "\n");
}
@@ -1052,6 +1053,45 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArray("CFRC_INT", m->nbody, 6, d->cfrc_int, fp, float_format);
printArray("CFRC_EXT", m->nbody, 6, d->cfrc_ext, fp, float_format);
if (d->nisland) {
fprintf(fp, NAME_FORMAT, "ISLAND_DOFADR");
for (int i = 0; i < d->nisland; i++) {
fprintf(fp, " %d", d->island_dofadr[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "ISLAND_EFCADR");
for (int i = 0; i < d->nisland; i++) {
fprintf(fp, " %d", d->island_efcadr[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "DOF_ISLAND");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->dof_island[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "DOF_ISLANDNEXT");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->dof_islandnext[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "EFC_ISLAND");
for (int i = 0; i < d->nefc; i++) {
fprintf(fp, " %d", d->efc_island[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "EFC_ISLANDNEXT");
for (int i = 0; i < d->nefc; i++) {
fprintf(fp, " %d", d->efc_islandnext[i]);
}
fprintf(fp, "\n\n");
}
#ifdef MEMORY_SANITIZER
// restore poisoned status
__msan_copy_shadow(d->buffer, shadow, d->nbuffer);
src/engine/engine_support.c
+2 -1
View File
@@ -66,7 +66,8 @@ const char* mjENABLESTRING[mjNENABLE] = {
"Energy",
"Fwdinv",
"Sensornoise",
"MultiCCD"
"MultiCCD",
"Island"
};
src/engine/engine_vis_init.c
+3 -1
View File
@@ -47,7 +47,8 @@ const char* mjLABELSTRING[mjNLABEL] = {
"Selection",
"SelPoint",
"Contact",
"ContactForce"
"ContactForce",
"Island"
};
@@ -81,6 +82,7 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
{"Pertur&b Force", "0", "B"},
{"Perturb &Object", "1", "O"},
{"&Contact Point", "0", "C"},
{"Island", "1", ""}, // TODO(b/295296178): turn off after islands are on by default.
{"Contact &Force", "0", "F"},
{"Contact S&plit", "0", "P"},
{"&Transparent", "0", "T"},
src/engine/engine_vis_state.c
+16
View File
@@ -173,6 +173,7 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData*
memcpy(d->warning, scnstate->data.warning, sizeof(d->warning));
d->nefc = scnstate->data.nefc;
d->ncon = scnstate->data.ncon;
d->nisland = scnstate->data.nisland;
d->time = scnstate->data.time;
#define X(dtype, var, dim0, dim1)
@@ -183,6 +184,12 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData*
d->contact = scnstate->data.contact;
d->efc_force = scnstate->data.efc_force;
if (d->nisland) {
d->island_dofadr = scnstate->data.island_dofadr;
d->dof_island = scnstate->data.dof_island;
d->efc_island = scnstate->data.efc_island;
}
}
}
@@ -317,6 +324,15 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt,
efc_address += dim;
}
}
// Copy island data.
scnstate->data.nisland = d->nisland;
if (d->nisland) {
memcpy(scnstate->data.island_dofadr, d->island_dofadr, sizeof(int) * d->nisland);
memcpy(scnstate->data.dof_island, d->dof_island, sizeof(int) * m->nv);
memcpy(scnstate->data.efc_island, d->efc_island, sizeof(int) * d->nefc);
}
}
src/engine/engine_vis_visualize.c
+97 -15
View File
@@ -84,7 +84,13 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) {
// advance counter
#define FINISH { scn->ngeom++; }
// assign pseudo-random rgba to constraint island using Halton sequence
static void islandColor(float rgba[4], int islanddofadr) {
rgba[0] = 0.1f + 0.8f*mju_Halton(islanddofadr + 1, 2);
rgba[1] = 0.1f + 0.8f*mju_Halton(islanddofadr + 1, 3);
rgba[2] = 0.1f + 0.8f*mju_Halton(islanddofadr + 1, 5);
rgba[3] = 1;
}
// add contact-related geoms in mjvObject
static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
@@ -102,7 +108,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
return;
}
// loop over contacts included in impulse solver
// loop over contacts
for (int i=0; i < d->ncon; i++) {
// get pointer
con = d->contact + i;
@@ -121,11 +127,21 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
mju_n2f(thisgeom->pos, con->pos, 3);
mju_n2f(thisgeom->mat, mat, 9);
// different colors for included and excluded contacts
if (d->contact[i].efc_address >= 0) {
f2f(thisgeom->rgba, m->vis.rgba.contactpoint, 4);
} else {
f2f(thisgeom->rgba, m->vis.rgba.contactgap, 4);
int efc_adr = d->contact[i].efc_address;
// override standard colors if visualizing islands
if (vopt->flags[mjVIS_ISLAND] && d->nisland && efc_adr >= 0) {
// set color using island's first dof
islandColor(thisgeom->rgba, d->island_dofadr[d->efc_island[efc_adr]]);
}
// otherwise regular colors (different for included and excluded contacts)
else {
if (efc_adr >= 0) {
f2f(thisgeom->rgba, m->vis.rgba.contactpoint, 4);
} else {
f2f(thisgeom->rgba, m->vis.rgba.contactgap, 4);
}
}
// label contacting geom names or ids
@@ -1095,6 +1111,28 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}
// island labels
objtype = mjOBJ_UNKNOWN;
category = mjCAT_DECOR;
if ((category & catmask) && (vopt->label == mjLABEL_ISLAND) && d->nisland) {
for (int i=1; i < m->nbody; i++) {
int weld_id = m->body_weldid[i];
if (m->body_dofnum[weld_id]) {
int islandid = d->dof_island[m->body_dofadr[weld_id]];
if (islandid > -1) {
START
thisgeom->type = mjGEOM_LABEL;
mju_n2f(thisgeom->pos, d->xipos+3*i, 3);
mju_n2f(thisgeom->mat, d->ximat+9*i, 9);
mjSNPRINTF(thisgeom->label, "%d", islandid);
FINISH
}
}
}
}
// geom
int planeid = -1;
for (int i=0; i < m->ngeom; i++) {
@@ -1126,8 +1164,23 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// copy rbound from model
thisgeom->modelrbound = (float)m->geom_rbound[i];
// set material properties
setMaterial(m, thisgeom, m->geom_matid[i], m->geom_rgba+4*i, vopt->flags);
// set material properties, override if visualizing islands
float* rgba = m->geom_rgba+4*i;
float rgba_island[4] = {.5, .5, .5, 1};
int geom_matid = m->geom_matid[i];
if (vopt->flags[mjVIS_ISLAND] && d->nisland) {
geom_matid = -1;
rgba = rgba_island;
int weld_id = m->body_weldid[m->geom_bodyid[i]];
if (m->body_dofnum[weld_id]) {
int island = d->dof_island[m->body_dofadr[weld_id]];
if (island > -1) {
// color using island's first dof
islandColor(rgba_island, d->island_dofadr[island]);
}
}
}
setMaterial(m, thisgeom, geom_matid, rgba, vopt->flags);
// set texcoord
if (m->geom_type[i] == mjGEOM_MESH &&
@@ -1474,17 +1527,21 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_TENDON] && (category & catmask)) {
for (int i=0; i < m->ntendon; i++) {
if (vopt->tendongroup[mjMAX(0, mjMIN(mjNGROUP-1, m->tendon_group[i]))]) {
// stiff tendon has a deadband spring
// tendon has a deadband spring
int limitedspring =
m->tendon_stiffness[i] > 0 && // positive stiffness
m->tendon_lengthspring[2*i] == 0 && // range lower-bound is 0
m->tendon_lengthspring[2*i+1] > 0; // range upper-bound is positive
// non-stiff tendon has a length constraint
// tendon has a simple length constraint, but is currently not limited
mjtNum ten_length = d->ten_length[i];
mjtNum lower = m->tendon_range[2*i];
mjtNum upper = m->tendon_range[2*i + 1];
int limitedconstraint =
m->tendon_stiffness[i] == 0 && // zero stiffness
m->tendon_limited[i] == 1 && // limited length range
m->tendon_range[2*i] == 0; // range lower-bound is 0
lower == 0 && // range lower-bound is 0
ten_length < upper; // current length is smaller than upper bound
// conditions for drawing a catenary
int draw_catenary =
@@ -1511,8 +1568,33 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// construct geom
mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], d->wrap_xpos+3*j, d->wrap_xpos+3*j+3);
// set material if given
setMaterial(m, thisgeom, m->tendon_matid[i], m->tendon_rgba+4*i, vopt->flags);
// set material properties, override if visualizing islands
float* rgba = m->tendon_rgba+4*i;
float rgba_island[4] = {.5, .5, .5, 1};
int tendon_matid = m->tendon_matid[i];
if (vopt->flags[mjVIS_ISLAND] && d->nisland) {
tendon_matid = -1;
rgba = rgba_island;
int frictional = m->tendon_frictionloss[i] > 0;
int limited = m->tendon_limited[i] && (ten_length <= lower || ten_length >= upper);
if (frictional || limited) {
// search for tendon's island
int island = -1;
for (int k=0; k < d->nefc; k++) {
int istendon = d->efc_type[k] == mjCNSTR_FRICTION_TENDON ||
d->efc_type[k] == mjCNSTR_LIMIT_TENDON;
if (istendon && d->efc_id[k] == i) {
island = d->efc_island[k];
break;
}
}
if (island > -1) {
// set color using island's first dof
islandColor(rgba_island, d->island_dofadr[island]);
}
}
}
setMaterial(m, thisgeom, tendon_matid, rgba, vopt->flags);
// vopt->label: only the first segment
if (vopt->label == mjLABEL_TENDON && j == d->ten_wrapadr[i]) {
@@ -1546,7 +1628,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
for (int j=0; j < npoints-1; j++) {
START
sz[0] = m->tendon_width[i];
sz[0] = m->tendon_width[i];
// construct geom
mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], catenary+3*j, catenary+3*j+3);