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

Deprecate mju_rotVecMat and mju_rotVecMatT in favor of mju_mulMatVec3 and mju_mulMatTVec3.

Browse Source 
These functions names and argument ordering are more consistent with the rest of the API.

PiperOrigin-RevId: 643788290
Change-Id: I783eda8021b80b82098e23ed95669b102bb82508
This commit is contained in:
Yuval Tassa
2024-06-16 10:01:31 -07:00
committed by Copybara-Service
parent 739512a0e4
commit 6067048537
33 changed files with 233 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+11 -11
View File
@@ -88,7 +88,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
// apply fixed translation and rotation relative to parent
if (pid) {
mju_rotVecMat(xpos, bodypos, d->xmat+9*pid);
mju_mulMatVec3(xpos, d->xmat+9*pid, bodypos);
mju_addTo3(xpos, d->xpos+3*pid);
mju_mulQuat(xquat, d->xquat+4*pid, bodyquat);
} else {
@@ -464,7 +464,7 @@ void mj_flex(const mjModel* m, mjData* d) {
// 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_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
@@ -1022,8 +1022,8 @@ void mj_transmission(const mjModel* m, mjData* d) {
// 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
mju_mulMatVec3(wrench, d->site_xmat+9*id, gear); // translation
mju_mulMatVec3(wrench+3, d->site_xmat+9*id, gear+3); // rotation
// moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); // translation
@@ -1071,7 +1071,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
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);
mju_mulMatTVec3(vec, d->site_xmat+9*refid, vec);
// length: dot product with gear
length[i] += mju_dot3(vec, gear);
@@ -1092,7 +1092,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
}
// wrench: translational gear expressed in global frame
mju_rotVecMat(wrench, gear, d->site_xmat+9*refid);
mju_mulMatVec3(wrench, d->site_xmat+9*refid, gear);
// moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv);
@@ -1128,7 +1128,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
}
// wrench: rotational gear expressed in global frame
mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid);
mju_mulMatVec3(wrench, d->site_xmat+9*refid, gear+3);
// moment_tmp: global Jacobian projected on wrench, add to moment
if (!moment_tmp) moment_tmp = mj_stackAllocNum(d, nv);
@@ -1691,11 +1691,11 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
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);
mju_mulMatTVec3(dv, d->ximat+9*i, body_vel+6*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);
mju_mulMatVec3(d->subtree_angmom+3*i, d->ximat+9*i, dv);
}
// subtree linvel
@@ -1838,8 +1838,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
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);
mju_mulMatTVec3(cfrc, con->frame, lfrc+3);
mju_mulMatTVec3(cfrc+3, con->frame, lfrc);
// body 1
int k;