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

Allow connect constraints to be specified using sites.

Browse Source 
PiperOrigin-RevId: 668925419
Change-Id: I10cb71e6706e617c830cd90d6d0836b94a5a8cfa
This commit is contained in:
Yuval Tassa
2024-08-29 07:09:52 -07:00
committed by Copybara-Service
parent 646455406c
commit a1036b86d3
19 changed files with 325 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_visualize.c
+12 -5
View File
@@ -2021,13 +2021,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) {
// connect or weld
for (int i=0; i < m->neq; i++) {
if (d->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) {
int is_weld = m->eq_type[i] == mjEQ_WELD;
int is_connect = m->eq_type[i] == mjEQ_CONNECT;
if (d->eq_active[i] && (is_connect || is_weld)) {
// compute endpoints in global coordinates
int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD));
mju_addTo3(vec, d->xpos+3*j);
mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_CONNECT));
mju_addTo3(end, d->xpos+3*k);
if (is_connect && m->eq_objtype[i] == mjOBJ_SITE) {
mju_copy3(vec, d->site_xpos+3*j);
mju_copy3(end, d->site_xpos+3*k);
} else {
mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*is_weld);
mju_addTo3(vec, d->xpos+3*j);
mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*is_connect);
mju_addTo3(end, d->xpos+3*k);
}
// construct geom
sz[0] = scl * m->vis.scale.constraint;