// 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_core_smooth.h" #include #include #include #include #include #include "engine/engine_core_constraint.h" #include "engine/engine_crossplatform.h" #include "engine/engine_io.h" #include "engine/engine_macro.h" #include "engine/engine_support.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) { mjtNum pos[3], quat[4], *bodypos, *bodyquat; mjtNum qloc[4], vec[3], vec1[3], xanchor[3], xaxis[3]; // set world position and orientation mju_zero3(d->xpos); mju_unit4(d->xquat); mju_zero3(d->xipos); mju_zero(d->xmat, 9); mju_zero(d->ximat, 9); d->xmat[0] = d->xmat[4] = d->xmat[8] = 1; d->ximat[0] = d->ximat[4] = d->ximat[8] = 1; // normalize all quaternions in qpos mj_normalizeQuat(m, d->qpos); // normalize mocap quaternions for (int i=0; i < m->nmocap; i++) { mju_normalize4(d->mocap_quat+4*i); } // compute global cartesian positions and orientations of all bodies for (int i=1; i < m->nbody; i++) { // free joint if (m->body_jntnum[i] == 1 && m->jnt_type[m->body_jntadr[i]] == mjJNT_FREE) { // get addresses int jid = m->body_jntadr[i]; int qadr = m->jnt_qposadr[jid]; // copy pos and quat from qpos mju_copy3(pos, d->qpos+qadr); mju_copy4(quat, d->qpos+qadr+3); // set xanchor = 0, xaxis = (0,0,1) mju_copy3(xanchor, pos); xaxis[0] = xaxis[1] = 0; xaxis[2] = 1; // assign xanchor and xaxis mju_copy3(d->xanchor+3*jid, xanchor); mju_copy3(d->xaxis+3*jid, xaxis); } // regular or no joint else { int pid = m->body_parentid[i]; // get body pos and quat: from model or mocap if (m->body_mocapid[i] >= 0) { bodypos = d->mocap_pos + 3*m->body_mocapid[i]; bodyquat = d->mocap_quat + 4*m->body_mocapid[i]; } else { bodypos = m->body_pos+3*i; bodyquat = m->body_quat+4*i; } // apply fixed translation and rotation relative to parent mju_rotVecMat(vec, bodypos, d->xmat+9*pid); mju_add3(pos, d->xpos+3*pid, vec); mju_mulQuat(quat, d->xquat+4*pid, bodyquat); // accumulate joints, compute pos and quat for this body for (int j=0; j < m->body_jntnum[i]; j++) { // get joint id, qpos address, joint type int jid = m->body_jntadr[i] + j; int qadr = m->jnt_qposadr[jid]; mjtJoint jtype = m->jnt_type[jid]; // compute axis in global frame; ball jnt_axis is (0,0,1), set by compiler mju_rotVecQuat(xaxis, m->jnt_axis+3*jid, quat); // compute anchor in global frame mju_rotVecQuat(xanchor, m->jnt_pos+3*jid, quat); mju_addTo3(xanchor, pos); // apply joint transformation switch (jtype) { case mjJNT_SLIDE: mju_addToScl3(pos, xaxis, d->qpos[qadr] - m->qpos0[qadr]); break; case mjJNT_BALL: case mjJNT_HINGE: // compute local quaternion rotation (qloc) if (jtype == mjJNT_BALL) { mju_copy4(qloc, d->qpos+qadr); } else { mju_axisAngle2Quat(qloc, m->jnt_axis+3*jid, d->qpos[qadr] - m->qpos0[qadr]); } // apply rotation mju_mulQuat(quat, quat, qloc); // correct for off-center rotation mju_sub3(vec, xanchor, pos); mju_rotVecQuat(vec1, m->jnt_pos+3*jid, quat); pos[0] += (vec[0] - vec1[0]); pos[1] += (vec[1] - vec1[1]); pos[2] += (vec[2] - vec1[2]); break; default: mjERROR("unknown joint type %d", jtype); // SHOULD NOT OCCUR } // assign xanchor and xaxis mju_copy3(d->xanchor+3*jid, xanchor); mju_copy3(d->xaxis+3*jid, xaxis); } } // assign xquat and xpos, construct xmat mju_normalize4(quat); mju_copy4(d->xquat+4*i, quat); mju_copy3(d->xpos+3*i, pos); mju_quat2Mat(d->xmat+9*i, quat); } // compute/copy Cartesian positions and orientations of body inertial frames for (int i=1; i < m->nbody; i++) { 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 < m->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]); } // compute/copy Cartesian positions and orientations of sites for (int i=0; i < m->nsite; i++) { 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]); } } // map inertias and motion dofs to global frame centered at subtree-CoM void mj_comPos(const mjModel* m, mjData* d) { mjtNum offset[3], axis[3]; mj_markStack(d); mjtNum* mass_subtree = mj_stackAllocNum(d, m->nbody); // clear subtree mju_zero(mass_subtree, m->nbody); mju_zero(d->subtree_com, m->nbody*3); // backwards pass over bodies: compute subtree_com and mass_subtree for (int i=m->nbody-1; i >= 0; i--) { // add local info mju_addToScl3(d->subtree_com+3*i, d->xipos+3*i, m->body_mass[i]); mass_subtree[i] += m->body_mass[i]; // add to parent, except for world if (i) { int j = m->body_parentid[i]; mju_addTo3(d->subtree_com+3*j, d->subtree_com+3*i); mass_subtree[j] += mass_subtree[i]; } // compute local com if (mass_subtree[i] < mjMINVAL) { mju_copy3(d->subtree_com+3*i, d->xipos+3*i); } else { mju_scl3(d->subtree_com+3*i, d->subtree_com+3*i, 1.0/mjMAX(mjMINVAL, mass_subtree[i])); } } // zero out CoM frame inertia for the world body mju_zero(d->cinert, 10); // map inertias to frame centered at subtree_com for (int i=1; i < m->nbody; i++) { 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]); } // map motion dofs to global frame centered at subtree_com for (int j=0; j < m->njnt; j++) { // get dof address, body index int da = 6*m->jnt_dofadr[j]; int bi = m->jnt_bodyid[j]; // compute com-anchor vector mju_sub3(offset, d->subtree_com+3*m->body_rootid[bi], d->xanchor+3*j); // create motion dof int skip = 0; switch ((mjtJoint) m->jnt_type[j]) { 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; } // rotation components: same as ball skip = 18; mjFALLTHROUGH; case mjJNT_BALL: for (int i=0; i < 3; i++) { // 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]; mju_dofCom(d->cdof+da+skip+6*i, axis, offset); } break; case mjJNT_SLIDE: mju_dofCom(d->cdof+da, d->xaxis+3*j, 0); break; case mjJNT_HINGE: mju_dofCom(d->cdof+da, d->xaxis+3*j, offset); break; } } mj_freeStack(d); } // compute camera and light positions and orientations void mj_camlight(const mjModel* m, mjData* d) { mjtNum pos[3], matT[9]; // 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); // get camera body id and target body id int id = m->cam_bodyid[i]; int id1 = m->cam_targetbodyid[i]; // adjust for mode switch ((mjtCamLight) m->cam_mode[i]) { case mjCAMLIGHT_FIXED: break; case mjCAMLIGHT_TRACK: case mjCAMLIGHT_TRACKCOM: // fixed global orientation mju_copy(d->cam_xmat+9*i, m->cam_mat0+9*i, 9); // position: track camera body if (m->cam_mode[i] == mjCAMLIGHT_TRACK) { mju_add3(d->cam_xpos+3*i, d->xpos+3*id, m->cam_pos0+3*i); } // position: track subtree com else { mju_add3(d->cam_xpos+3*i, d->subtree_com+3*id, m->cam_poscom0+3*i); } break; case mjCAMLIGHT_TARGETBODY: case mjCAMLIGHT_TARGETBODYCOM: // only if target body is specified if (id1 >= 0) { // get position to look at if (m->cam_mode[i] == mjCAMLIGHT_TARGETBODY) { mju_copy3(pos, d->xpos+3*id1); } else { mju_copy3(pos, d->subtree_com+3*id1); } // zaxis = -desired camera direction, in global frame mju_sub3(matT+6, d->cam_xpos+3*i, pos); mju_normalize3(matT+6); // xaxis: orthogonal to zaxis and to (0,0,1) matT[3] = 0; matT[4] = 0; matT[5] = 1; mju_cross(matT, matT+3, matT+6); mju_normalize3(matT); // yaxis: orthogonal to xaxis and zaxis mju_cross(matT+3, matT+6, matT); mju_normalize3(matT+3); // set camera frame mju_transpose(d->cam_xmat+9*i, matT, 3, 3); } } } // 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]); // get light body id and target body id int id = m->light_bodyid[i]; int id1 = m->light_targetbodyid[i]; // adjust for mode switch ((mjtCamLight) m->light_mode[i]) { case mjCAMLIGHT_FIXED: break; case mjCAMLIGHT_TRACK: case mjCAMLIGHT_TRACKCOM: // fixed global orientation mju_copy3(d->light_xdir+3*i, m->light_dir0+3*i); // position: track light body if (m->light_mode[i] == mjCAMLIGHT_TRACK) { mju_add3(d->light_xpos+3*i, d->xpos+3*id, m->light_pos0+3*i); } // position: track subtree com else { mju_add3(d->light_xpos+3*i, d->subtree_com+3*id, m->light_poscom0+3*i); } break; case mjCAMLIGHT_TARGETBODY: case mjCAMLIGHT_TARGETBODYCOM: // only if target body is specified if (id1 >= 0) { // get position to look at if (m->light_mode[i] == mjCAMLIGHT_TARGETBODY) { mju_copy3(pos, d->xpos+3*id1); } else { mju_copy3(pos, d->subtree_com+3*id1); } // set dir mju_sub3(d->light_xdir+3*i, pos, d->light_xpos+3*i); } } // normalize dir mju_normalize3(d->light_xdir+3*i); } } // update dynamic BVH; leaf aabbs must be updated before call void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) { mj_markStack(d); int* modified = mj_stackAllocInt(d, bvhnum); mju_zeroInt(modified, bvhnum); // mark leafs as modified for (int i=0; i < bvhnum; i++) { if (m->bvh_nodeid[bvhadr+i] >= 0) { modified[i] = 1; } } // update non-leafs in backward pass (parents come before children) for (int i=bvhnum-1; i >= 0; i--) { if (m->bvh_nodeid[bvhadr+i] < 0) { int child1 = m->bvh_child[2*(bvhadr+i)]; int child2 = m->bvh_child[2*(bvhadr+i)+1]; // update if either child is modified if (modified[child1] || modified[child2]) { mjtNum* aabb = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic + i); const mjtNum* aabb1 = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic + child1); const mjtNum* aabb2 = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic + child2); // compute new (min, max) mjtNum xmin[3], xmax[3]; for (int k=0; k < 3; k++) { xmin[k] = mju_min(aabb1[k] - aabb1[k+3], aabb2[k] - aabb2[k+3]); xmax[k] = mju_max(aabb1[k] + aabb1[k+3], aabb2[k] + aabb2[k+3]); } // convert to (center, size) for (int k=0; k < 3; k++) { aabb[k] = 0.5*(xmax[k]+xmin[k]); aabb[k+3] = 0.5*(xmax[k]-xmin[k]); } modified[i] = 1; } } } mj_freeStack(d); } // compute flex-related quantities void mj_flex(const mjModel* m, mjData* d) { int nv = m->nv, issparse = mj_isSparse(m); int* rowadr = d->flexedge_J_rowadr, *rownnz = d->flexedge_J_rownnz; mjtNum* J = d->flexedge_J; // skip if no flexes if (!m->nflex) { return; } // compute Cartesian positions of flex vertices for (int f=0; f < m->nflex; f++) { int vstart = m->flex_vertadr[f]; int vend = m->flex_vertadr[f] + m->flex_vertnum[f]; // centered: copy body position if (m->flex_centered[f]) { for (int i=vstart; i < vend; i++) { mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); } } // non-centered: map from local to global else { for (int i=vstart; i < vend; i++) { mju_rotVecMat(d->flexvert_xpos+3*i, m->flex_vert+3*i, d->xmat+9*m->flex_vertbodyid[i]); mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); } } } // compute flex element aabb for (int f=0; f < m->nflex; f++) { int dim = m->flex_dim[f]; // process elements of this flex for (int e=0; e < m->flex_elemnum[f]; e++) { const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f]; // compute min and max along each global axis mjtNum xmin[3], xmax[3]; mju_copy3(xmin, vert+3*edata[0]); mju_copy3(xmax, vert+3*edata[0]); for (int i=1; i <= dim; i++) { for (int j=0; j < 3; j++) { mjtNum value = vert[3*edata[i]+j]; xmin[j] = mju_min(xmin[j], value); xmax[j] = mju_max(xmax[j], value); } } // compute aabb (center, size) int base = m->flex_elemadr[f] + e; d->flexelem_aabb[6*base+0] = 0.5*(xmax[0]+xmin[0]); d->flexelem_aabb[6*base+1] = 0.5*(xmax[1]+xmin[1]); d->flexelem_aabb[6*base+2] = 0.5*(xmax[2]+xmin[2]); d->flexelem_aabb[6*base+3] = 0.5*(xmax[0]-xmin[0]) + m->flex_radius[f]; d->flexelem_aabb[6*base+4] = 0.5*(xmax[1]-xmin[1]) + m->flex_radius[f]; d->flexelem_aabb[6*base+5] = 0.5*(xmax[2]-xmin[2]) + m->flex_radius[f]; } } // update flex bhv_aabb_dyn if needed if (!mjDISABLED(mjDSBL_MIDPHASE)) { for (int f=0; f < m->nflex; f++) { if (m->flex_bvhadr[f] >= 0) { int flex_bvhadr = m->flex_bvhadr[f]; int flex_bvhnum = m->flex_bvhnum[f]; // copy element aabbs to bhv leaf aabbs for (int i=flex_bvhadr; i < flex_bvhadr+flex_bvhnum; i++) { if (m->bvh_nodeid[i] >= 0) { mju_copy(d->bvh_aabb_dyn + 6*(i - m->nbvhstatic), d->flexelem_aabb + 6*(m->flex_elemadr[f] + m->bvh_nodeid[i]), 6); } } // update dynamic BVH mj_updateDynamicBVH(m, d, m->flex_bvhadr[f], m->flex_bvhnum[f]); } } } // allocate space mj_markStack(d); mjtNum* jac1 = mj_stackAllocNum(d, 3*nv); mjtNum* jac2 = mj_stackAllocNum(d, 3*nv); mjtNum* jacdif = mj_stackAllocNum(d, 3*nv); int* chain = issparse ? mj_stackAllocInt(d, nv) : NULL; // clear Jacobian: sparse or dense if (issparse) { mju_zeroInt(rowadr, m->nflexedge); mju_zeroInt(rownnz, m->nflexedge); } else { mju_zero(J, m->nflexedge*nv); } // 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]) { 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]; // 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++) { 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]; mjtNum* pos1 = d->flexvert_xpos + 3*(vbase+v1); mjtNum* pos2 = d->flexvert_xpos + 3*(vbase+v2); // vec = unit vector from v1 to v2, compute edge length mjtNum vec[3]; mju_sub3(vec, pos2, pos1); d->flexedge_length[ebase+e] = mju_normalize3(vec); // skip Jacobian if not needed if (skipjacobian) { continue; } // sparse edge Jacobian if (issparse) { // set rowadr if (ebase+e > 0) { rowadr[ebase+e] = rowadr[ebase+e-1] + rownnz[ebase+e-1]; } // get endpoint Jacobians, subtract int NV = mj_jacDifPair(m, d, chain, b1, b2, pos1, pos2, jac1, jac2, jacdif, NULL, NULL, NULL); // no dofs: skip if (!NV) { continue; } // apply chain rule to compute edge Jacobian mju_mulMatTVec(J + rowadr[ebase+e], jacdif, vec, 3, NV); // copy sparsity info rownnz[ebase+e] = NV; mju_copyInt(d->flexedge_J_colind + rowadr[ebase+e], chain, NV); } // dense edge Jacobian else { // get endpoint Jacobians, subtract mj_jac(m, d, jac1, NULL, pos1, b1); mj_jac(m, d, jac2, NULL, pos2, b2); mju_sub(jacdif, jac2, jac1, 3*nv); // apply chain rule to compute edge Jacobian mju_mulMatTVec(J + (ebase+e)*nv, jacdif, vec, 3, nv); } } } mj_freeStack(d); } // compute tendon lengths and moments void mj_tendon(const mjModel* m, mjData* d) { int issparse = mj_isSparse(m), nv = m->nv, nten = m->ntendon; int id0, id1, idw, adr, wcnt, wbody[4], sideid; int tp0, tp1, tpw, NV, *chain = NULL, *buf_ind = NULL; int *rownnz = d->ten_J_rownnz, *rowadr = d->ten_J_rowadr, *colind = d->ten_J_colind; mjtNum dif[3], divisor, wpnt[12], wlen; mjtNum *L = d->ten_length, *J = d->ten_J; mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf = NULL; if (!nten) { return; } // allocate space mj_markStack(d); jac1 = mj_stackAllocNum(d, 3*nv); jac2 = mj_stackAllocNum(d, 3*nv); jacdif = mj_stackAllocNum(d, 3*nv); tmp = mj_stackAllocNum(d, nv); if (issparse) { chain = mj_stackAllocInt(d, nv); buf_ind = mj_stackAllocInt(d, nv); sparse_buf = mj_stackAllocNum(d, nv); } // clear results mju_zero(L, nten); wcnt = 0; // clear Jacobian: sparse or dense if (issparse) { mju_zeroInt(rownnz, nten); } else { mju_zero(J, nten*nv); } // loop over tendons for (int i=0; i < nten; i++) { // initialize tendon path adr = m->tendon_adr[i]; d->ten_wrapadr[i] = wcnt; d->ten_wrapnum[i] = 0; // sparse Jacobian row init if (issparse) { rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0); } // process joint tendon if (m->wrap_type[adr] == mjWRAP_JOINT) { // process all defined joints for (int j=0; j < m->tendon_num[i]; j++) { // get joint id int k = m->wrap_objid[adr+j]; // add to length L[i] += m->wrap_prm[adr+j] * d->qpos[m->jnt_qposadr[k]]; // add to moment if (issparse) { J[rowadr[i] + rownnz[i]] = m->wrap_prm[adr+j]; colind[rowadr[i] + rownnz[i]] = m->jnt_dofadr[k]; rownnz[i]++; } // add to moment: dense else { J[i*nv + m->jnt_dofadr[k]] = m->wrap_prm[adr+j]; } } // sort on colind if sparse: custom insertion sort if (issparse) { int x, *list = colind+rowadr[i]; mjtNum y, *listy = J+rowadr[i]; for (int k=1; k < rownnz[i]; k++) { x = list[k]; y = listy[k]; int j = k-1; while (j >= 0 && list[j] > x) { list[j+1] = list[j]; listy[j+1] = listy[j]; j--; } list[j+1] = x; listy[j+1] = y; } } continue; } // process spatial tendon divisor = 1; int j = 0; while (j < m->tendon_num[i]-1) { // get 1st and 2nd object tp0 = m->wrap_type[adr+j]; id0 = m->wrap_objid[adr+j]; tp1 = m->wrap_type[adr+j+1]; id1 = m->wrap_objid[adr+j+1]; // pulley if (tp0 == mjWRAP_PULLEY || tp1 == mjWRAP_PULLEY) { // get divisor, insert obj=-2 if (tp0 == mjWRAP_PULLEY) { divisor = m->wrap_prm[adr+j]; mju_zero3(d->wrap_xpos+wcnt*3); d->wrap_obj[wcnt] = -2; d->ten_wrapnum[i]++; wcnt++; } // move to next j++; continue; } // init sequence; assume it starts with site wlen = -1; mju_copy3(wpnt, d->site_xpos+3*id0); wbody[0] = m->site_bodyid[id0]; // second object is geom: process site-geom-site if (tp1 == mjWRAP_SPHERE || tp1 == mjWRAP_CYLINDER) { // reassign, get 2nd site info tpw = tp1; idw = id1; tp1 = m->wrap_type[adr+j+2]; id1 = m->wrap_objid[adr+j+2]; // do wrapping, possibly get 2 extra points (wlen>=0) sideid = mju_round(m->wrap_prm[adr+j+1]); if (sideid < -1 || sideid >= m->nsite) { mjERROR("invalid sideid %d in wrap_prm", sideid); // SHOULD NOT OCCUR } wlen = mju_wrap(wpnt+3, d->site_xpos+3*id0, d->site_xpos+3*id1, d->geom_xpos+3*idw, d->geom_xmat+9*idw, m->geom_size+3*idw, tpw, (sideid >= 0 ? d->site_xpos+3*sideid : 0)); } else { tpw = mjWRAP_NONE; } // complete sequence, accumulate lengths if (wlen < 0) { mju_copy3(wpnt+3, d->site_xpos+3*id1); wbody[1] = m->site_bodyid[id1]; L[i] += mju_dist3(wpnt, wpnt+3)/divisor; } else { mju_copy3(wpnt+9, d->site_xpos+3*id1); wbody[1] = wbody[2] = m->geom_bodyid[idw]; wbody[3] = m->site_bodyid[id1]; L[i] += (mju_dist3(wpnt, wpnt+3) + wlen + mju_dist3(wpnt+6, wpnt+9))/divisor; } // accumulate moments if consequtive points are in different bodies for (int k=0; k < (wlen < 0 ? 1 : 3); k++) { if (wbody[k] != wbody[k+1]) { // get 3D position difference, normalize mju_sub3(dif, wpnt+3*k+3, wpnt+3*k); mju_normalize3(dif); // sparse if (issparse) { // get endpoint Jacobians, subtract NV = mj_jacDifPair(m, d, chain, wbody[k], wbody[k+1], wpnt+3*k, wpnt+3*k+3, jac1, jac2, jacdif, NULL, NULL, NULL); // no dofs: skip if (!NV) { continue; } // apply chain rule to compute tendon Jacobian mju_mulMatTVec(tmp, jacdif, dif, 3, NV); // add to existing rownnz[i] = mju_combineSparse(J+rowadr[i], tmp, 1, 1/divisor, rownnz[i], NV, colind+rowadr[i], chain, sparse_buf, buf_ind); } // dense else { // get endpoint Jacobians, subtract mj_jac(m, d, jac1, 0, wpnt+3*k, wbody[k]); mj_jac(m, d, jac2, 0, wpnt+3*k+3, wbody[k+1]); mju_sub(jacdif, jac2, jac1, 3*nv); // apply chain rule to compute tendon Jacobian mju_mulMatTVec(tmp, jacdif, dif, 3, nv); // add to existing mju_addToScl(J + i*nv, tmp, 1/divisor, nv); } } } // assign to wrap mju_copy(d->wrap_xpos+wcnt*3, wpnt, (wlen < 0 ? 3:9)); d->wrap_obj[wcnt] = -1; if (wlen >= 0) { d->wrap_obj[wcnt+1] = d->wrap_obj[wcnt+2] = idw; } d->ten_wrapnum[i] += (wlen < 0 ? 1:3); wcnt += (wlen < 0 ? 1:3); // advance j += (tpw != mjWRAP_NONE ? 2 : 1); // assign last site before pulley or tendon end if (j == m->tendon_num[i]-1 || m->wrap_type[adr+j+1] == mjWRAP_PULLEY) { mju_copy3(d->wrap_xpos+wcnt*3, d->site_xpos+3*id1); d->wrap_obj[wcnt] = -1; d->ten_wrapnum[i]++; wcnt++; } } } mj_freeStack(d); } // compute actuator/transmission lengths and moments void mj_transmission(const mjModel* m, mjData* d) { int id, idslider, ok, nv = m->nv, nu = m->nu; mjtNum det, sdet, av, rod, axis[3], vec[3], dlda[3], dldv[3], quat[4]; mjtNum wrench[6], gearAxis[3]; mjtNum *jac, *jacA, *jacS; mjtNum *length = d->actuator_length, *moment = d->actuator_moment, *gear; mjtNum *jacref = NULL, *moment_tmp = NULL; // required for site actuators if (!nu) { return; } // allocate space, clear moments mj_markStack(d); jac = mj_stackAllocNum(d, 3*nv); jacA = mj_stackAllocNum(d, 3*nv); jacS = mj_stackAllocNum(d, 3*nv); // define variables required for body transmission, don't allocate int issparse = mj_isSparse(m); mjtNum* efc_force = NULL; // used as marker for allocation requirement mjtNum *moment_exclude, *jacdifp, *jac1p, *jac2p; int *chain; // compute lengths and moments for (int i=0; i < nu; i++) { // extract info id = m->actuator_trnid[2*i]; idslider = m->actuator_trnid[2*i+1]; // for slider-crank only gear = m->actuator_gear+6*i; // process according to transmission type switch ((mjtTrn) m->actuator_trntype[i]) { case mjTRN_JOINT: // joint case mjTRN_JOINTINPARENT: // joint, force in parent frame // slide and hinge joint: scalar gear if (m->jnt_type[id] == mjJNT_SLIDE || m->jnt_type[id] == mjJNT_HINGE) { length[i] = d->qpos[m->jnt_qposadr[id]]*gear[0]; moment[i*nv + m->jnt_dofadr[id]] = gear[0]; } // ball joint: 3D wrench gear else if (m->jnt_type[id] == mjJNT_BALL) { // j: qpos start address int j = m->jnt_qposadr[id]; // axis: expmap representation of quaternion mju_quat2Vel(axis, d->qpos+j, 1); // gearAxis: rotate to parent frame if necessary if (m->actuator_trntype[i] == mjTRN_JOINT) { mju_copy3(gearAxis, gear); } else { mju_negQuat(quat, d->qpos+j); mju_rotVecQuat(gearAxis, gear, quat); } // length: axis*gearAxis length[i] = mju_dot3(axis, gearAxis); // j: dof start address j = m->jnt_dofadr[id]; // moment: gearAxis mju_copy3(moment+i*nv+j, gearAxis); } // free joint: 6D wrench gear else { // cannot compute meaningful length, set to 0 length[i] = 0; // j: qpos start address int j = m->jnt_qposadr[id]; // vec: translational components mju_copy3(vec, d->qpos+j); // axis: expmap representation of quaternion mju_quat2Vel(axis, d->qpos+j+3, 1); // gearAxis: rotate to world frame if necessary if (m->actuator_trntype[i] == mjTRN_JOINT) { mju_copy3(gearAxis, gear+3); } else { mju_negQuat(quat, d->qpos+j+3); mju_rotVecQuat(gearAxis, gear+3, quat); } // j: dof start address j = m->jnt_dofadr[id]; // moment: gear(tran), gearAxis mju_copy3(moment+i*nv+j, gear); mju_copy3(moment+i*nv+j+3, gearAxis); } break; case mjTRN_SLIDERCRANK: // slider-crank // get data rod = m->actuator_cranklength[i]; axis[0] = d->site_xmat[9*idslider+2]; axis[1] = d->site_xmat[9*idslider+5]; axis[2] = d->site_xmat[9*idslider+8]; mju_sub3(vec, d->site_xpos+3*id, d->site_xpos+3*idslider); // compute length and determinant // length = a'*v - sqrt(det); det = (a'*v)^2 + r^2 - v'*v) av = mju_dot3(vec, axis); det = av*av + rod*rod - mju_dot3(vec, vec); ok = 1; if (det <= 0) { ok = 0; sdet = 0; length[i] = av; } else { sdet = mju_sqrt(det); length[i] = av - sdet; } // compute derivatives of length w.r.t. vec and axis if (ok) { mju_scl3(dldv, axis, 1-av/sdet); mju_scl3(dlda, vec, 1/sdet); // use dlda as temp mju_addTo3(dldv, dlda); mju_scl3(dlda, vec, 1-av/sdet); } else { mju_copy3(dlda, vec); mju_copy3(dldv, axis); } // get Jacobians of axis(jacA) and vec(jac) mj_jacPointAxis(m, d, jacS, jacA, d->site_xpos+3*idslider, axis, m->site_bodyid[idslider]); mj_jacSite(m, d, jac, 0, id); mju_subFrom(jac, jacS, 3*nv); // apply chain rule for (int j=0; j < nv; j++) { for (int k=0; k < 3; k++) { moment[i*nv+j] += dlda[k]*jacA[k*nv+j] + dldv[k]*jac[k*nv+j]; } } // scale by gear ratio length[i] *= gear[0]; for (int j = 0; j < nv; j++) { moment[i*nv + j] *= gear[0]; } break; case mjTRN_TENDON: // tendon length[i] = d->ten_length[id]*gear[0]; // moment: sparse or dense if (mj_isSparse(m)) { // clear moment mju_zero(moment+i*nv, nv); int end = d->ten_J_rowadr[id] + d->ten_J_rownnz[id]; for (int j=d->ten_J_rowadr[id]; j < end; j++) { moment[i*nv + d->ten_J_colind[j]] = d->ten_J[j] * gear[0]; } } else { mju_scl(moment + i*nv, d->ten_J + id*nv, gear[0], nv); } break; case mjTRN_SITE: // site // get site translation (jac) and rotation (jacS) Jacobians in global frame mj_jacSite(m, d, jac, jacS, id); // clear length length[i] = 0; // reference site undefined if (m->actuator_trnid[2*i+1] == -1) { // wrench: gear expressed in global frame mju_rotVecMat(wrench, gear, d->site_xmat+9*id); // translation mju_rotVecMat(wrench+3, gear+3, d->site_xmat+9*id); // rotation // moment: global Jacobian projected on wrench mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); // translation mju_mulMatTVec(jac, jacS, wrench+3, 3, nv); // rotation mju_addTo(moment+i*nv, jac, nv); // add the two } // reference site defined else { int refid = m->actuator_trnid[2*i+1]; if (!jacref) jacref = mj_stackAllocNum(d, 3*nv); // intialize last dof address for each body int b0 = m->body_weldid[m->site_bodyid[id]]; int b1 = m->body_weldid[m->site_bodyid[refid]]; int dofadr0 = m->body_dofadr[b0] + m->body_dofnum[b0] - 1; int dofadr1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1; // find common ancestral dof, if any int dofadr_common = -1; if (dofadr0 >= 0 && dofadr1 >= 0) { // traverse up the tree until common ancestral dof is found while (dofadr0 != dofadr1) { if (dofadr0 < dofadr1) { dofadr1 = m->dof_parentid[dofadr1]; } else { dofadr0 = m->dof_parentid[dofadr0]; } if (dofadr0 == -1 || dofadr1 == -1) { // reached tree root, no common ancestral dof break; } } // found common ancestral dof if (dofadr0 == dofadr1) { dofadr_common = dofadr0; } } // clear moment mju_zero(moment+i*nv, nv); // translational transmission if (!mju_isZero(gear, 3)) { // vec: site position in reference site frame mju_sub3(vec, d->site_xpos+3*id, d->site_xpos+3*refid); mju_rotVecMatT(vec, vec, d->site_xmat+9*refid); // length: dot product with gear length[i] += mju_dot3(vec, gear); // jacref: global Jacobian of reference site mj_jacSite(m, d, jacref, NULL, refid); // subtract jacref from jac mju_subFrom(jac, jacref, 3*nv); // if common ancestral dof was found, clear the columns of its parental chain int da = dofadr_common; while (da >= 0) { jac[nv*0 + da] = 0; jac[nv*1 + da] = 0; jac[nv*2 + da] = 0; da = m->dof_parentid[da]; } // wrench: translational gear expressed in global frame mju_rotVecMat(wrench, gear, d->site_xmat+9*refid); // moment: global Jacobian projected on wrench mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); } // rotational transmission if (!mju_isZero(gear+3, 3)) { mjtNum refquat[4]; // get site and refsite quats from parent bodies (avoiding mju_mat2Quat) mju_mulQuat(quat, m->site_quat+4*id, d->xquat+4*m->site_bodyid[id]); mju_mulQuat(refquat, m->site_quat+4*refid, d->xquat+4*m->site_bodyid[refid]); // convert difference to expmap (axis-angle) mju_subQuat(vec, quat, refquat); // add length: dot product with gear length[i] += mju_dot3(vec, gear+3); // jacref: global rotational Jacobian of reference site mj_jacSite(m, d, NULL, jacref, refid); // subtract jacref from jacS mju_subFrom(jacS, jacref, 3*nv); // if common ancestral dof was found, clear the columns of its parental chain int da = dofadr_common; while (da >= 0) { jacS[nv*0 + da] = 0; jacS[nv*1 + da] = 0; jacS[nv*2 + da] = 0; da = m->dof_parentid[da]; } // wrench: rotational gear expressed in global frame mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid); // moment_tmp: global Jacobian projected on wrench, add to moment if (!moment_tmp) moment_tmp = mj_stackAllocNum(d, nv); mju_mulMatTVec(moment_tmp, jacS, wrench, 3, nv); mju_addTo(moment+i*nv, moment_tmp, nv); } } break; case mjTRN_BODY: // body (adhesive contacts) // cannot compute meaningful length, set to 0 length[i] = 0; // clear moment mju_zero(moment+i*nv, nv); // moment is average of all contact normal Jacobians { // allocate stack variables for the first mjTRN_BODY if (!efc_force) { efc_force = mj_stackAllocNum(d, d->nefc); moment_exclude = mj_stackAllocNum(d, nv); jacdifp = mj_stackAllocNum(d, 3*nv); jac1p = mj_stackAllocNum(d, 3*nv); jac2p = mj_stackAllocNum(d, 3*nv); chain = issparse ? mj_stackAllocInt(d, nv) : NULL; } // clear efc_force and moment_exclude mju_zero(efc_force, d->nefc); mju_zero(moment_exclude, nv); // count all relevant contacts, accumulate Jacobians int counter = 0; for (int j=0; j < d->ncon; j++) { const mjContact* con = d->contact+j; // contact involving flex, continue if (con->geom[0] < 0 || con->geom[1] < 0) { continue; } // get body ids int b1 = m->geom_bodyid[con->geom[0]]; int b2 = m->geom_bodyid[con->geom[1]]; // irrelevant contact, continue if (b1 != id && b2 != id) { continue; } // mark contact normals in efc_force if (!con->exclude) { counter++; // condim 1 or elliptic cones: normal is in the first row if (con->dim == 1 || m->opt.cone == mjCONE_ELLIPTIC) { efc_force[con->efc_address] = 1; } // pyramidal cones: average all pyramid directions else { int npyramid = con->dim-1; // number of frictional directions for (int k=0; k < 2*npyramid; k++) { efc_force[con->efc_address+k] = 0.5/npyramid; } } } // excluded contact in gap: get sparse or dense Jacobian, accumulate else if (con->exclude == 1) { counter++; // get Jacobian difference int NV = mj_jacDifPair(m, d, chain, b1, b2, con->pos, con->pos, jac1p, jac2p, jacdifp, NULL, NULL, NULL); // project Jacobian along the normal of the contact frame mju_mulMatMat(jac, con->frame, jacdifp, 1, 3, NV); // accumulate in moment_exclude if (issparse) { for (int k=0; k < NV; k++) { moment_exclude[chain[k]] += jac[k]; } } else { mju_addTo(moment_exclude, jac, nv); } } } // moment is average over contact normal Jacobians, make negative for adhesion if (counter) { // accumulate active contact Jacobians into moment mj_mulJacTVec(m, d, moment+i*nv, efc_force); // add Jacobians from excluded contacts mju_addTo(moment+i*nv, moment_exclude, nv); // normalize by total contacts, flip sign mju_scl(moment+i*nv, moment+i*nv, -1.0/counter, nv); } } break; default: mjERROR("unknown transmission type %d", m->actuator_trntype[i]); // SHOULD NOT OCCUR } } mj_freeStack(d); } //-------------------------- inertia --------------------------------------------------------------- // composite rigid body inertia algorithm void mj_crb(const mjModel* m, mjData* d) { TM_START; mjtNum buf[6]; mjtNum* crb = d->crb; int last_body = m->nbody - 1, nv = m->nv; // crb = cinert 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); } } // clear qM mju_zero(d->qM, m->nM); // dense forward pass over dofs for (int i=0; i < nv; i++) { // process block of diagonals (simple bodies) if (m->dof_simplenum[i]) { int n = i + m->dof_simplenum[i]; for (; i < n; i++) { d->qM[m->dof_Madr[i]] = m->dof_M0[i]; } // finish or else fall through with next row if (i == nv) { break; } } // init M(i,i) with armature inertia int Madr_ij = m->dof_Madr[i]; d->qM[Madr_ij] = m->dof_armature[i]; // precompute buf = crb_body_i * cdof_i mju_mulInertVec(buf, crb+10*m->dof_bodyid[i], d->cdof+6*i); // sparse backward pass over ancestors for (int j=i; j >= 0; j = m->dof_parentid[j]) { // M(i,j) += cdof_j * (crb_body_i * cdof_i) d->qM[Madr_ij++] += mju_dot(d->cdof+6*j, buf, 6); } } TM_END(mjTIMER_POS_INERTIA); } // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv, mjtNum* qLDiagSqrtInv) { int cnt; int Madr_kk, Madr_ki; mjtNum tmp; // local copies of key variables int* dof_Madr = m->dof_Madr; int* dof_parentid = m->dof_parentid; int nv = m->nv; // copy M into LD mju_copy(qLD, M, m->nM); // dense backward loop over dofs (regular only, simple diagonal already copied) for (int k=nv-1; k >= 0; k--) { // get address of M(k,k) Madr_kk = dof_Madr[k]; // check for small/negative numbers on diagonal if (qLD[Madr_kk] < mjMINVAL) { mj_warning(d, mjWARN_INERTIA, k); qLD[Madr_kk] = mjMINVAL; } // skip the rest if simple if (m->dof_simplenum[k]) { continue; } // sparse backward loop over ancestors of k (excluding k) Madr_ki = Madr_kk + 1; int i = dof_parentid[k]; while (i >= 0) { tmp = qLD[Madr_ki] / qLD[Madr_kk]; // tmp = M(k,i) / M(k,k) // get number of ancestors of i (including i) if (i < nv-1) { cnt = dof_Madr[i+1] - dof_Madr[i]; } else { cnt = m->nM - dof_Madr[i+1]; } // M(i,j) -= M(k,j) * tmp mju_addToScl(qLD+dof_Madr[i], qLD+Madr_ki, -tmp, cnt); qLD[Madr_ki] = tmp; // M(k,i) = tmp // advance to i's parent i = dof_parentid[i]; Madr_ki++; } } // compute 1/diag(D), 1/sqrt(diag(D)) for (int i=0; i < nv; i++) { mjtNum qLDi = qLD[dof_Madr[i]]; qLDiagInv[i] = 1.0/qLDi; if (qLDiagSqrtInv) { qLDiagSqrtInv[i] = 1.0/mju_sqrt(qLDi); } } } // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd void mj_factorM(const mjModel* m, mjData* d) { TM_START; mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv, d->qLDiagSqrtInv); TM_ADD(mjTIMER_POS_INERTIA); } // in-place sparse backsubstitution: x = inv(L'*D*L)*x // L is in lower triangle of qLD; D is on diagonal of qLD // handle n vectors at once void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, const mjtNum* qLD, const mjtNum* qLDiagInv) { // local copies of key variables int* dof_Madr = m->dof_Madr; int* dof_parentid = m->dof_parentid; int nv = m->nv; // single vector if (n == 1) { // x <- inv(L') * x; skip simple, exploit sparsity of input vector for (int i=nv-1; i >= 0; i--) { if (!m->dof_simplenum[i] && x[i]) { // init int Madr_ij = dof_Madr[i]+1; int j = dof_parentid[i]; // traverse ancestors backwards // read directly from x[i] since i cannot be a parent of itself while (j >= 0) { x[j] -= qLD[Madr_ij++]*x[i]; // x(j) -= L(i,j) * x(i) // advance to parent j = dof_parentid[j]; } } } // x <- inv(D) * x for (int i=0; i < nv; i++) { x[i] *= qLDiagInv[i]; // x(i) /= L(i,i) } // x <- inv(L) * x; skip simple for (int i=0; i < nv; i++) { if (!m->dof_simplenum[i]) { // init int Madr_ij = dof_Madr[i]+1; int j = dof_parentid[i]; // traverse ancestors backwards // write directly in x[i] since i cannot be a parent of itself while (j >= 0) { x[i] -= qLD[Madr_ij++]*x[j]; // x(i) -= L(i,j) * x(j) // advance to parent j = dof_parentid[j]; } } } } // multiple vectors else { int offset; mjtNum tmp; // x <- inv(L') * x; skip simple for (int i=nv-1; i >= 0; i--) { if (!m->dof_simplenum[i]) { // init int Madr_ij = dof_Madr[i]+1; int j = dof_parentid[i]; // traverse ancestors backwards while (j >= 0) { // process all vectors, exploit sparsity for (offset=0; offset < n*nv; offset+=nv) if ((tmp = x[i+offset])) { x[j+offset] -= qLD[Madr_ij]*tmp; // x(j) -= L(i,j) * x(i) } // advance to parent Madr_ij++; j = dof_parentid[j]; } } } // x <- inv(D) * x for (int i=0; i < nv; i++) { for (offset=0; offset < n*nv; offset+=nv) { x[i+offset] *= qLDiagInv[i]; // x(i) /= L(i,i) } } // x <- inv(L) * x; skip simple for (int i=0; i < nv; i++) { if (!m->dof_simplenum[i]) { // init int Madr_ij = dof_Madr[i]+1; int j = dof_parentid[i]; // traverse ancestors backwards tmp = x[i+offset]; while (j >= 0) { // process all vectors for (offset=0; offset < n*nv; offset+=nv) { x[i+offset] -= qLD[Madr_ij]*x[j+offset]; // x(i) -= L(i,j) * x(j) } // advance to parent Madr_ij++; j = dof_parentid[j]; } } } } } // sparse backsubstitution: x = inv(L'*D*L)*y // use factorization in d 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(m, x, n, d->qLD, d->qLDiagInv); } // in-place sparse backsubstitution for one island: x = inv(L'*D*L)*x // L is in lower triangle of qLD; D is on diagonal of qLD void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int island) { // if no islands, call mj_solveLD const mjtNum* qLD = d->qLD; const mjtNum* qLDiagInv = d->qLDiagInv; if (island < 0) { mj_solveLD(m, x, 1, qLD, qLDiagInv); return; } // local constants: general const int* Madr = m->dof_Madr; const int* parentid = m->dof_parentid; const int* simplenum = m->dof_simplenum; // local constants: island specific int ndof = d->island_dofnum[island]; const int* dofind = d->island_dofind + d->island_dofadr[island]; const int* islandind = d->dof_islandind; // x <- inv(L') * x; skip simple, exploit sparsity of input vector for (int k=ndof-1; k >= 0; k--) { int i = dofind[k]; if (!simplenum[i] && x[k]) { // init int Madr_ij = Madr[i]+1; int j = parentid[i]; // traverse ancestors backwards // read directly from x[l] since j cannot be a parent of itself while (j >= 0) { x[islandind[j]] -= qLD[Madr_ij++]*x[k]; // x(j) -= L(i,j) * x(i) // advance to parent j = parentid[j]; } } } // x <- inv(D) * x for (int k=ndof-1; k >= 0; k--) { x[k] *= qLDiagInv[dofind[k]]; // x(i) /= L(i,i) } // x <- inv(L) * x; skip simple for (int k=0; k < ndof; k++) { int i = dofind[k]; if (!simplenum[i]) { // init int Madr_ij = Madr[i]+1; int j = parentid[i]; // traverse ancestors backwards // write directly in x[i] since i cannot be a parent of itself while (j >= 0) { x[k] -= qLD[Madr_ij++]*x[islandind[j]]; // x(i) -= L(i,j) * x(j) // advance to parent j = parentid[j]; } } } } // half of sparse backsubstitution: x = sqrt(inv(D))*inv(L')*y void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) { // local copies of key variables mjtNum* qLD = d->qLD; mjtNum* qLDiagSqrtInv = d->qLDiagSqrtInv; int* dof_Madr = m->dof_Madr; int* dof_parentid = m->dof_parentid; int nv = m->nv; // x = y mju_copy(x, y, n * nv); // loop over the n input vectors for (int ivec=0; ivec < n; ivec++) { int offset = ivec*nv; // x <- inv(L') * x; skip simple, exploit sparsity of input vector for (int i=nv-1; i >= 0; i--) { mjtNum tmp; if (!m->dof_simplenum[i] && (tmp = x[i+offset])) { // init int Madr_ij = dof_Madr[i]+1; int j = dof_parentid[i]; // traverse ancestors backwards while (j >= 0) { x[j+offset] -= qLD[Madr_ij++] * tmp; // x(j) -= L(i,j) * x(i) // advance to parent j = dof_parentid[j]; } } } // x <- sqrt(inv(D)) * x for (int i=0; i < nv; i++) { x[i+offset] *= qLDiagSqrtInv[i]; // x(i) /= sqrt(L(i,i)) } } } //---------------------------------- velocity ------------------------------------------------------ // compute cvel, cdof_dot void mj_comVel(const mjModel* m, mjData* d) { mjtNum tmp[6], cvel[6], cdofdot[36]; // set world vel to 0 mju_zero(d->cvel, 6); // forward pass over bodies for (int i=1; i < m->nbody; i++) { // get body's first dof address int bda = m->body_dofadr[i]; // cvel = cvel_parent mju_copy(cvel, d->cvel+6*m->body_parentid[i], 6); // cvel = cvel_parent + cdof * qvel, cdofdot = cvel x cdof for (int j=0; j < m->body_dofnum[i]; j++) { // compute cvel and cdofdot switch ((mjtJoint) m->jnt_type[m->dof_jntid[bda+j]]) { case mjJNT_FREE: // cdofdot = 0 mju_zero(cdofdot, 18); // update velocity mju_mulDofVec(tmp, d->cdof+6*bda, d->qvel+bda, 3); mju_addTo(cvel, tmp, 6); // continue with rotations j += 3; mjFALLTHROUGH; 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)); } // update velocity mju_mulDofVec(tmp, d->cdof+6*(bda+j), d->qvel+bda+j, 3); mju_addTo(cvel, tmp, 6); // adjust for 3-dof joint j += 2; break; default: // in principle we should use the new velocity to compute cdofdot, // but it makes no difference becase crossMotion(cdof, cdof) = 0, // and using the old velocity may be more accurate numerically mju_crossMotion(cdofdot+6*j, cvel, d->cdof+6*(bda+j)); // update velocity mju_mulDofVec(tmp, d->cdof+6*(bda+j), d->qvel+bda+j, 1); mju_addTo(cvel, tmp, 6); } } // assign cvel, cdofdot mju_copy(d->cvel+6*i, cvel, 6); mju_copy(d->cdof_dot+6*bda, cdofdot, 6*m->body_dofnum[i]); } } // subtree linear velocity and angular momentum void mj_subtreeVel(const mjModel* m, mjData* d) { mjtNum dx[3], dv[3], dp[3], dL[3]; mj_markStack(d); mjtNum* body_vel = mj_stackAllocNum(d, 6*m->nbody); // bodywise quantities for (int i=0; i < m->nbody; i++) { // compute and save body velocity mj_objectVelocity(m, d, mjOBJ_BODY, i, body_vel+6*i, 0); // body linear momentum mju_scl3(d->subtree_linvel+3*i, body_vel+6*i+3, m->body_mass[i]); // body angular momentum mju_rotVecMatT(dv, body_vel+6*i, d->ximat+9*i); dv[0] *= m->body_inertia[3*i]; dv[1] *= m->body_inertia[3*i+1]; dv[2] *= m->body_inertia[3*i+2]; mju_rotVecMat(d->subtree_angmom+3*i, dv, d->ximat+9*i); } // subtree linvel for (int i=m->nbody-1; i >= 0; i--) { // non-world: add linear momentum to parent if (i) { mju_addTo3(d->subtree_linvel+3*m->body_parentid[i], d->subtree_linvel+3*i); } // 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])); } // subtree angmom for (int i=m->nbody-1; i > 0; i--) { int parent = m->body_parentid[i]; // momentum wrt body i 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]); mju_cross(dL, dx, dp); // add to subtree i mju_addTo3(d->subtree_angmom+3*i, dL); // add to parent mju_addTo3(d->subtree_angmom+3*parent, d->subtree_angmom+3*i); // momentum wrt parent mju_sub3(dx, d->subtree_com+3*i, d->subtree_com+3*parent); mju_sub3(dv, d->subtree_linvel+3*i, d->subtree_linvel+3*parent); mju_scl3(dv, dv, m->body_subtreemass[i]); mju_cross(dL, dx, dv); // add to parent mju_addTo3(d->subtree_angmom+3*parent, dL); } mj_freeStack(d); } //---------------------------------- RNE ----------------------------------------------------------- // 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) { mjtNum tmp[6], tmp1[6]; mj_markStack(d); mjtNum* loc_cacc = mj_stackAllocNum(d, m->nbody*6); mjtNum* loc_cfrc_body = mj_stackAllocNum(d, m->nbody*6); // set world acceleration to -gravity mju_zero(loc_cacc, 6); if (!mjDISABLED(mjDSBL_GRAVITY)) { mju_scl3(loc_cacc+3, m->opt.gravity, -1); } // forward pass over bodies: accumulate cacc, set cfrc_body for (int i=1; i < m->nbody; i++) { // get body's first dof address int bda = m->body_dofadr[i]; // cacc = cacc_parent + cdofdot * qvel 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); // cacc += cdof * qacc if (flg_acc) { mju_mulDofVec(tmp, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[i]); mju_addTo(loc_cacc+6*i, tmp, 6); } // 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); mju_crossForce(tmp1, d->cvel+6*i, tmp); mju_addTo(loc_cfrc_body+6*i, tmp1, 6); } // clear world cfrc_body, for style mju_zero(loc_cfrc_body, 6); // backward pass over bodies: accumulate cfrc_body from children for (int i=m->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); } // result = cdof * cfrc_body for (int i=0; i < m->nv; i++) { result[i] = mju_dot(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i], 6); } mj_freeStack(d); } // RNE with complete data: compute cacc, cfrc_ext, cfrc_int void mj_rnePostConstraint(const mjModel* m, mjData* d) { int nbody=m->nbody; mjtNum cfrc_com[6], cfrc[6], lfrc[6]; mjContact* con; // clear cacc, set world acceleration to -gravity mju_zero(d->cacc, 6); if (!mjDISABLED(mjDSBL_GRAVITY)) { mju_scl3(d->cacc+3, m->opt.gravity, -1); } // cfrc_ext = perturb mju_zero(d->cfrc_ext, 6*nbody); for (int i=1; i < nbody; i++) if (!mju_isZero(d->xfrc_applied+6*i, 6)) { // rearrange as torque:force mju_copy3(cfrc, d->xfrc_applied+6*i+3); mju_copy3(cfrc+3, d->xfrc_applied+6*i); // map force from application point to com; both world-oriented mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[i], d->xipos+3*i, 0); // accumulate mju_addTo(d->cfrc_ext+6*i, cfrc_com, 6); } // cfrc_ext += contacts for (int i=0; i < d->ncon; i++) if (d->contact[i].efc_address >= 0) { // get contact pointer con = d->contact+i; // skip contact involving flex if (con->geom[0] < 0 || con->geom[1] < 0) { continue; } // tmp = contact-local force:torque vector mj_contactForce(m, d, i, lfrc); // cfrc = world-oriented torque:force vector (swap in the process) mju_rotVecMatT(cfrc, lfrc+3, con->frame); mju_rotVecMatT(cfrc+3, lfrc, con->frame); // body 1 int k; if ((k = m->geom_bodyid[con->geom[0]])) { // tmp = subtree CoM-based torque_force vector mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); // apply (opposite for body 1) mju_subFrom(d->cfrc_ext+6*k, cfrc_com, 6); } // body 2 if ((k = m->geom_bodyid[con->geom[1]])) { // tmp = subtree CoM-based torque_force vector mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); // apply mju_addTo(d->cfrc_ext+6*k, cfrc_com, 6); } } // cfrc_ext += connect and weld constraints int i = 0; while (i < d->ne) { if (d->efc_type[i] != mjCNSTR_EQUALITY) mjERROR("row %d of efc is not an equality constraint", i); // SHOULD NOT OCCUR int id = d->efc_id[i]; mjtNum* eq_data = m->eq_data + mjNEQDATA*id; mjtNum pos[3]; int k; switch ((mjtEq) m->eq_type[id]) { case mjEQ_CONNECT: case mjEQ_WELD: // cfrc = world-oriented torque:force vector mju_copy3(cfrc + 3, d->efc_force + i); if (m->eq_type[id] == mjEQ_WELD) { mju_copy3(cfrc, d->efc_force + i + 3); } else { mju_zero3(cfrc); // no torque from connect } // body 1 if ((k = m->eq_obj1id[id])) { // transform point on body1: local -> global mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_WELD), 0, k, 0); // tmp = subtree CoM-based torque_force vector mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); // apply (opposite for body 1) mju_addTo(d->cfrc_ext+6*k, cfrc_com, 6); } // body 2 if ((k = m->eq_obj2id[id])) { // transform point on body2: local -> global mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_CONNECT), 0, k, 0); // tmp = subtree CoM-based torque_force vector mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); // apply mju_subFrom(d->cfrc_ext+6*k, cfrc_com, 6); } // increment rows i += m->eq_type[id] == mjEQ_WELD ? 6 : 3; break; case mjEQ_JOINT: case mjEQ_TENDON: // increment 1 row i++; break; case mjEQ_FLEX: // increment with number of non-rigid edges k = m->eq_obj1id[id]; int flex_edgeadr = m->flex_edgeadr[k]; int flex_edgenum = m->flex_edgenum[k]; for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) { if (!m->flexedge_rigid[e]) { i++; } } break; default: mjERROR("unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR } } // forward pass over bodies: compute cacc, cfrc_int mjtNum cacc[6], cfrc_body[6], cfrc_corr[6]; mju_zero(d->cfrc_int, 6); for (int j=1; j < m->nbody; j++) { // get body's first dof address int bda = m->body_dofadr[j]; // cacc = cacc_parent + cdofdot * qvel + cdof * qacc mju_mulDofVec(cacc, d->cdof_dot+6*bda, d->qvel+bda, m->body_dofnum[j]); mju_add(d->cacc+6*j, d->cacc+6*m->body_parentid[j], cacc, 6); mju_mulDofVec(cacc, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[j]); mju_addTo(d->cacc+6*j, cacc, 6); // cfrc_body = cinert * cacc + cvel x (cinert * cvel) mju_mulInertVec(cfrc_body, d->cinert+10*j, d->cacc+6*j); mju_mulInertVec(cfrc_corr, d->cinert+10*j, d->cvel+6*j); mju_crossForce(cfrc, d->cvel+6*j, cfrc_corr); mju_addTo(cfrc_body, cfrc, 6); // set cfrc_int = cfrc_body - cfrc_ext mju_sub(d->cfrc_int+6*j, cfrc_body, d->cfrc_ext+6*j, 6); } // backward pass over bodies: accumulate cfrc_int from children for (int j=m->nbody-1; j > 0; j--) { mju_addTo(d->cfrc_int+6*m->body_parentid[j], d->cfrc_int+6*j, 6); } }