Alessio
2a3554c8a3
Contact of a flex with `passive` collisions enabled was applied as an explicit spring of fixed stiffness 1e4, which the timestep bounds: any stiffness worth having oscillates faster than the step can resolve, so the force was too soft to keep sheets apart and interpenetration was routine. Carry its curvature in the effective metric M + K instead, alongside the flex's own stretch and bending stiffness. The contact block k*J^T*J is appended to the per-vertex candidate list already assembled for the flex stencils, so it costs additional entries in an existing matrix rather than a new one, and the accompanying shift -h*K*v is what damps the stiff modes. At a 2 ms timestep this holds roughly 50x the stiffness an explicit force of the same step could. With the timestep no longer setting the bound, the stiffness is chosen as a natural frequency scaled by the participating vertex mass rather than left at a fixed 1e4, so one value suits models of any scale. Passive handling is scoped to contacts whose every dof is a flex vertex carried by the metric: flex against flex, flex against itself, and flex against static geometry, which contributes no dofs of its own. For those the Hessian is assembled in full. Contact with a body that can move would have that body's dofs dropped from it, and is left on the constraint solver. The feature now requires an integrator whose constraint solve runs in that metric, and is rejected with an error otherwise. Add model/flex/drape.xml as the example model, replacing sphere_passive, whose contacts no longer demonstrated the feature.
1726 lines
55 KiB
C
1726 lines
55 KiB
C
// Copyright 2021 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_setconst.h"
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#include <mujoco/mjdata.h>
|
|
#include <mujoco/mjmacro.h>
|
|
#include <mujoco/mjmodel.h>
|
|
#include <mujoco/mjsan.h> // IWYU pragma: keep
|
|
#include "engine/engine_core_smooth.h"
|
|
#include "engine/engine_core_util.h"
|
|
#include "engine/engine_derivative.h"
|
|
#include "engine/engine_forward.h"
|
|
#include "engine/engine_io.h"
|
|
#include "engine/engine_memory.h"
|
|
#include "engine/engine_util_blas.h"
|
|
#include "engine/engine_util_errmem.h"
|
|
#include "engine/engine_util_solve.h"
|
|
#include "engine/engine_util_misc.h"
|
|
#include "engine/engine_util_sparse.h"
|
|
#include "engine/engine_util_spatial.h"
|
|
|
|
|
|
// compute dof_M0 via composite rigid body algorithm
|
|
static void mj_setM0(mjModel* m, mjData* d) {
|
|
mjtNum buf[6];
|
|
mjtNum* crb = d->crb;
|
|
int last_body = m->nbody - 1, nv = m->nv;
|
|
|
|
// copy cinert into crb
|
|
mju_copy(crb, d->cinert, 10*m->nbody);
|
|
|
|
// backward pass over bodies, accumulate composite inertias
|
|
for (int i=last_body; i > 0; i--) {
|
|
if (m->body_parentid[i] > 0) {
|
|
mju_addTo(crb+10*m->body_parentid[i], crb+10*i, 10);
|
|
}
|
|
}
|
|
|
|
for (int i=0; i < nv; i++) {
|
|
// precomute buf = crb_body_i * cdof_i
|
|
mju_mulInertVec(buf, crb+10*m->dof_bodyid[i], d->cdof+6*i);
|
|
|
|
// dof_M0(i) = armature inertia + cdof_i * (crb_body_i * cdof_i)
|
|
mjtNum armature = m->dof_armature[i] + mj_actuatorArmature(m, mjOBJ_JOINT, m->dof_jntid[i]);
|
|
m->dof_M0[i] = armature + mju_dot(d->cdof+6*i, buf, 6);
|
|
}
|
|
}
|
|
|
|
|
|
// helper function to get the tree id of a wrap object
|
|
static int GetWrapBodyTreeId(const mjModel* m, int wrap_index) {
|
|
int bodyid = -1;
|
|
int objid = m->wrap_objid[wrap_index];
|
|
switch ((mjtWrap)m->wrap_type[wrap_index]) {
|
|
case mjWRAP_JOINT:
|
|
bodyid = m->jnt_bodyid[objid];
|
|
break;
|
|
case mjWRAP_SITE:
|
|
bodyid = m->site_bodyid[objid];
|
|
break;
|
|
case mjWRAP_SPHERE:
|
|
case mjWRAP_CYLINDER:
|
|
bodyid = m->geom_bodyid[objid];
|
|
break;
|
|
case mjWRAP_PULLEY:
|
|
case mjWRAP_NONE:
|
|
break;
|
|
}
|
|
return (bodyid != -1) ? m->body_treeid[bodyid] : -1;
|
|
}
|
|
|
|
|
|
// tolerance for frame comparison, must match compiler's kFrameEps
|
|
static const mjtNum kSameFrameEps = 1e-6;
|
|
|
|
|
|
// return 1 if all 3 elements are near zero
|
|
static int isNullVec3(const mjtNum v[3]) {
|
|
return mju_abs(v[0]) < kSameFrameEps &&
|
|
mju_abs(v[1]) < kSameFrameEps &&
|
|
mju_abs(v[2]) < kSameFrameEps;
|
|
}
|
|
|
|
|
|
// return 1 if quaternion is near identity (1,0,0,0), accounting for double-cover
|
|
static int isNullQuat(const mjtNum q[4]) {
|
|
int plus = mju_abs(q[0] - 1) < kSameFrameEps &&
|
|
mju_abs(q[1]) < kSameFrameEps &&
|
|
mju_abs(q[2]) < kSameFrameEps &&
|
|
mju_abs(q[3]) < kSameFrameEps;
|
|
int minus = mju_abs(q[0] + 1) < kSameFrameEps &&
|
|
mju_abs(q[1]) < kSameFrameEps &&
|
|
mju_abs(q[2]) < kSameFrameEps &&
|
|
mju_abs(q[3]) < kSameFrameEps;
|
|
return plus || minus;
|
|
}
|
|
|
|
|
|
// return 1 if two quaternions are near equal, accounting for double-cover
|
|
static int isSameQuat(const mjtNum q1[4], const mjtNum q2[4]) {
|
|
int plus = mju_abs(q1[0] - q2[0]) < kSameFrameEps &&
|
|
mju_abs(q1[1] - q2[1]) < kSameFrameEps &&
|
|
mju_abs(q1[2] - q2[2]) < kSameFrameEps &&
|
|
mju_abs(q1[3] - q2[3]) < kSameFrameEps;
|
|
int minus = mju_abs(q1[0] + q2[0]) < kSameFrameEps &&
|
|
mju_abs(q1[1] + q2[1]) < kSameFrameEps &&
|
|
mju_abs(q1[2] + q2[2]) < kSameFrameEps &&
|
|
mju_abs(q1[3] + q2[3]) < kSameFrameEps;
|
|
return plus || minus;
|
|
}
|
|
|
|
|
|
// return 1 if two 3-vectors are near equal
|
|
static int isSameVec3(const mjtNum v1[3], const mjtNum v2[3]) {
|
|
return mju_abs(v1[0] - v2[0]) < kSameFrameEps &&
|
|
mju_abs(v1[1] - v2[1]) < kSameFrameEps &&
|
|
mju_abs(v1[2] - v2[2]) < kSameFrameEps;
|
|
}
|
|
|
|
|
|
// recompute body_sameframe, geom_sameframe, site_sameframe from model geometry
|
|
static void setSameframe(mjModel* m) {
|
|
// body_sameframe: compare body inertial frame to body frame
|
|
for (int i=1; i < m->nbody; i++) {
|
|
mjtNum* ipos = m->body_ipos+3*i;
|
|
mjtNum* iquat = m->body_iquat+4*i;
|
|
if (isNullVec3(ipos) && isNullQuat(iquat)) {
|
|
m->body_sameframe[i] = mjSAMEFRAME_BODY;
|
|
} else if (isNullQuat(iquat)) {
|
|
m->body_sameframe[i] = mjSAMEFRAME_BODYROT;
|
|
} else {
|
|
m->body_sameframe[i] = mjSAMEFRAME_NONE;
|
|
}
|
|
}
|
|
|
|
// geom_sameframe: compare geom frame to body and inertial frames
|
|
for (int i=0; i < m->ngeom; i++) {
|
|
int b = m->geom_bodyid[i];
|
|
mjtNum* gpos = m->geom_pos+3*i;
|
|
mjtNum* gquat = m->geom_quat+4*i;
|
|
mjtNum* ipos = m->body_ipos+3*b;
|
|
mjtNum* iquat = m->body_iquat+4*b;
|
|
if (isNullVec3(gpos) && isNullQuat(gquat)) {
|
|
m->geom_sameframe[i] = mjSAMEFRAME_BODY;
|
|
} else if (isNullQuat(gquat)) {
|
|
m->geom_sameframe[i] = mjSAMEFRAME_BODYROT;
|
|
} else if (isSameVec3(gpos, ipos) && isSameQuat(gquat, iquat)) {
|
|
m->geom_sameframe[i] = mjSAMEFRAME_INERTIA;
|
|
} else if (isSameQuat(gquat, iquat)) {
|
|
m->geom_sameframe[i] = mjSAMEFRAME_INERTIAROT;
|
|
} else {
|
|
m->geom_sameframe[i] = mjSAMEFRAME_NONE;
|
|
}
|
|
}
|
|
|
|
// site_sameframe: compare site frame to body and inertial frames
|
|
for (int i=0; i < m->nsite; i++) {
|
|
int b = m->site_bodyid[i];
|
|
mjtNum* spos = m->site_pos+3*i;
|
|
mjtNum* squat = m->site_quat+4*i;
|
|
mjtNum* ipos = m->body_ipos+3*b;
|
|
mjtNum* iquat = m->body_iquat+4*b;
|
|
if (isNullVec3(spos) && isNullQuat(squat)) {
|
|
m->site_sameframe[i] = mjSAMEFRAME_BODY;
|
|
} else if (isNullQuat(squat)) {
|
|
m->site_sameframe[i] = mjSAMEFRAME_BODYROT;
|
|
} else if (isSameVec3(spos, ipos) && isSameQuat(squat, iquat)) {
|
|
m->site_sameframe[i] = mjSAMEFRAME_INERTIA;
|
|
} else if (isSameQuat(squat, iquat)) {
|
|
m->site_sameframe[i] = mjSAMEFRAME_INERTIAROT;
|
|
} else {
|
|
m->site_sameframe[i] = mjSAMEFRAME_NONE;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// set fixed quantities (do not depend on qpos0)
|
|
static void setFixed(mjModel* m, mjData* d) {
|
|
mj_markStack(d);
|
|
|
|
// ----- general
|
|
|
|
// compute subtreemass
|
|
for (int i=0; i < m->nbody; i++) {
|
|
m->body_subtreemass[i] = m->body_mass[i];
|
|
}
|
|
for (int i=m->nbody-1; i > 0; i--) {
|
|
m->body_subtreemass[m->body_parentid[i]] += m->body_subtreemass[i];
|
|
}
|
|
|
|
// compute ngravcomp: number of bodies with gravity compensation
|
|
int ngravcomp = 0;
|
|
for (int i=0; i < m->nbody; i++) {
|
|
ngravcomp += (m->body_gravcomp[i] > 0);
|
|
}
|
|
m->ngravcomp = ngravcomp;
|
|
m->flg_gravcomp = (ngravcomp > 0);
|
|
|
|
// compute flg_surfacevel: whether any geom has nonzero surfacevel
|
|
mjtBool flg_surfacevel = 0;
|
|
for (int i=0; i < m->ngeom; i++) {
|
|
const mjtNum* sv = m->geom_surfacevel + 6*i;
|
|
if (sv[0] || sv[1] || sv[2] || sv[3] || sv[4] || sv[5]) {
|
|
flg_surfacevel = 1;
|
|
break;
|
|
}
|
|
}
|
|
m->flg_surfacevel = flg_surfacevel;
|
|
|
|
// compute flg_adhesion: whether any geom or pair has nonzero adhesion
|
|
mjtBool flg_adhesion = 0;
|
|
for (int i=0; i < m->ngeom; i++) {
|
|
if (m->geom_adhesion[i]) {
|
|
flg_adhesion = 1;
|
|
break;
|
|
}
|
|
}
|
|
for (int i=0; i < m->npair && !flg_adhesion; i++) {
|
|
if (m->pair_adhesion[i]) {
|
|
flg_adhesion = 1;
|
|
}
|
|
}
|
|
m->flg_adhesion = flg_adhesion;
|
|
|
|
// set jnt_actuatorid and tendon_actuatorid
|
|
mju_fillInt(m->jnt_actuatorid, -1, m->njnt);
|
|
mju_fillInt(m->tendon_actuatorid, -1, m->ntendon);
|
|
for (int i=0; i < m->nactuator; i++) {
|
|
// skip actuator with no damping and no armature
|
|
if (m->actuator_damping[i] == 0 &&
|
|
mju_isZero(m->actuator_dampingpoly+mjNPOLY*i, mjNPOLY) &&
|
|
m->actuator_armature[i] == 0) {
|
|
continue;
|
|
}
|
|
|
|
// joint or jointinparent transmission
|
|
if (m->actuator_trntype[i] == mjTRN_JOINT ||
|
|
m->actuator_trntype[i] == mjTRN_JOINTINPARENT) {
|
|
int jntid = m->actuator_trnid[2*i];
|
|
|
|
// first actuator: set id to i
|
|
if (m->jnt_actuatorid[jntid] == -1) {
|
|
m->jnt_actuatorid[jntid] = i;
|
|
}
|
|
|
|
// multiple actuators acting on single transmission: use -2 sentinel
|
|
else {
|
|
m->jnt_actuatorid[jntid] = -2;
|
|
}
|
|
}
|
|
|
|
// tendon transmission
|
|
else if (m->actuator_trntype[i] == mjTRN_TENDON) {
|
|
int tenid = m->actuator_trnid[2*i];
|
|
|
|
// first actuator: set id to i
|
|
if (m->tendon_actuatorid[tenid] == -1) {
|
|
m->tendon_actuatorid[tenid] = i;
|
|
}
|
|
|
|
// multiple actuators acting on single transmission: use -2 sentinel
|
|
else {
|
|
m->tendon_actuatorid[tenid] = -2;
|
|
}
|
|
}
|
|
}
|
|
|
|
// ----- tree related (body_treeid and dof_treeid already computed)
|
|
|
|
// compute body_treeid
|
|
for (int i=0; i < m->nbody; i++) {
|
|
int weldid = m->body_weldid[i];
|
|
if (m->body_dofnum[weldid]) {
|
|
m->body_treeid[i] = m->dof_treeid[m->body_dofadr[weldid]];
|
|
} else {
|
|
m->body_treeid[i] = -1;
|
|
}
|
|
}
|
|
|
|
// compute tree_bodyadr, tree_bodynum
|
|
mju_zeroInt(m->tree_bodynum, m->ntree);
|
|
int tree_current = -1;
|
|
for (int i=1; i < m->nbody; i++) {
|
|
int treeid = m->body_treeid[i];
|
|
if (treeid != -1) {
|
|
if (treeid > tree_current) {
|
|
m->tree_bodyadr[++tree_current] = i;
|
|
}
|
|
m->tree_bodynum[tree_current]++;
|
|
}
|
|
}
|
|
|
|
// compute tree_dofadr, tree_dofnum
|
|
mju_zeroInt(m->tree_dofnum, m->ntree);
|
|
tree_current = -1;
|
|
for (int i=0; i < m->nv; i++) {
|
|
if (m->dof_treeid[i] > tree_current) {
|
|
m->tree_dofadr[++tree_current] = i;
|
|
}
|
|
m->tree_dofnum[tree_current]++;
|
|
}
|
|
|
|
// compute tendon_treeid, tendon_treenum
|
|
int* tree_marker = mjSTACKALLOC(d, m->ntree, int); // 1 if tree has been visited, 0 otherwise
|
|
for (int i = 0; i < m->ntendon; i++) {
|
|
mju_zeroInt(tree_marker, m->ntree);
|
|
m->tendon_treenum[i] = 0;
|
|
m->tendon_treeid[2*i] = -1;
|
|
m->tendon_treeid[2*i+1] = -1;
|
|
|
|
for (int j = m->tendon_adr[i]; j < m->tendon_adr[i] + m->tendon_num[i]; j++) {
|
|
int wrap_treeid = GetWrapBodyTreeId(m, j);
|
|
if (wrap_treeid != -1 && !tree_marker[wrap_treeid]) {
|
|
tree_marker[wrap_treeid] = 1;
|
|
if (m->tendon_treenum[i] == 0) {
|
|
m->tendon_treeid[2*i] = wrap_treeid;
|
|
} else if (m->tendon_treenum[i] == 1) {
|
|
m->tendon_treeid[2*i+1] = wrap_treeid;
|
|
}
|
|
m->tendon_treenum[i]++;
|
|
}
|
|
}
|
|
}
|
|
|
|
// ----- apply compiler AUTO tree sleep policy
|
|
|
|
// actuators: trees with any actuated joint, site, body, or tendon do not auto-sleep
|
|
for (int i=0; i < m->nactuator; i++) {
|
|
int bodyid = -1;
|
|
int tid = m->actuator_trnid[2*i];
|
|
switch ((mjtTrn)m->actuator_trntype[i]) {
|
|
case mjTRN_JOINT:
|
|
case mjTRN_JOINTINPARENT:
|
|
bodyid = m->jnt_bodyid[tid];
|
|
break;
|
|
case mjTRN_SO3:
|
|
bodyid = m->actuator_trnid[2*i+1] >= 0 ? m->site_bodyid[tid] : m->jnt_bodyid[tid];
|
|
break;
|
|
case mjTRN_SITE:
|
|
case mjTRN_SLIDERCRANK:
|
|
bodyid = m->site_bodyid[tid];
|
|
break;
|
|
case mjTRN_BODY:
|
|
bodyid = tid;
|
|
break;
|
|
case mjTRN_TENDON:
|
|
// wake all trees connected by this actuated tendon
|
|
for (int j = m->tendon_adr[tid]; j < m->tendon_adr[tid] + m->tendon_num[tid]; j++) {
|
|
int treeid = GetWrapBodyTreeId(m, j);
|
|
if (treeid != -1 && m->tree_sleep_policy[treeid] == mjSLEEP_AUTO) {
|
|
m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER;
|
|
}
|
|
}
|
|
continue; // next actuator
|
|
case mjTRN_UNDEFINED:
|
|
continue; // next actuator
|
|
}
|
|
|
|
// wake tree containing bodyid, if any
|
|
if (bodyid != -1) {
|
|
int treeid = m->body_treeid[bodyid];
|
|
if (treeid != -1 && m->tree_sleep_policy[treeid] == mjSLEEP_AUTO) {
|
|
m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER;
|
|
}
|
|
}
|
|
}
|
|
|
|
// trees with inter-tree tendons that have non-zero stiffness or damping do not auto-sleep
|
|
// if the tendon spans more than 2 trees.
|
|
for (int i=0; i < m->ntendon; i++) {
|
|
int treenum = m->tendon_treenum[i];
|
|
|
|
// tendon spans 1 or 0 trees: skip
|
|
if (treenum < 2) {
|
|
continue;
|
|
}
|
|
|
|
// tendon spans 2 trees and has no stiffness or damping: skip
|
|
if (treenum == 2 &&
|
|
m->tendon_stiffness[i] == 0 && mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY) &&
|
|
m->tendon_damping[i] == 0 && mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY) &&
|
|
m->tendon_actuatorid[i] == -1) {
|
|
continue;
|
|
}
|
|
|
|
// tendon spans two trees with stiffness or damping or more than two trees: wake all trees
|
|
mju_zeroInt(tree_marker, m->ntree);
|
|
for (int j = m->tendon_adr[i]; j < m->tendon_adr[i] + m->tendon_num[i]; j++) {
|
|
int treeid = GetWrapBodyTreeId(m, j);
|
|
|
|
// if the tree is not yet marked, mark it and wake it up
|
|
if (treeid != -1 && !tree_marker[treeid]) {
|
|
tree_marker[treeid] = 1;
|
|
int policy = m->tree_sleep_policy[treeid];
|
|
|
|
// mark tree as never sleeping
|
|
if (policy == mjSLEEP_AUTO) {
|
|
m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER;
|
|
}
|
|
|
|
// if the user marked it as sleepable, throw an error
|
|
else if (policy == mjSLEEP_ALLOWED || policy == mjSLEEP_INIT) {
|
|
mj_freeStack(d);
|
|
if (treenum > 2) {
|
|
mjERROR("tree %d connected to tendon %d which spans more than 2 trees, "
|
|
"sleeping not allowed", treeid, i);
|
|
} else {
|
|
mjERROR("tree %d connected to tendon %d with non-zero stiffness or damping, "
|
|
"sleeping not allowed", treeid, i);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// flexes: constraint-free trees are not allowed to sleep
|
|
for (int i = 0; i < m->nflex; ++i) {
|
|
// constrained flexes are allowed to sleep
|
|
if (m->flex_edgeequality[i]) {
|
|
continue;
|
|
}
|
|
|
|
// node-based flex
|
|
if (m->flex_interp[i]) {
|
|
int nodenum = m->flex_nodenum[i];
|
|
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[i];
|
|
for (int j = 0; j < nodenum; ++j) {
|
|
int treeid = m->body_treeid[bodyid[j]];
|
|
if (treeid != -1 && m->tree_sleep_policy[treeid] == mjSLEEP_AUTO) {
|
|
m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER;
|
|
}
|
|
}
|
|
}
|
|
|
|
// vertex-based flex
|
|
else {
|
|
int vertnum = m->flex_vertnum[i];
|
|
int* bodyid = m->flex_vertbodyid + m->flex_vertadr[i];
|
|
for (int j = 0; j < vertnum; ++j) {
|
|
int treeid = m->body_treeid[bodyid[j]];
|
|
if (treeid != -1 && m->tree_sleep_policy[treeid] == mjSLEEP_AUTO) {
|
|
m->tree_sleep_policy[treeid] = mjSLEEP_AUTO_NEVER;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// set remaining trees with mjSLEEP_AUTO policy to mjSLEEP_AUTO_ALLOWED
|
|
for (int i = 0; i < m->ntree; i++) {
|
|
if (m->tree_sleep_policy[i] == mjSLEEP_AUTO) {
|
|
m->tree_sleep_policy[i] = mjSLEEP_AUTO_ALLOWED;
|
|
}
|
|
}
|
|
|
|
mj_freeStack(d);
|
|
}
|
|
|
|
// compute tendon Jacobian sparsity
|
|
static void makeTendonSparse(mjModel* m) {
|
|
int ntendon = m->ntendon;
|
|
int* rownnz = m->ten_J_rownnz;
|
|
int* rowadr = m->ten_J_rowadr;
|
|
int* colind = m->ten_J_colind;
|
|
|
|
if (!ntendon) {
|
|
return;
|
|
}
|
|
|
|
// clear
|
|
mju_zeroInt(rownnz, ntendon);
|
|
mju_zeroInt(rowadr, ntendon);
|
|
|
|
// compute rownnz, rowadr, and colind for each tendon
|
|
for (int i = 0; i < ntendon; i++) {
|
|
rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0);
|
|
int adr = m->tendon_adr[i];
|
|
int num = m->tendon_num[i];
|
|
|
|
// joint tendon: each wrap object is a joint, colind is its dofadr
|
|
if (m->wrap_type[adr] == mjWRAP_JOINT) {
|
|
for (int j = 0; j < num; j++) {
|
|
colind[rowadr[i] + j] = m->jnt_dofadr[m->wrap_objid[adr + j]];
|
|
}
|
|
rownnz[i] = num;
|
|
} else {
|
|
// spatial tendon: collect used dofs from wrap object bodies
|
|
int nnz = 0;
|
|
for (int j = 0; j < num; j++) {
|
|
int type = m->wrap_type[adr + j];
|
|
|
|
// get body id from site or geom wrap object
|
|
int bodyid = -1;
|
|
if (type == mjWRAP_SITE) {
|
|
bodyid = m->site_bodyid[m->wrap_objid[adr + j]];
|
|
} else if (type == mjWRAP_SPHERE || type == mjWRAP_CYLINDER) {
|
|
bodyid = m->geom_bodyid[m->wrap_objid[adr + j]];
|
|
}
|
|
|
|
// walk up the body tree, collecting used dofs
|
|
if (bodyid > 0) {
|
|
int bid = bodyid;
|
|
while (bid > 0) {
|
|
int bdofadr = m->body_dofadr[bid];
|
|
int bdofnum = m->body_dofnum[bid];
|
|
for (int k = 0; k < bdofnum; k++) {
|
|
int dof = bdofadr + k;
|
|
|
|
// check if dof already in colind
|
|
int found = 0;
|
|
for (int l = 0; l < nnz; l++) {
|
|
if (colind[rowadr[i] + l] == dof) {
|
|
found = 1;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// append new dof
|
|
if (!found) {
|
|
colind[rowadr[i] + nnz] = dof;
|
|
nnz++;
|
|
}
|
|
}
|
|
bid = m->body_parentid[bid];
|
|
}
|
|
}
|
|
}
|
|
rownnz[i] = nnz;
|
|
}
|
|
|
|
// sort colind for this tendon
|
|
int nnz = rownnz[i];
|
|
for (int j = 0; j < nnz - 1; j++) {
|
|
for (int k = j + 1; k < nnz; k++) {
|
|
// swap out-of-order entries
|
|
if (colind[rowadr[i] + k] < colind[rowadr[i] + j]) {
|
|
int tmp = colind[rowadr[i] + j];
|
|
colind[rowadr[i] + j] = colind[rowadr[i] + k];
|
|
colind[rowadr[i] + k] = tmp;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// compute flex sparsity: flexedge_J_{rowadr,rownnz,colind} and flexvert_J_{rowadr,rownnz}
|
|
static void makeFlexSparse(mjModel* m, mjData* d) {
|
|
int nv = m->nv;
|
|
int* rowadr = m->flexedge_J_rowadr;
|
|
int* rownnz = m->flexedge_J_rownnz;
|
|
int* colind = m->flexedge_J_colind;
|
|
int* vrowadr = m->flexvert_J_rowadr;
|
|
int* vrownnz = m->flexvert_J_rownnz;
|
|
|
|
if (!m->nflex) {
|
|
return;
|
|
}
|
|
|
|
mj_markStack(d);
|
|
int* chain = mjSTACKALLOC(d, nv, int);
|
|
int* chain1 = mjSTACKALLOC(d, nv, int);
|
|
int* chain2 = mjSTACKALLOC(d, nv, int);
|
|
int* buf_ind = mjSTACKALLOC(d, nv, int);
|
|
mjtNum* dummy_pos = mjSTACKALLOC(d, 3, mjtNum);
|
|
mju_zero(dummy_pos, 3);
|
|
|
|
// clear
|
|
mju_zeroInt(rowadr, m->nflexedge);
|
|
mju_zeroInt(rownnz, m->nflexedge);
|
|
mju_zeroInt(vrowadr, 2 * m->nflexvert);
|
|
mju_zeroInt(vrowadr, 2 * m->nflexvert);
|
|
mju_zeroInt(vrownnz, 2 * m->nflexvert);
|
|
mju_zeroInt(m->flex_vertedgeadr, m->nflexvert);
|
|
mju_zeroInt(m->flex_vertedgenum, m->nflexvert);
|
|
mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge);
|
|
mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge);
|
|
mju_zero(m->flex_vertmetric, 4 * m->nflexvert);
|
|
mju_zeroInt(m->flexedge_J_colind, m->nJfe);
|
|
mju_zeroInt(m->flexvert_J_colind, 2 * m->nJfv);
|
|
int current_adj_offset = 0;
|
|
|
|
// compute lengths and Jacobians of edges
|
|
for (int f = 0; f < m->nflex; f++) {
|
|
// skip if edges cannot generate forces
|
|
if (m->flex_rigid[f] || m->flex_interp[f]) {
|
|
continue;
|
|
}
|
|
|
|
// skip Jacobian if no built-in passive force is needed
|
|
int skipjacobian = !m->flex_edgeequality[f] && !m->flex_edgedamping[f] &&
|
|
!m->flex_edgestiffness[f] && !m->flex_damping[f];
|
|
|
|
// process edges of this flex
|
|
int vbase = m->flex_vertadr[f];
|
|
int ebase = m->flex_edgeadr[f];
|
|
for (int e = 0; e < m->flex_edgenum[f]; e++) {
|
|
if (skipjacobian) {
|
|
continue;
|
|
}
|
|
|
|
// set rowadr
|
|
if (ebase + e > 0) {
|
|
rowadr[ebase + e] = rowadr[ebase + e - 1] + rownnz[ebase + e - 1];
|
|
}
|
|
|
|
int v1 = m->flex_edge[2 * (ebase + e)];
|
|
int v2 = m->flex_edge[2 * (ebase + e) + 1];
|
|
int b1 = m->flex_vertbodyid[vbase + v1];
|
|
int b2 = m->flex_vertbodyid[vbase + v2];
|
|
|
|
// get sparsity
|
|
int NV = mj_jacDifPair(m, d, chain, b1, b2, dummy_pos, dummy_pos, NULL,
|
|
NULL, NULL, NULL, NULL, NULL, /*issparse=*/1, /*skipcommon=*/0);
|
|
|
|
// copy sparsity info
|
|
rownnz[ebase + e] = NV;
|
|
mju_copyInt(colind + rowadr[ebase + e], chain, NV);
|
|
}
|
|
|
|
// if dim=2 and constraints are active we use the vertex-based constraint
|
|
if (m->flex_dim[f] == 2 && m->flex_edgeequality[f] == 2) {
|
|
int nvert = m->flex_vertnum[f];
|
|
|
|
// populate global vertex adjacency list
|
|
int* v_edge_cnt = m->flex_vertedgenum + vbase;
|
|
int* v_edge_adr = m->flex_vertedgeadr + vbase;
|
|
int* adj_edges = m->flex_vertedge; // global array
|
|
|
|
for (int e = 0; e < m->flex_edgenum[f]; ++e) {
|
|
v_edge_cnt[m->flex_edge[2 * (ebase + e) + 0]]++;
|
|
v_edge_cnt[m->flex_edge[2 * (ebase + e) + 1]]++;
|
|
}
|
|
int total_adj_edges = 0;
|
|
for (int v = 0; v < nvert; ++v) {
|
|
v_edge_adr[v] = current_adj_offset + total_adj_edges;
|
|
total_adj_edges += v_edge_cnt[v];
|
|
}
|
|
int* v_edge_fill = mjSTACKALLOC(d, nvert, int);
|
|
mju_zeroInt(v_edge_fill, nvert);
|
|
for (int e = 0; e < m->flex_edgenum[f]; ++e) {
|
|
int v1 = m->flex_edge[2 * (ebase + e) + 0];
|
|
int v2 = m->flex_edge[2 * (ebase + e) + 1];
|
|
adj_edges[v_edge_adr[v1] + v_edge_fill[v1]] = e;
|
|
v_edge_fill[v1]++;
|
|
adj_edges[v_edge_adr[v2] + v_edge_fill[v2]] = e;
|
|
v_edge_fill[v2]++;
|
|
}
|
|
|
|
// precompute metric (Binv)
|
|
for (int v = 0; v < nvert; ++v) {
|
|
mjtNum B[4] = {0};
|
|
int v_global = vbase + v;
|
|
|
|
for (int k = 0; k < v_edge_cnt[v]; ++k) {
|
|
int e = adj_edges[v_edge_adr[v] + k];
|
|
|
|
// compute rest edge vector
|
|
mjtNum dx[3];
|
|
int v1 = m->flex_edge[2 * (ebase + e)];
|
|
int v2 = m->flex_edge[2 * (ebase + e) + 1];
|
|
mju_sub3(dx, m->flex_vert0 + 3 * (vbase + v2),
|
|
m->flex_vert0 + 3 * (vbase + v1));
|
|
|
|
// apply scaling since they are half sizes
|
|
dx[0] *= 2 * m->flex_size[3 * f + 0];
|
|
dx[1] *= 2 * m->flex_size[3 * f + 1];
|
|
dx[2] *= 2 * m->flex_size[3 * f + 2];
|
|
|
|
if (mju_abs(dx[2]) > mjMINVAL) {
|
|
mjERROR("flex vertices are not in the same plane");
|
|
}
|
|
|
|
// get mass of neighbor vertex
|
|
mjtNum weight = 1.0;
|
|
int neighbor_v = (v == v1) ? v2 : v1;
|
|
int b_neighbor = m->flex_vertbodyid[vbase + neighbor_v];
|
|
if (b_neighbor >= 0) {
|
|
weight = m->body_mass[b_neighbor];
|
|
if (weight < mjMINVAL) weight = mjMINVAL;
|
|
}
|
|
|
|
// accumulate B += w * dx * dx'
|
|
for (int row = 0; row < 2; row++) {
|
|
for (int col = 0; col < 2; col++) {
|
|
B[2 * row + col] += weight * dx[row] * dx[col];
|
|
}
|
|
}
|
|
}
|
|
|
|
mjtNum* metric = m->flex_vertmetric + 4 * v_global;
|
|
mjtNum det = B[0] * B[3] - B[1] * B[2];
|
|
|
|
if (mju_abs(det) < mjMINVAL) {
|
|
mju_zero(metric, 4);
|
|
} else {
|
|
mjtNum invdet = 1.0 / det;
|
|
metric[0] = B[3] * invdet;
|
|
metric[1] = -B[1] * invdet;
|
|
metric[2] = -B[2] * invdet;
|
|
metric[3] = B[0] * invdet;
|
|
}
|
|
}
|
|
|
|
// advance global offset
|
|
current_adj_offset += total_adj_edges;
|
|
|
|
// determine start address for this flex
|
|
int v0_base = 2 * vbase;
|
|
int current_adr = 0;
|
|
if (v0_base > 0) {
|
|
current_adr = vrowadr[v0_base - 1] + vrownnz[v0_base - 1];
|
|
}
|
|
vrowadr[v0_base] = current_adr;
|
|
|
|
for (int v = 0; v < nvert; ++v) {
|
|
// clear buf_ind
|
|
mju_zeroInt(buf_ind, nv);
|
|
int current_nnz = 0;
|
|
for (int i = 0; i < v_edge_cnt[v]; ++i) {
|
|
int e = adj_edges[v_edge_adr[v] + i];
|
|
int v1 = m->flex_edge[2 * (ebase + e)];
|
|
int v2 = m->flex_edge[2 * (ebase + e) + 1];
|
|
|
|
// chains from edge e
|
|
int b1 = m->flex_vertbodyid[vbase + v1];
|
|
int b2 = m->flex_vertbodyid[vbase + v2];
|
|
int NV1 = mj_bodyChain(m, b1, chain1);
|
|
int NV2 = mj_bodyChain(m, b2, chain2);
|
|
|
|
for (int j = 0; j < NV1; ++j) {
|
|
if (!buf_ind[chain1[j]]) {
|
|
buf_ind[chain1[j]] = 1;
|
|
current_nnz++;
|
|
}
|
|
}
|
|
for (int j = 0; j < NV2; ++j) {
|
|
if (!buf_ind[chain2[j]]) {
|
|
buf_ind[chain2[j]] = 1;
|
|
current_nnz++;
|
|
}
|
|
}
|
|
}
|
|
int row0 = 2 * (vbase + v);
|
|
int row1 = 2 * (vbase + v) + 1;
|
|
vrownnz[row0] = vrownnz[row1] = current_nnz;
|
|
|
|
// set rowadr for next rows
|
|
vrowadr[row1] = vrowadr[row0] + current_nnz;
|
|
if (row1 + 1 < 2 * m->nflexvert) {
|
|
vrowadr[row1 + 1] = vrowadr[row1] + current_nnz;
|
|
}
|
|
|
|
// fill colind
|
|
int count = 0;
|
|
for (int j = 0; j < nv; j++) {
|
|
if (buf_ind[j]) {
|
|
m->flexvert_J_colind[vrowadr[row0] + count] = j;
|
|
m->flexvert_J_colind[vrowadr[row1] + count] = j;
|
|
count++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
mj_freeStack(d);
|
|
}
|
|
|
|
|
|
// align 2D flexes to the XY plane
|
|
static void mj_alignFlex(mjModel* m, mjData* d) {
|
|
for (int f = 0; f < m->nflex; f++) {
|
|
// only for 2D flexes with vertex equality constraints
|
|
if (m->flex_dim[f] == 2 && m->flex_edgeequality[f] == 2) {
|
|
// get element data
|
|
int t_adr = m->flex_elemdataadr[f];
|
|
int vbase = m->flex_vertadr[f];
|
|
int t0 = m->flex_elem[t_adr];
|
|
int t1 = m->flex_elem[t_adr + 1];
|
|
int t2 = m->flex_elem[t_adr + 2];
|
|
|
|
// compute normal from first element
|
|
mjtNum edge1[3], edge2[3], normal[3];
|
|
mju_sub3(edge1, m->flex_vert0 + 3 * (vbase + t1), m->flex_vert0 + 3 * (vbase + t0));
|
|
mju_sub3(edge2, m->flex_vert0 + 3 * (vbase + t2),
|
|
m->flex_vert0 + 3 * (vbase + t0));
|
|
mju_cross(normal, edge1, edge2);
|
|
mju_normalize3(normal);
|
|
|
|
// compute rotation to Z
|
|
mjtNum quat[4], mat[9];
|
|
mju_quatZ2Vec(quat, normal);
|
|
mju_quat2Mat(mat, quat);
|
|
|
|
// rotate all vertices of this flex
|
|
int nvert = m->flex_vertnum[f];
|
|
for (int v = 0; v < nvert; v++) {
|
|
mjtNum* vert = m->flex_vert0 + 3 * (vbase + v);
|
|
mjtNum res[3];
|
|
|
|
mju_mulMatTVec3(res, mat, vert);
|
|
mju_copy3(vert, res);
|
|
|
|
// check planarity (warning if not planar)
|
|
if (mju_abs(vert[2] - m->flex_vert0[3 * (vbase + t0) + 2]) > 100 * mjMINVAL) {
|
|
static int warned = 0;
|
|
if (!warned) {
|
|
warned = 1;
|
|
mju_warning("flex %d is not planar", f);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// set quantities that depend on qpos0
|
|
static void set0(mjModel* m, mjData* d) {
|
|
makeTendonSparse(m);
|
|
makeFlexSparse(m, d);
|
|
|
|
mj_alignFlex(m, d);
|
|
int nv = m->nv;
|
|
mjtNum A[36] = {0}, pos[3], quat[4];
|
|
mj_markStack(d);
|
|
mjtNum* jac = mjSTACKALLOC(d, 6*nv, mjtNum);
|
|
mjtNum* tmp = mjSTACKALLOC(d, 6*nv, mjtNum);
|
|
mjtNum* moment = mjSTACKALLOC(d, nv, mjtNum);
|
|
int* cammode = 0;
|
|
int* lightmode = 0;
|
|
|
|
// save camera and light mode, set to fixed
|
|
if (m->ncam) {
|
|
cammode = mjSTACKALLOC(d, m->ncam, int);
|
|
for (int i=0; i < m->ncam; i++) {
|
|
cammode[i] = m->cam_mode[i];
|
|
m->cam_mode[i] = mjCAMLIGHT_FIXED;
|
|
}
|
|
}
|
|
if (m->nlight) {
|
|
lightmode = mjSTACKALLOC(d, m->nlight, int);
|
|
for (int i=0; i < m->nlight; i++) {
|
|
lightmode[i] = m->light_mode[i];
|
|
m->light_mode[i] = mjCAMLIGHT_FIXED;
|
|
}
|
|
}
|
|
|
|
// run computations in qpos0
|
|
mju_copy(d->qpos, m->qpos0, m->nq);
|
|
mj_kinematics(m, d);
|
|
mj_comPos(m, d);
|
|
mj_camlight(m, d);
|
|
|
|
// compute dof_M0 for CRB algorithm
|
|
mj_setM0(m, d);
|
|
|
|
// save flex_rigid, temporarily make all flexes non-rigid
|
|
mjtByte* rigid = NULL;
|
|
if (m->nflex) {
|
|
rigid = mjSTACKALLOC(d, m->nflex, mjtByte);
|
|
memcpy(rigid, m->flex_rigid, m->nflex);
|
|
memset(m->flex_rigid, 0, m->nflex);
|
|
}
|
|
|
|
// run remaining computations
|
|
mj_tendon(m, d);
|
|
mj_makeM(m, d);
|
|
mj_factorM(m, d);
|
|
mj_flex(m, d);
|
|
mj_transmission(m, d);
|
|
|
|
// restore flex rigidity
|
|
if (m->nflex) {
|
|
memcpy(m->flex_rigid, rigid, m->nflex);
|
|
}
|
|
|
|
// restore camera and light mode
|
|
for (int i=0; i < m->ncam; i++) {
|
|
m->cam_mode[i] = cammode[i];
|
|
}
|
|
for (int i=0; i < m->nlight; i++) {
|
|
m->light_mode[i] = lightmode[i];
|
|
}
|
|
|
|
// copy fields
|
|
mju_copy(m->flexedge_length0, d->flexedge_length, m->nflexedge);
|
|
mju_copy(m->tendon_length0, d->ten_length, m->ntendon);
|
|
mju_copy(m->actuator_length0, d->actuator_length, m->nout);
|
|
|
|
// compute body_invweight0
|
|
m->body_invweight0[0] = m->body_invweight0[1] = 0.0;
|
|
for (int i=1; i < m->nbody; i++) {
|
|
// static bodies: zero invweight0
|
|
if (m->body_weldid[i] == 0) {
|
|
m->body_invweight0[2*i] = m->body_invweight0[2*i+1] = 0;
|
|
}
|
|
|
|
// accelerate simple bodies with no rotations
|
|
else if (m->body_simple[i] == 2) {
|
|
mjtNum mass = m->body_mass[i];
|
|
if (!mass) { // SHOULD NOT OCCUR
|
|
mjERROR("moving body %d has 0 mass", i);
|
|
}
|
|
m->body_invweight0[2*i+0] = 1/mju_max(mjMINVAL, mass);
|
|
m->body_invweight0[2*i+1] = 0;
|
|
}
|
|
|
|
// general body: full inertia
|
|
else {
|
|
if (nv) {
|
|
// inverse spatial inertia: A = J*inv(M)*J'
|
|
mj_jacBodyCom(m, d, jac, jac+3*nv, i);
|
|
mj_solveM(m, d, tmp, jac, 6);
|
|
mju_mulMatMatT(A, jac, tmp, 6, nv, 6);
|
|
}
|
|
|
|
// average diagonal and assign
|
|
m->body_invweight0[2*i] = (A[0] + A[7] + A[14])/3;
|
|
m->body_invweight0[2*i+1] = (A[21] + A[28] + A[35])/3;
|
|
}
|
|
}
|
|
|
|
// compute dof_invweight0
|
|
for (int i=0; i < m->njnt; i++) {
|
|
// simple body with no rotations: no off-diagonal inertia
|
|
if (m->body_simple[m->jnt_bodyid[i]] == 2) {
|
|
int id = m->jnt_dofadr[i];
|
|
int bi = m->jnt_bodyid[i];
|
|
mjtNum mass = m->body_mass[bi];
|
|
if (!mass) { // SHOULD NOT OCCUR
|
|
mjERROR("moving body %d has 0 mass", bi);
|
|
}
|
|
m->dof_invweight0[id] = 1/mju_max(mjMINVAL, mass);
|
|
}
|
|
|
|
// general joint: full inertia
|
|
else {
|
|
int dnum, id = m->jnt_dofadr[i];
|
|
|
|
// get number of components
|
|
if (m->jnt_type[i] == mjJNT_FREE) {
|
|
dnum = 6;
|
|
} else if (m->jnt_type[i] == mjJNT_BALL) {
|
|
dnum = 3;
|
|
} else {
|
|
dnum = 1;
|
|
}
|
|
|
|
// inverse joint inertia: A = J*inv(M)*J'
|
|
if (nv) {
|
|
mju_zero(jac, dnum*nv);
|
|
for (int j=0; j < dnum; j++) {
|
|
jac[j*(nv+1) + id] = 1;
|
|
}
|
|
mj_solveM(m, d, tmp, jac, dnum);
|
|
mju_mulMatMatT(A, jac, tmp, dnum, nv, dnum);
|
|
}
|
|
|
|
// average diagonal and assign
|
|
if (dnum == 6) {
|
|
m->dof_invweight0[id] = m->dof_invweight0[id+1] = m->dof_invweight0[id+2] =
|
|
(A[0] + A[7] + A[14])/3;
|
|
m->dof_invweight0[id+3] = m->dof_invweight0[id+4] = m->dof_invweight0[id+5] =
|
|
(A[21] + A[28] + A[35])/3;
|
|
} else if (dnum == 3) {
|
|
m->dof_invweight0[id] = m->dof_invweight0[id+1] = m->dof_invweight0[id+2] =
|
|
(A[0] + A[4] + A[8])/3;
|
|
} else {
|
|
m->dof_invweight0[id] = A[0];
|
|
}
|
|
}
|
|
}
|
|
|
|
// compute flexedge_invweight0, tendon_invweight0, actuator_acc0
|
|
if (nv) {
|
|
// compute flexedge_invweight0
|
|
for (int f=0; f < m->nflex; f++) {
|
|
if (m->flex_interp[f]) {
|
|
continue;
|
|
}
|
|
|
|
for (int i=m->flex_edgeadr[f]; i < m->flex_edgeadr[f]+m->flex_edgenum[f]; i++) {
|
|
// bodies connected by edge
|
|
int b1 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i]];
|
|
int b2 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i+1]];
|
|
|
|
// rigid edge: set to 0
|
|
if (m->flexedge_rigid[i]) {
|
|
m->flexedge_invweight0[i] = 0;
|
|
}
|
|
|
|
// accelerate edges that connect simple bodies with no rotations
|
|
else if (m->body_simple[b1] == 2 && m->body_simple[b2] == 2) {
|
|
m->flexedge_invweight0[i] = (1/m->body_mass[b1] + 1/m->body_mass[b2])/2;
|
|
}
|
|
|
|
// handle general edge
|
|
else {
|
|
// make dense vector into tmp
|
|
mju_zero(tmp, nv);
|
|
int end = m->flexedge_J_rowadr[i] + m->flexedge_J_rownnz[i];
|
|
for (int j=m->flexedge_J_rowadr[i]; j < end; j++) {
|
|
tmp[m->flexedge_J_colind[j]] = d->flexedge_J[j];
|
|
}
|
|
|
|
// solve into tmp+nv
|
|
mj_solveM(m, d, tmp+nv, tmp, 1);
|
|
m->flexedge_invweight0[i] = mju_dot(tmp, tmp+nv, nv);
|
|
}
|
|
}
|
|
}
|
|
|
|
// compute tendon_invweight0
|
|
for (int i=0; i < m->ntendon; i++) {
|
|
mju_sparse2dense(tmp, d->ten_J, 1, nv, m->ten_J_rownnz+i, m->ten_J_rowadr+i, m->ten_J_colind);
|
|
|
|
// solve into tmp+nv
|
|
mj_solveM(m, d, tmp+nv, tmp, 1);
|
|
m->tendon_invweight0[i] = mju_dot(tmp, tmp+nv, nv);
|
|
}
|
|
|
|
// compute actuator_acc0, one per force output (moment row)
|
|
for (int i=0; i < m->nout; i++) {
|
|
mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + i,
|
|
d->moment_rowadr + i, d->moment_colind);
|
|
mj_solveM(m, d, tmp, moment, 1);
|
|
m->actuator_acc0[i] = mju_norm(tmp, nv);
|
|
}
|
|
} else {
|
|
mju_zero(m->tendon_invweight0, m->ntendon);
|
|
mju_zero(m->actuator_acc0, m->nout);
|
|
}
|
|
|
|
// compute missing eq_data for body constraints
|
|
for (int i=0; i < m->neq; i++) {
|
|
// get ids
|
|
int id1 = m->eq_obj1id[i];
|
|
int id2 = m->eq_obj2id[i];
|
|
|
|
// connect constraint
|
|
if (m->eq_type[i] == mjEQ_CONNECT) {
|
|
switch ((mjtObj) m->eq_objtype[i]) {
|
|
case mjOBJ_BODY:
|
|
// pos = anchor position in global frame
|
|
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0);
|
|
|
|
// data[3-5] = anchor position in body2 local frame
|
|
mju_subFrom3(pos, d->xpos+3*id2);
|
|
mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id2, pos);
|
|
break;
|
|
case mjOBJ_SITE:
|
|
// site-based connect, eq_data is unused
|
|
mju_zero(m->eq_data+mjNEQDATA*i, mjNEQDATA);
|
|
break;
|
|
default:
|
|
mjERROR("invalid objtype in connect constraint %d", i);
|
|
}
|
|
}
|
|
|
|
// weld constraint
|
|
else if (m->eq_type[i] == mjEQ_WELD) {
|
|
switch ((mjtObj) m->eq_objtype[i]) {
|
|
case mjOBJ_BODY: {
|
|
// skip if user has set any quaternion data
|
|
if (!mju_isZero(m->eq_data + mjNEQDATA*i + 6, 4)) {
|
|
// normalize quaternion just in case
|
|
mju_normalize4(m->eq_data+mjNEQDATA*i+6);
|
|
continue;
|
|
}
|
|
|
|
// anchor position is in body2 local frame
|
|
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0);
|
|
|
|
// data[3-5] = anchor position in body1 local frame
|
|
mju_subFrom3(pos, d->xpos+3*id1);
|
|
mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id1, pos);
|
|
|
|
// data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
|
|
mju_negQuat(quat, d->xquat+4*id1);
|
|
mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2);
|
|
break;
|
|
}
|
|
case mjOBJ_SITE: {
|
|
break;
|
|
}
|
|
default:
|
|
mjERROR("invalid objtype in weld constraint %d", i);
|
|
}
|
|
}
|
|
}
|
|
|
|
// camera compos0, pos0, mat0
|
|
for (int i=0; i < m->ncam; i++) {
|
|
// get body ids
|
|
int id = m->cam_bodyid[i]; // camera body
|
|
int id1 = m->cam_targetbodyid[i]; // target body
|
|
|
|
// compute positional offsets
|
|
mju_sub3(m->cam_pos0+3*i, d->cam_xpos+3*i, d->xpos+3*id);
|
|
mju_sub3(m->cam_poscom0+3*i, d->cam_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id));
|
|
|
|
// copy mat
|
|
mju_copy9(m->cam_mat0+9*i, d->cam_xmat+9*i);
|
|
}
|
|
|
|
// light compos0, pos0, dir0
|
|
for (int i=0; i < m->nlight; i++) {
|
|
// get body ids
|
|
int id = m->light_bodyid[i]; // light body
|
|
int id1 = m->light_targetbodyid[i]; // target body
|
|
|
|
// compute positional offsets
|
|
mju_sub3(m->light_pos0+3*i, d->light_xpos+3*i, d->xpos+3*id);
|
|
mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com + (id1 >= 0 ? 3*id1 : 3*id));
|
|
|
|
// copy dir
|
|
mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i);
|
|
}
|
|
|
|
// compute actuator damping from dampratio
|
|
for (int i=0; i < m->nactuator; i++) {
|
|
// get bias, gain parameters
|
|
mjtNum* biasprm = m->actuator_biasprm + i*mjNBIAS;
|
|
mjtNum* gainprm = m->actuator_gainprm + i*mjNGAIN;
|
|
|
|
// not a position-like actuator: skip (PID single-sources kp in biasprm[1])
|
|
int is_pid = m->actuator_gaintype[i] == mjGAIN_PID;
|
|
if (!is_pid && gainprm[0] != -biasprm[1]) {
|
|
continue;
|
|
}
|
|
|
|
// damping is 0 or negative (interpreted as regular "kv"): skip
|
|
if (biasprm[2] <= 0) {
|
|
continue;
|
|
}
|
|
|
|
// === interpret biasprm[2] > 0 as dampratio for position-like actuators
|
|
|
|
// "reflected" inertia (inversely scaled by transmission squared)
|
|
int rownnz = d->moment_rownnz[m->actuator_outadr[i]];
|
|
int rowadr = d->moment_rowadr[m->actuator_outadr[i]];
|
|
mjtNum* transmission = d->actuator_moment + rowadr;
|
|
mjtNum mass = 0;
|
|
for (int j=0; j < rownnz; j++) {
|
|
mjtNum trn = mju_abs(transmission[j]);
|
|
mjtNum trn2 = trn*trn; // transmission squared
|
|
if (trn2 > mjMINVAL) {
|
|
int dof = d->moment_colind[rowadr + j];
|
|
mass += m->dof_M0[dof] / trn2;
|
|
}
|
|
}
|
|
|
|
// damping = dampratio * 2 * sqrt(kp * mass)
|
|
mjtNum kp = is_pid ? -biasprm[1] : gainprm[0];
|
|
mjtNum damping = biasprm[2] * 2 * mju_sqrt(kp * mass);
|
|
|
|
// set biasprm[2] to negative damping
|
|
biasprm[2] = -damping;
|
|
}
|
|
|
|
mj_freeStack(d);
|
|
}
|
|
|
|
|
|
// accumulate bounding box
|
|
static void updateBox(mjtNum* xmin, mjtNum* xmax, mjtNum* pos, mjtNum radius) {
|
|
for (int i=0; i < 3; i++) {
|
|
xmin[i] = mjMIN(xmin[i], pos[i] - radius);
|
|
xmax[i] = mjMAX(xmax[i], pos[i] + radius);
|
|
}
|
|
}
|
|
|
|
|
|
// compute stat; assume computations already executed in qpos0
|
|
static void setStat(mjModel* m, mjData* d) {
|
|
mjtNum xmin[3] = {1E+10, 1E+10, 1E+10};
|
|
mjtNum xmax[3] = {-1E+10, -1E+10, -1E+10};
|
|
mjtNum rbound;
|
|
mj_markStack(d);
|
|
|
|
// approximate length associated with each body
|
|
mjtNum* body = mjSTACKALLOC(d, m->nbody, mjtNum);
|
|
|
|
// compute bounding box of bodies, joint centers, geoms and sites
|
|
for (int i=1; i < m->nbody; i++) {
|
|
updateBox(xmin, xmax, d->xpos+3*i, 0);
|
|
updateBox(xmin, xmax, d->xipos+3*i, 0);
|
|
}
|
|
for (int i=0; i < m->njnt; i++) {
|
|
updateBox(xmin, xmax, d->xanchor+3*i, 0);
|
|
}
|
|
for (int i=0; i < m->nsite; i++) {
|
|
updateBox(xmin, xmax, d->site_xpos+3*i, 0);
|
|
}
|
|
for (int i=0; i < m->ngeom; i++) {
|
|
// set rbound: regular geom rbound, or 0.1 of plane or hfield max size
|
|
rbound = 0;
|
|
if (m->geom_rbound[i] > 0) {
|
|
rbound = m->geom_rbound[i];
|
|
} else if (m->geom_type[i] == mjGEOM_PLANE) {
|
|
// finite in at least one direction
|
|
if (m->geom_size[3*i] || m->geom_size[3*i+1]) {
|
|
rbound = mjMAX(m->geom_size[3*i], m->geom_size[3*i+1]) * 0.1;
|
|
}
|
|
|
|
// infinite in both directions
|
|
else {
|
|
rbound = 0.01;
|
|
}
|
|
} else if (m->geom_type[i] == mjGEOM_HFIELD) {
|
|
int j = m->geom_dataid[i];
|
|
rbound = mjMAX(m->hfield_size[4*j],
|
|
mjMAX(m->hfield_size[4*j+1],
|
|
mjMAX(m->hfield_size[4*j+2], m->hfield_size[4*j+3]))) * 0.1;
|
|
}
|
|
|
|
updateBox(xmin, xmax, d->geom_xpos+3*i, rbound);
|
|
}
|
|
|
|
// compute center
|
|
mju_add3(m->stat.center, xmin, xmax);
|
|
mju_scl3(m->stat.center, m->stat.center, 0.5);
|
|
|
|
// compute bounding box size
|
|
if (xmax[0] > xmin[0])
|
|
m->stat.extent = mju_max(1E-5,
|
|
mju_max(xmax[0]-xmin[0], mju_max(xmax[1]-xmin[1], xmax[2]-xmin[2])));
|
|
|
|
// set body size to max com-joint distance
|
|
mju_zero(body, m->nbody);
|
|
for (int i=0; i < m->njnt; i++) {
|
|
// handle this body
|
|
int id = m->jnt_bodyid[i];
|
|
body[id] = mju_max(body[id], mju_dist3(d->xipos+3*id, d->xanchor+3*i));
|
|
|
|
// handle parent body
|
|
id = m->body_parentid[id];
|
|
body[id] = mju_max(body[id], mju_dist3(d->xipos+3*id, d->xanchor+3*i));
|
|
}
|
|
body[0] = 0;
|
|
|
|
// set body size to max of old value, and geom rbound + com-geom dist
|
|
for (int i=1; i < m->nbody; i++) {
|
|
for (int id=m->body_geomadr[i]; id < m->body_geomadr[i]+m->body_geomnum[i]; id++) {
|
|
if (m->geom_rbound[id] > 0) {
|
|
body[i] = mju_max(body[i], m->geom_rbound[id] + mju_dist3(d->xipos+3*i, d->geom_xpos+3*id));
|
|
}
|
|
}
|
|
}
|
|
|
|
// adjust body size for flex edges involving body
|
|
for (int f=0; f < m->nflex; f++) {
|
|
if (m->flex_interp[f]) {
|
|
for (int v1=m->flex_nodeadr[f]; v1 < m->flex_nodeadr[f]+m->flex_nodenum[f]; v1++) {
|
|
for (int v2=m->flex_nodeadr[f]; v2 < m->flex_nodeadr[f]+m->flex_nodenum[f]; v2++) {
|
|
mjtNum edge = mju_dist3(d->xpos+3*m->flex_nodebodyid[v1],
|
|
d->xpos+3*m->flex_nodebodyid[v2]);
|
|
body[m->flex_nodebodyid[v1]] = mju_max(body[m->flex_nodebodyid[v1]], edge);
|
|
}
|
|
}
|
|
continue;
|
|
}
|
|
for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
|
|
int b1 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e]];
|
|
int b2 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e+1]];
|
|
|
|
body[b1] = mju_max(body[b1], m->flexedge_length0[e]);
|
|
body[b2] = mju_max(body[b2], m->flexedge_length0[e]);
|
|
}
|
|
}
|
|
|
|
// compute meansize, make sure all sizes are above min
|
|
if (m->nbody > 1) {
|
|
m->stat.meansize = 0;
|
|
for (int i=1; i < m->nbody; i++) {
|
|
body[i] = mju_max(body[i], 1E-5);
|
|
m->stat.meansize += body[i]/(m->nbody-1);
|
|
}
|
|
}
|
|
|
|
// inherit dof length from parent body
|
|
for (int i=0; i < m->nv; i++) {
|
|
// default to linear dof, already has length units
|
|
m->dof_length[i] = 1;
|
|
|
|
// if rotational dof, inherit from body
|
|
int jnt = m->dof_jntid[i];
|
|
mjtJoint type = m->jnt_type[jnt];
|
|
int offset = i - m->jnt_dofadr[jnt];
|
|
if (type == mjJNT_BALL ||
|
|
type == mjJNT_HINGE ||
|
|
(type == mjJNT_FREE && offset >= 3)) {
|
|
m->dof_length[i] = body[m->dof_bodyid[i]];
|
|
}
|
|
}
|
|
|
|
// fix extent if too small compared to meanbody
|
|
m->stat.extent = mju_max(m->stat.extent, 2 * m->stat.meansize);
|
|
|
|
// compute meanmass
|
|
if (m->nbody > 1) {
|
|
m->stat.meanmass = 0;
|
|
for (int i=1; i < m->nbody; i++) {
|
|
m->stat.meanmass += m->body_mass[i];
|
|
}
|
|
m->stat.meanmass /= (m->nbody-1);
|
|
}
|
|
|
|
// compute meaninertia
|
|
if (m->nv) {
|
|
m->stat.meaninertia = 0;
|
|
for (int i=0; i < m->nv; i++) {
|
|
m->stat.meaninertia += d->M[m->M_rowadr[i] + m->M_rownnz[i] - 1];
|
|
}
|
|
m->stat.meaninertia /= m->nv;
|
|
}
|
|
|
|
mj_freeStack(d);
|
|
}
|
|
|
|
|
|
// set quantities that depend qpos_spring
|
|
static void setSpring(mjModel* m, mjData* d) {
|
|
// run computations in qpos_spring
|
|
mju_copy(d->qpos, m->qpos_spring, m->nq);
|
|
mj_kinematics(m, d);
|
|
mj_comPos(m, d);
|
|
mj_tendon(m, d);
|
|
mj_transmission(m, d);
|
|
|
|
// copy if model spring length is -1
|
|
for (int i=0; i < m->ntendon; i++) {
|
|
if (m->tendon_lengthspring[2*i] == -1 && m->tendon_lengthspring[2*i+1] == -1) {
|
|
// explicit springlength unused, set equal to ten_length
|
|
m->tendon_lengthspring[2*i] = m->tendon_lengthspring[2*i+1] = d->ten_length[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// entry point: set all remaining constant fields of mjModel, except for lengthrange
|
|
|
|
// constant part of the implicit effective metric factor (currently dim-2 bending): sparse
|
|
// reverse-Cholesky of M + (h^2 + h*damping)*K_bend over
|
|
// the dofs of unpinned vertices of standard dim-2 flexes with bending. The matrix is constant
|
|
// (flat-rest bending stiffness, point masses), so the factor is computed here once and reused
|
|
// by the implicit-flex constraint solve every step. Bending couples only same-coordinate dofs,
|
|
// so the pattern is three interleaved copies of the vertex flap adjacency. Row order (flex
|
|
// order, vertex order, coordinate fastest) and the resulting fill count must match the
|
|
// compiler's symbolic sizing (checked below).
|
|
static void setEfm0Factor(mjModel* m, mjData* d) {
|
|
int nbd = m->nefm0dof;
|
|
if (!nbd) {
|
|
return;
|
|
}
|
|
mj_markStack(d);
|
|
mjtNum h = m->opt.timestep;
|
|
|
|
// enumerate covered vertices: compact slot per unpinned vertex of qualifying flexes
|
|
// (filter matches the compiler's sizing and, for bending, flexStiff_active in
|
|
// engine_derivative.c: bending data exists only for dim-2 flexes)
|
|
int* vslot = mjSTACKALLOC(d, m->nflexvert > 0 ? m->nflexvert : 1, int);
|
|
for (int i=0; i < m->nflexvert; i++) {
|
|
vslot[i] = -1;
|
|
}
|
|
int nfree = 0;
|
|
for (int f=0; f < m->nflex; f++) {
|
|
if (m->flex_interp[f] || m->flex_rigid[f] || m->flex_dim[f] != 2 ||
|
|
m->flex_bendingadr[f] < 0) {
|
|
continue;
|
|
}
|
|
for (int lv=0; lv < m->flex_vertnum[f]; lv++) {
|
|
int gv = m->flex_vertadr[f] + lv;
|
|
if (m->body_dofnum[m->flex_vertbodyid[gv]] == 3) {
|
|
vslot[gv] = nfree;
|
|
nfree++;
|
|
}
|
|
}
|
|
}
|
|
if (3*nfree != nbd) {
|
|
mj_freeStack(d);
|
|
mjERROR("constant metric factor dof count mismatch: compiler sized %d, engine found %d",
|
|
nbd, 3*nfree);
|
|
}
|
|
|
|
// fill row -> dof address (row 3*slot + k, coordinate fastest)
|
|
for (int gv=0; gv < m->nflexvert; gv++) {
|
|
if (vslot[gv] >= 0) {
|
|
int da = m->body_dofadr[m->flex_vertbodyid[gv]];
|
|
for (int k=0; k < 3; k++) {
|
|
m->efm0_dofid[3*vslot[gv] + k] = da + k;
|
|
}
|
|
}
|
|
}
|
|
|
|
// assemble the bending-only stiffness K = (h^2 + h*damping)*K_bend over all dofs with the
|
|
// shared stencil walker from engine_derivative: bending values are configuration-independent
|
|
// and stretch/interp are gated off, so the call is valid at set-constants time (d is used
|
|
// for stack scratch only)
|
|
int nv = m->nv;
|
|
int* K_rownnz = mjSTACKALLOC(d, nv, int);
|
|
int* K_rowadr = mjSTACKALLOC(d, nv, int);
|
|
int nK = mjd_flexStiff_assemble(m, d, K_rownnz, K_rowadr, NULL, NULL, h*h, h,
|
|
/*flg_bend=*/1, /*flg_stretch=*/0, /*flg_contact=*/0,
|
|
NULL);
|
|
int* K_colind = mjSTACKALLOC(d, nK > 0 ? nK : 1, int);
|
|
mjtNum* K_val = mjSTACKALLOC(d, nK > 0 ? nK : 1, mjtNum);
|
|
mjd_flexStiff_assemble(m, d, K_rownnz, K_rowadr, K_colind, K_val, h*h, h, 1, 0, 0, NULL);
|
|
|
|
// inverse map: dof address -> compact factor row (monotone: slots follow dof order)
|
|
int* dofrow = mjSTACKALLOC(d, nv, int);
|
|
for (int i=0; i < nv; i++) {
|
|
dofrow[i] = -1;
|
|
}
|
|
for (int r=0; r < nbd; r++) {
|
|
dofrow[m->efm0_dofid[r]] = r;
|
|
}
|
|
|
|
// compact B to covered rows, keeping same-coordinate entries only: bending blocks are
|
|
// isotropic (q * I3), so the off-coordinate entries of assemble's 3x3 block pattern are
|
|
// structurally zero and dropping them preserves the pattern the compiler sized.
|
|
// H = M + (h^2+h*d)*K_bend in compact dof indices: lower CSR (values) + upper CSR (pattern)
|
|
int nHl = 0, nHu = 0;
|
|
for (int r=0; r < nbd; r++) {
|
|
int dof = m->efm0_dofid[r];
|
|
for (int c=0; c < K_rownnz[dof]; c++) {
|
|
int rc = dofrow[K_colind[K_rowadr[dof] + c]];
|
|
if (rc < 0 || (rc - r) % 3 != 0) continue; // uncovered or off-coordinate
|
|
if (rc < r) nHl++;
|
|
else if (rc > r) nHu++;
|
|
}
|
|
}
|
|
nHl += nbd; // diagonals: always present, also for rows without bending entries
|
|
int* Hl_rownnz = mjSTACKALLOC(d, nbd, int);
|
|
int* Hl_rowadr = mjSTACKALLOC(d, nbd, int);
|
|
int* Hl_colind = mjSTACKALLOC(d, nHl, int);
|
|
mjtNum* Hl_val = mjSTACKALLOC(d, nHl, mjtNum);
|
|
int* Hu_rownnz = mjSTACKALLOC(d, nbd, int);
|
|
int* Hu_rowadr = mjSTACKALLOC(d, nbd, int);
|
|
int* Hu_colind = mjSTACKALLOC(d, nHu > 0 ? nHu : 1, int);
|
|
|
|
int ladr = 0, uadr = 0;
|
|
for (int r=0; r < nbd; r++) {
|
|
int dof = m->efm0_dofid[r];
|
|
Hl_rowadr[r] = ladr;
|
|
Hu_rowadr[r] = uadr;
|
|
mjtNum diag = 0;
|
|
// B columns are dof-ascending, so filtered lower/upper columns stay ascending
|
|
for (int c=0; c < K_rownnz[dof]; c++) {
|
|
int adr = K_rowadr[dof] + c;
|
|
int rc = dofrow[K_colind[adr]];
|
|
if (rc < 0 || (rc - r) % 3 != 0) continue;
|
|
if (rc < r) {
|
|
Hl_colind[ladr] = rc;
|
|
Hl_val[ladr++] = K_val[adr];
|
|
} else if (rc > r) {
|
|
Hu_colind[uadr++] = rc;
|
|
} else {
|
|
diag = K_val[adr];
|
|
}
|
|
}
|
|
// diagonal last: point mass + armature + bending diagonal
|
|
Hl_colind[ladr] = r;
|
|
Hl_val[ladr++] = m->body_mass[m->dof_bodyid[dof]] + m->dof_armature[dof] + diag;
|
|
Hl_rownnz[r] = ladr - Hl_rowadr[r];
|
|
Hu_rownnz[r] = uadr - Hu_rowadr[r];
|
|
}
|
|
|
|
// symbolic factorization: counting phase (from the upper-triangle pattern)
|
|
int* LT_rownnz = mjSTACKALLOC(d, nbd, int);
|
|
int* LT_rowadr = mjSTACKALLOC(d, nbd, int);
|
|
int nnz = mju_cholFactorSymbolic(NULL, m->efm0_L_rownnz, m->efm0_L_rowadr,
|
|
NULL, LT_rownnz, LT_rowadr, NULL,
|
|
Hu_rownnz, Hu_rowadr, Hu_colind, nbd, d);
|
|
if (nnz != m->nefm0L) {
|
|
mj_freeStack(d);
|
|
mjERROR("constant metric factor size mismatch: compiler sized %d, symbolic found %d",
|
|
(int)m->nefm0L, nnz);
|
|
}
|
|
|
|
// symbolic factorization: filling phase
|
|
int* LT_colind = mjSTACKALLOC(d, nnz, int);
|
|
int* LT_map = mjSTACKALLOC(d, nnz, int);
|
|
mju_cholFactorSymbolic(m->efm0_L_colind, m->efm0_L_rownnz, m->efm0_L_rowadr,
|
|
LT_colind, LT_rownnz, LT_rowadr, LT_map,
|
|
Hu_rownnz, Hu_rowadr, Hu_colind, nbd, d);
|
|
|
|
// numeric factorization
|
|
int rank = mju_cholFactorNumeric(m->efm0_L, nbd, mjMINVAL,
|
|
m->efm0_L_rownnz, m->efm0_L_rowadr, m->efm0_L_colind,
|
|
LT_rownnz, LT_rowadr, LT_colind, LT_map,
|
|
Hl_val, Hl_rownnz, Hl_rowadr, Hl_colind, d);
|
|
if (rank != nbd) {
|
|
mj_freeStack(d);
|
|
mjERROR("constant metric factor is rank-deficient (%d of %d)", rank, nbd);
|
|
}
|
|
|
|
mj_freeStack(d);
|
|
}
|
|
|
|
|
|
void mj_setConst(mjModel* m, mjData* d) {
|
|
// compute npolygonmax and nmeshdegmax
|
|
m->npolygonmax = 0;
|
|
for (int i=0; i < m->nmeshpoly; i++) {
|
|
m->npolygonmax = mjMAX(m->npolygonmax, m->mesh_polyvertnum[i]);
|
|
}
|
|
|
|
m->nmeshdegmax = 0;
|
|
for (int i=0; i < m->nmeshvert; i++) {
|
|
m->nmeshdegmax = mjMAX(m->nmeshdegmax, m->mesh_polymapnum[i]);
|
|
}
|
|
|
|
// recompute sameframe flags from current model geometry
|
|
setSameframe(m);
|
|
|
|
// error if simple body lost sameframe (user must set simple="false")
|
|
for (int i = 1; i < m->nbody; i++) {
|
|
if (m->body_simple[i] > 0 && m->body_sameframe[i] != mjSAMEFRAME_BODY) {
|
|
mjERROR("body %d is compiled as simple but sameframe no longer holds, "
|
|
"use body/simple='false'", i);
|
|
}
|
|
}
|
|
|
|
// set fixed quantities
|
|
setFixed(m, d);
|
|
|
|
// set quantities that depend on qpos0
|
|
set0(m, d);
|
|
|
|
// compute statistics
|
|
setStat(m, d);
|
|
|
|
// set quantities that depend qpos_spring
|
|
setSpring(m, d);
|
|
|
|
// precompute the constant part of the implicit effective metric factor
|
|
setEfm0Factor(m, d);
|
|
}
|
|
|
|
|
|
//----------------------------- actuator length range computation ----------------------------------
|
|
|
|
// evaluate actuator length, advance special dynamics
|
|
static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side,
|
|
const mjLROpt* opt) {
|
|
int nv = m->nv;
|
|
int out = m->actuator_outadr[index];
|
|
|
|
// reduce velocity
|
|
mju_scl(d->qvel, d->qvel, mju_exp(-m->opt.timestep/mjMAX(0.01, opt->timeconst)), nv);
|
|
|
|
// step1: compute inertia and actuator moments
|
|
mj_step1(m, d);
|
|
|
|
// dense actuator_moment row
|
|
mj_markStack(d);
|
|
mjtNum* moment = mjSTACKALLOC(d, nv, mjtNum);
|
|
mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + out,
|
|
d->moment_rowadr + out, d->moment_colind);
|
|
|
|
// set force to generate desired acceleration
|
|
mj_solveM(m, d, d->qfrc_applied, moment, 1);
|
|
mjtNum nrm = mju_norm(d->qfrc_applied, nv);
|
|
mju_scl(d->qfrc_applied, moment, (2*side-1)*opt->accel/mjMAX(mjMINVAL, nrm), nv);
|
|
|
|
// impose maxforce
|
|
nrm = mju_norm(d->qfrc_applied, nv);
|
|
if (opt->maxforce > 0 && nrm > opt->maxforce) {
|
|
mju_scl(d->qfrc_applied, d->qfrc_applied, opt->maxforce/mjMAX(mjMINVAL, nrm), nv);
|
|
}
|
|
|
|
// step2: apply force
|
|
mj_step2(m, d);
|
|
|
|
mj_freeStack(d);
|
|
|
|
// return actuator length
|
|
return d->actuator_length[out];
|
|
}
|
|
|
|
|
|
// Set length range for specified actuator, return 1 if ok, 0 if error.
|
|
int mj_setLengthRange(mjModel* m, mjData* d, int index,
|
|
const mjLROpt* opt, char* error, int error_sz) {
|
|
// check index
|
|
if (index < 0 || index >= m->nactuator) {
|
|
mjERROR("invalid actuator index");
|
|
}
|
|
int out = m->actuator_outadr[index];
|
|
|
|
// skip depending on mode and type
|
|
int ismuscle = (m->actuator_gaintype[index] == mjGAIN_MUSCLE ||
|
|
m->actuator_biastype[index] == mjBIAS_MUSCLE);
|
|
int isuser = (m->actuator_gaintype[index] == mjGAIN_USER ||
|
|
m->actuator_biastype[index] == mjBIAS_USER);
|
|
if ((opt->mode == mjLRMODE_NONE) ||
|
|
(opt->mode == mjLRMODE_MUSCLE && !ismuscle) ||
|
|
(opt->mode == mjLRMODE_MUSCLEUSER && !ismuscle && !isuser)) {
|
|
return 1;
|
|
}
|
|
|
|
// use existing length range if available
|
|
if (opt->useexisting && (m->actuator_lengthrange[2*out] < m->actuator_lengthrange[2*out+1])) {
|
|
return 1;
|
|
}
|
|
|
|
// get transmission id
|
|
int threadid = m->actuator_trnid[index];
|
|
|
|
// use joint and tendon limits if available
|
|
if (opt->uselimit) {
|
|
// joint or jointinparent
|
|
if (m->actuator_trntype[index] == mjTRN_JOINT ||
|
|
m->actuator_trntype[index] == mjTRN_JOINTINPARENT) {
|
|
// make sure joint is limited
|
|
if (m->jnt_limited[threadid]) {
|
|
// copy range
|
|
m->actuator_lengthrange[2*out] = m->jnt_range[2*threadid];
|
|
m->actuator_lengthrange[2*out+1] = m->jnt_range[2*threadid+1];
|
|
|
|
// skip optimization
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
// tendon
|
|
if (m->actuator_trntype[index] == mjTRN_TENDON) {
|
|
// make sure tendon is limited
|
|
if (m->tendon_limited[threadid]) {
|
|
// copy range
|
|
m->actuator_lengthrange[2*out] = m->tendon_range[2*threadid];
|
|
m->actuator_lengthrange[2*out+1] = m->tendon_range[2*threadid+1];
|
|
|
|
// skip optimization
|
|
return 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
// optimize in both directions
|
|
mjtNum lmin[2] = {0, 0}, lmax[2] = {0, 0};
|
|
int side;
|
|
for (side=0; side < 2; side++) {
|
|
// init at qpos0
|
|
mj_resetData(m, d);
|
|
|
|
// simulate
|
|
int updated = 0;
|
|
while (d->time < opt->inttotal) {
|
|
// advance and get length
|
|
mjtNum len = evalAct(m, d, index, side, opt);
|
|
|
|
// reset: cannot proceed
|
|
if (d->time == 0) {
|
|
snprintf(error, error_sz, "Unstable lengthrange simulation in actuator %d", index);
|
|
return 0;
|
|
}
|
|
|
|
// update limits
|
|
if (d->time > opt->inttotal-opt->interval) {
|
|
if (len < lmin[side] || !updated) {
|
|
lmin[side] = len;
|
|
}
|
|
if (len > lmax[side] || !updated) {
|
|
lmax[side] = len;
|
|
}
|
|
|
|
updated = 1;
|
|
}
|
|
}
|
|
|
|
// assign
|
|
m->actuator_lengthrange[2*index+side] = (side == 0 ? lmin[side] : lmax[side]);
|
|
}
|
|
|
|
// check range
|
|
mjtNum dif = m->actuator_lengthrange[2*out+1] - m->actuator_lengthrange[2*out];
|
|
if (dif <= 0) {
|
|
snprintf(error, error_sz,
|
|
"Invalid lengthrange (%g, %g) in actuator %d",
|
|
m->actuator_lengthrange[2*out],
|
|
m->actuator_lengthrange[2*out+1], index);
|
|
return 0;
|
|
}
|
|
|
|
// check convergence, side 0
|
|
if (lmax[0]-lmin[0] > opt->tolrange*dif) {
|
|
snprintf(error, error_sz,
|
|
"Lengthrange computation did not converge in actuator %d:\n"
|
|
" eval (%g, %g)\n range (%g, %g)",
|
|
index, lmin[0], lmax[0],
|
|
m->actuator_lengthrange[2*out],
|
|
m->actuator_lengthrange[2*out+1]);
|
|
return 0;
|
|
}
|
|
|
|
// check convergence, side 1
|
|
if (lmax[1]-lmin[1] > opt->tolrange*dif) {
|
|
snprintf(error, error_sz,
|
|
"Lengthrange computation did not converge in actuator %d:\n"
|
|
" eval (%g, %g)\n range (%g, %g)",
|
|
index, lmin[1], lmax[1],
|
|
m->actuator_lengthrange[2*out],
|
|
m->actuator_lengthrange[2*out+1]);
|
|
return 0;
|
|
}
|
|
|
|
return 1;
|
|
}
|