Add two new attributes to weld constraints:

- `anchor` determines the point of wrench application, in the frame of body2.
- `tfratio` scales applied torques relative to applied forces.
- Add visualisation of both anchor points to both weld and connect constraints.
- Add a test model showing how the new weld parameters behave.

PiperOrigin-RevId: 469228149
Change-Id: I836b0791f10fb624607a12ef3c687da991c21789

src/engine/engine_core_constraint.c

@@ -398,16 +398,18 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
         break;
 
       case mjEQ_WELD:                 // fix relative position and orientation
-        // find global points
+        // find global points and their Jacobians
         for (int j=0; j<2; j++) {
-          mjtNum* anchor = data + 3*(1-j);
-          mju_rotVecMat(pos[j], anchor, d->xmat + 9*id[j]);
+          // position offset for body1 only
+          if (j==0) {
+            mju_rotVecMat(pos[j], data, d->xmat + 9*id[j]);
+          } else {
+            mju_zero3(pos[j]);
+          }
+
           mju_addTo3(pos[j], d->xpos + 3*id[j]);
         }
 
-        // compute position error
-        mju_sub3(cpos, pos[0], pos[1]);
-
         // compute error Jacobian (opposite of contact: 0 - 1)
         NV = mj_jacDifPair(m, d, chain, id[1], id[0], pos[1], pos[0],
                            jac[1], jac[0], jacdif,
@@ -417,14 +419,19 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
         mju_copy(jac[0], jacdif, 3*NV);
         mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV);
 
-        // compute orientation error: neg(q1) * q0 * relpose (axis components only)
-        mjtNum* relpose = data+6;
-        mju_mulQuat(quat, d->xquat+4*id[0], relpose);   // quat = q0*relpose
+        // get desired position offset in global frame
+        mju_rotVecMat(cpos, data, d->xmat+9*id[0]);
+
+        // compute position error: p0 - p1 - data
+        mju_sub3(cpos, pos[0], pos[1]);
+
+        // compute orientation error: neg(q1) * q0 * data (axis components only)
+        mju_mulQuat(quat, d->xquat+4*id[0], data+3);    // quat = q0*data
         mju_negQuat(quat1, d->xquat+4*id[1]);           // quat1 = neg(q1)
-        mju_mulQuat(quat2, quat1, quat);                // quat2 = neg(q1)*q0*relpose
+        mju_mulQuat(quat2, quat1, quat);                // quat2 = neg(q1)*q0*data
         mju_copy3(cpos+3, quat2+1);                     // copy axis components
 
-        // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose
+        // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * data
         for (int j=0; j<NV; j++) {
           // axis = [jac0-jac1]_col(j)
           axis[0] = jac[0][3*NV+j];
@@ -433,7 +440,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
 
           // apply formula
           mju_mulQuatAxis(quat2, quat1, axis);    // quat2 = neg(q1)*(jac0-jac1)
-          mju_mulQuat(quat3, quat2, quat);        // quat3 = neg(q1)*(jac0-jac1)*q0*relpose
+          mju_mulQuat(quat3, quat2, quat);        // quat3 = neg(q1)*(jac0-jac1)*q0*data
 
           // correct Jacobian
           jac[0][3*NV+j] = 0.5*quat3[1];
@@ -441,10 +448,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
           jac[0][5*NV+j] = 0.5*quat3[3];
         }
 
-        // scale rotational jacobian by tfratio factor
-        mjtNum tfratio = data[10];
-        mju_scl(jac[0]+3*NV, jac[0]+3*NV, tfratio, 3*NV);
-
         size = 6;
         break;
 
@@ -1051,16 +1054,8 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int
 
   case mjCNSTR_EQUALITY:
     if (m->eq_type[id]==mjEQ_WELD) {
-      mjtNum rotlinratio = m->eq_data[mjNEQDATA*id+10];
-      mjtNum efc_pos[6];
-
-      // copy translational residual
-      mju_copy3(efc_pos, d->efc_pos+i);
-
-      // multiply orientations by tfratio
-      mju_scl3(efc_pos+3, d->efc_pos+i+3, rotlinratio);
       *dim = 6;
-      *pos = mju_norm(efc_pos, 6);
+      *pos = mju_norm(d->efc_pos+i, 6);  // mixes translation and rotation!
     } else if (m->eq_type[id]==mjEQ_CONNECT) {
       *dim = 3;
       *pos = mju_norm(d->efc_pos+i, 3);

src/engine/engine_core_smooth.c

@@ -1814,7 +1814,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
 // 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];
+  mjtNum cfrc_body[6], tmp[6], tmp1[6];
   mjContact* con;
 
   // clear cacc, set world acceleration to -gravity
@@ -1828,14 +1828,14 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
   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);
+      mju_copy3(tmp1, d->xfrc_applied+6*i+3);
+      mju_copy3(tmp1+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);
+      mju_transformSpatial(tmp, tmp1, 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);
+      mju_addTo(d->cfrc_ext+6*i, tmp, 6);
     }
 
   // cfrc_ext += contacts
@@ -1845,29 +1845,29 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
       con = d->contact+i;
 
       // tmp = contact-local force:torque vector
-      mj_contactForce(m, d, i, lfrc);
+      mj_contactForce(m, d, i, tmp);
 
-      // cfrc = world-oriented torque:force vector (swap in the process)
-      mju_rotVecMatT(cfrc, lfrc+3, con->frame);
-      mju_rotVecMatT(cfrc+3, lfrc, con->frame);
+      // tmp1 = world-oriented torque:force vector (swap in the process)
+      mju_rotVecMatT(tmp1, tmp+3, con->frame);
+      mju_rotVecMatT(tmp1+3, tmp, con->frame);
 
       // body 1
       int k;
       if ((k = m->geom_bodyid[con->geom1])) {
         // tmp = subtree CoM-based torque_force vector
-        mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0);
+        mju_transformSpatial(tmp, tmp1, 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);
+        mju_subFrom(d->cfrc_ext+6*k, tmp, 6);
       }
 
       // body 2
       if ((k = m->geom_bodyid[con->geom2])) {
         // tmp = subtree CoM-based torque_force vector
-        mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0);
+        mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0);
 
         // apply
-        mju_addTo(d->cfrc_ext+6*k, cfrc_com, 6);
+        mju_addTo(d->cfrc_ext+6*k, tmp, 6);
       }
     }
 
@@ -1883,41 +1883,73 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
     int k;
     switch (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
-      }
+      // tmp1 = world-oriented torque:force vector
+      mju_zero3(tmp1);  // no torque from connect
+      mju_copy3(tmp1 + 3, d->efc_force + i);
 
       // 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);
+        // transform connect point on body1: local -> global
+        mju_rotVecMat(pos, eq_data, d->xmat+9*k);
+        mju_addTo3(pos, d->xpos+3*k);
 
         // tmp = subtree CoM-based torque_force vector
-        mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
+        mju_transformSpatial(tmp, tmp1, 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);
+        mju_addTo(d->cfrc_ext+6*k, tmp, 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);
+        // transform connect point on body2: local -> global
+        mju_rotVecMat(pos, eq_data + 3, d->xmat+9*k);
+        mju_addTo3(pos, d->xpos+3*k);
 
         // tmp = subtree CoM-based torque_force vector
-        mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
+        mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
 
         // apply
-        mju_subFrom(d->cfrc_ext+6*k, cfrc_com, 6);
+        mju_subFrom(d->cfrc_ext+6*k, tmp, 6);
       }
 
-      // increment rows
-      i += m->eq_type[id]==mjEQ_WELD ? 6 : 3;
+      // increment 3 rows of connect
+      i += 3;
+
+      break;
+
+    case mjEQ_WELD:
+      // tmp1 = world-oriented torque:force vector (efc is f:t, so swap)
+      mju_copy3(tmp1, d->efc_force + i + 3);
+      mju_copy3(tmp1 + 3, d->efc_force + i);
+
+      // body 1
+      if ((k = m->eq_obj1id[id])) {
+        // transform weld point on body1: local -> global
+        mju_rotVecMat(pos, eq_data, d->xmat+9*k);
+        mju_addTo3(pos, d->xpos+3*k);
+
+        // tmp = subtree CoM-based torque_force vector
+        mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
+
+        // apply (opposite for body 1)
+        mju_addTo(d->cfrc_ext+6*k, tmp, 6);
+      }
+
+      // body 2
+      if ((k = m->eq_obj2id[id])) {
+        // weld force on body2 is always applied at body root
+        mju_copy3(pos, d->xpos+3*k);
+
+        // tmp = subtree CoM-based torque_force vector
+        mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
+
+        // apply
+        mju_subFrom(d->cfrc_ext+6*k, tmp, 6);
+      }
+
+      // increment 6 rows of weld
+      i += 6;
       break;
 
     case mjEQ_JOINT:
@@ -1935,23 +1967,22 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
   }
 
   // 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 i=1; i<m->nbody; i++) {
     // get body's first dof address
     int bda = m->body_dofadr[i];
 
     // cacc = cacc_parent + cdofdot * qvel + cdof * qacc
-    mju_mulDofVec(cacc, d->cdof_dot+6*bda, d->qvel+bda, m->body_dofnum[i]);
-    mju_add(d->cacc+6*i, d->cacc+6*m->body_parentid[i], cacc, 6);
-    mju_mulDofVec(cacc, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[i]);
-    mju_addTo(d->cacc+6*i, cacc, 6);
+    mju_mulDofVec(tmp, d->cdof_dot+6*bda, d->qvel+bda, m->body_dofnum[i]);
+    mju_add(d->cacc+6*i, d->cacc+6*m->body_parentid[i], tmp, 6);
+    mju_mulDofVec(tmp, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[i]);
+    mju_addTo(d->cacc+6*i, tmp, 6);
 
     // cfrc_body = cinert * cacc + cvel x (cinert * cvel)
     mju_mulInertVec(cfrc_body, d->cinert+10*i, d->cacc+6*i);
-    mju_mulInertVec(cfrc_corr, d->cinert+10*i, d->cvel+6*i);
-    mju_crossForce(cfrc, d->cvel+6*i, cfrc_corr);
-    mju_addTo(cfrc_body, cfrc, 6);
+    mju_mulInertVec(tmp, d->cinert+10*i, d->cvel+6*i);
+    mju_crossForce(tmp1, d->cvel+6*i, tmp);
+    mju_addTo(cfrc_body, tmp1, 6);
 
     // set cfrc_int = cfrc_body - cfrc_ext
     mju_sub(d->cfrc_int+6*i, cfrc_body, d->cfrc_ext+6*i, 6);

src/engine/engine_setconst.c

@@ -185,25 +185,22 @@ static void set0(mjModel* m, mjData* d) {
     // weld constraint
     else if (m->eq_type[i]==mjEQ_WELD) {
       // skip if user has set any quaternion data
-      if (m->eq_data[mjNEQDATA*i+6] ||
-          m->eq_data[mjNEQDATA*i+7] ||
-          m->eq_data[mjNEQDATA*i+8] ||
-          m->eq_data[mjNEQDATA*i+9]) {
+      if (m->eq_data[mjNEQDATA*i+3] ||
+          m->eq_data[mjNEQDATA*i+4] ||
+          m->eq_data[mjNEQDATA*i+5] ||
+          m->eq_data[mjNEQDATA*i+6]) {
         // normalize quaternion just in case
-        mju_normalize4(m->eq_data+mjNEQDATA*i+6);
+        mju_normalize4(m->eq_data+mjNEQDATA*i+3);
         continue;
       }
 
-      // anchor position is in body2 local frame
-      mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0);
+      // data[0-2] = xpos2-xpos1 in body1 local frame
+      mju_sub3(pos, d->xpos+3*id2, d->xpos+3*id1);
+      mju_rotVecMatT(m->eq_data+mjNEQDATA*i, pos, d->xmat+9*id1);
 
-      // data[3-5] = anchor position in body1 local frame
-      mju_subFrom3(pos, d->xpos+3*id1);
-      mju_rotVecMatT(m->eq_data+mjNEQDATA*i+3, pos, d->xmat+9*id1);
-
-      // data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
+      // data[3-6] = 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);
+      mju_mulQuat(m->eq_data+mjNEQDATA*i+3, quat, d->xquat+4*id2);
     }
   }
 

src/engine/engine_vis_visualize.c

@@ -1606,31 +1606,33 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
   objtype = mjOBJ_EQUALITY;
   category = mjCAT_DECOR;
   if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) {
-    // connect or weld
+    // connect
     for (int i=0; i<m->neq; i++) {
-      if (m->eq_active[i] && (m->eq_type[i]==mjEQ_CONNECT || m->eq_type[i]==mjEQ_WELD)) {
+      if (m->eq_active[i] && m->eq_type[i]==mjEQ_CONNECT) {
         // compute endpoints in global coordinates
-        int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
-        mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i]==mjEQ_WELD), d->xmat+9*j);
+        int j = m->eq_obj1id[i];
+        int k = m->eq_obj2id[i];
+        mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i, d->xmat+9*j);
         mju_addTo3(vec, d->xpos+3*j);
-        mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i]==mjEQ_CONNECT), d->xmat+9*k);
+        mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3, d->xmat+9*k);
         mju_addTo3(end, d->xpos+3*k);
 
+        // connect endpoints
+        START
+
         // construct geom
         sz[0] = scl * m->vis.scale.constraint;
+        mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, sz[0],
+                          vec[0], vec[1], vec[2],
+                          end[0], end[1], end[2]);
 
-        START
-        mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, d->xmat+9*j, m->vis.rgba.connect);
+        f2f(thisgeom->rgba, m->vis.rgba.constraint, 4);
+
+        // label flag
         if (vopt->label==mjLABEL_CONSTRAINT) {
           makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);
         }
-        FINISH
 
-        START
-        mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, d->xmat+9*k, m->vis.rgba.constraint);
-        if (vopt->label==mjLABEL_CONSTRAINT) {
-          makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);
-        }
         FINISH
       }
     }