Yuval Tassa
77b3533d90
- Allow in-place cross product a = a x b using `mju_cross(a, a, b)`. - Easier for compiler to optimize since tmp cannot alias a or b (verified with godbolt). PiperOrigin-RevId: 547265908 Change-Id: If965867e08c1bc980e71729e38f6855e32de18b2
507 lines
14 KiB
C
507 lines
14 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_util_spatial.h"
|
|
|
|
#include <math.h>
|
|
|
|
#include <mujoco/mjmodel.h>
|
|
#include "engine/engine_util_blas.h"
|
|
#include "engine/engine_util_errmem.h"
|
|
|
|
|
|
//------------------------------ quaternion operations ---------------------------------------------
|
|
|
|
// rotate vector by quaternion
|
|
void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) {
|
|
// null quat: copy vec
|
|
if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) {
|
|
mju_copy3(res, vec);
|
|
}
|
|
|
|
// regular processing
|
|
else {
|
|
mjtNum tmp[3];
|
|
// tmp = q_w * v + cross(q_xyz, v)
|
|
tmp[0] = quat[0]*vec[0] + quat[2]*vec[2] - quat[3]*vec[1];
|
|
tmp[1] = quat[0]*vec[1] + quat[3]*vec[0] - quat[1]*vec[2];
|
|
tmp[2] = quat[0]*vec[2] + quat[1]*vec[1] - quat[2]*vec[0];
|
|
// res = v + 2 * cross(q_xyz, t)
|
|
res[0] = vec[0] + 2 * (quat[2]*tmp[2] - quat[3]*tmp[1]);
|
|
res[1] = vec[1] + 2 * (quat[3]*tmp[0] - quat[1]*tmp[2]);
|
|
res[2] = vec[2] + 2 * (quat[1]*tmp[1] - quat[2]*tmp[0]);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// negate quaternion
|
|
void mju_negQuat(mjtNum res[4], const mjtNum quat[4]) {
|
|
res[0] = quat[0];
|
|
res[1] = -quat[1];
|
|
res[2] = -quat[2];
|
|
res[3] = -quat[3];
|
|
}
|
|
|
|
|
|
|
|
// multiply quaternions
|
|
void mju_mulQuat(mjtNum res[4], const mjtNum qa[4], const mjtNum qb[4]) {
|
|
mjtNum tmp[4] = {
|
|
qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3],
|
|
qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2],
|
|
qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1],
|
|
qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0]
|
|
};
|
|
res[0] = tmp[0];
|
|
res[1] = tmp[1];
|
|
res[2] = tmp[2];
|
|
res[3] = tmp[3];
|
|
}
|
|
|
|
|
|
|
|
// multiply quaternion and axis
|
|
void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3]) {
|
|
mjtNum tmp[4] = {
|
|
-quat[1]*axis[0] - quat[2]*axis[1] - quat[3]*axis[2],
|
|
quat[0]*axis[0] + quat[2]*axis[2] - quat[3]*axis[1],
|
|
quat[0]*axis[1] + quat[3]*axis[0] - quat[1]*axis[2],
|
|
quat[0]*axis[2] + quat[1]*axis[1] - quat[2]*axis[0]
|
|
};
|
|
res[0] = tmp[0];
|
|
res[1] = tmp[1];
|
|
res[2] = tmp[2];
|
|
res[3] = tmp[3];
|
|
}
|
|
|
|
|
|
|
|
// convert axisAngle to quaternion
|
|
void mju_axisAngle2Quat(mjtNum res[4], const mjtNum axis[3], mjtNum angle) {
|
|
// zero angle: null quat
|
|
if (angle == 0) {
|
|
res[0] = 1;
|
|
res[1] = 0;
|
|
res[2] = 0;
|
|
res[3] = 0;
|
|
}
|
|
|
|
// regular processing
|
|
else {
|
|
mjtNum s = mju_sin(angle*0.5);
|
|
res[0] = mju_cos(angle*0.5);
|
|
res[1] = axis[0]*s;
|
|
res[2] = axis[1]*s;
|
|
res[3] = axis[2]*s;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// convert quaternion (corresponding to orientation difference) to 3D velocity
|
|
void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt) {
|
|
mjtNum axis[3] = {quat[1], quat[2], quat[3]};
|
|
mjtNum sin_a_2 = mju_normalize3(axis);
|
|
mjtNum speed = 2 * mju_atan2(sin_a_2, quat[0]);
|
|
|
|
// when axis-angle is larger than pi, rotation is in the opposite direction
|
|
if (speed > mjPI) {
|
|
speed -= 2*mjPI;
|
|
}
|
|
speed /= dt;
|
|
|
|
mju_scl3(res, axis, speed);
|
|
}
|
|
|
|
|
|
|
|
// Subtract quaternions, express as 3D velocity: qb*quat(res) = qa.
|
|
void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]) {
|
|
// qdif = neg(qb)*qa
|
|
mjtNum qneg[4], qdif[4];
|
|
mju_negQuat(qneg, qb);
|
|
mju_mulQuat(qdif, qneg, qa);
|
|
|
|
// convert to 3D velocity
|
|
mju_quat2Vel(res, qdif, 1);
|
|
}
|
|
|
|
|
|
|
|
// convert quaternion to 3D rotation matrix
|
|
void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) {
|
|
// null quat: identity
|
|
if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) {
|
|
res[0] = 1;
|
|
res[1] = 0;
|
|
res[2] = 0;
|
|
res[3] = 0;
|
|
res[4] = 1;
|
|
res[5] = 0;
|
|
res[6] = 0;
|
|
res[7] = 0;
|
|
res[8] = 1;
|
|
}
|
|
|
|
// regular processing
|
|
else {
|
|
const mjtNum q00 = quat[0]*quat[0];
|
|
const mjtNum q01 = quat[0]*quat[1];
|
|
const mjtNum q02 = quat[0]*quat[2];
|
|
const mjtNum q03 = quat[0]*quat[3];
|
|
const mjtNum q11 = quat[1]*quat[1];
|
|
const mjtNum q12 = quat[1]*quat[2];
|
|
const mjtNum q13 = quat[1]*quat[3];
|
|
const mjtNum q22 = quat[2]*quat[2];
|
|
const mjtNum q23 = quat[2]*quat[3];
|
|
const mjtNum q33 = quat[3]*quat[3];
|
|
|
|
res[0] = q00 + q11 - q22 - q33;
|
|
res[4] = q00 - q11 + q22 - q33;
|
|
res[8] = q00 - q11 - q22 + q33;
|
|
|
|
res[1] = 2*(q12 - q03);
|
|
res[2] = 2*(q13 + q02);
|
|
res[3] = 2*(q12 + q03);
|
|
res[5] = 2*(q23 - q01);
|
|
res[6] = 2*(q13 - q02);
|
|
res[7] = 2*(q23 + q01);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// convert 3D rotation matrix to quaternion
|
|
void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) {
|
|
// q0 largest
|
|
if (mat[0]+mat[4]+mat[8] > 0) {
|
|
quat[0] = 0.5 * mju_sqrt(1 + mat[0] + mat[4] + mat[8]);
|
|
quat[1] = 0.25 * (mat[7] - mat[5]) / quat[0];
|
|
quat[2] = 0.25 * (mat[2] - mat[6]) / quat[0];
|
|
quat[3] = 0.25 * (mat[3] - mat[1]) / quat[0];
|
|
}
|
|
|
|
// q1 largest
|
|
else if (mat[0] > mat[4] && mat[0] > mat[8]) {
|
|
quat[1] = 0.5 * mju_sqrt(1 + mat[0] - mat[4] - mat[8]);
|
|
quat[0] = 0.25 * (mat[7] - mat[5]) / quat[1];
|
|
quat[2] = 0.25 * (mat[1] + mat[3]) / quat[1];
|
|
quat[3] = 0.25 * (mat[2] + mat[6]) / quat[1];
|
|
}
|
|
|
|
// q2 largest
|
|
else if (mat[4] > mat[8]) {
|
|
quat[2] = 0.5 * mju_sqrt(1 - mat[0] + mat[4] - mat[8]);
|
|
quat[0] = 0.25 * (mat[2] - mat[6]) / quat[2];
|
|
quat[1] = 0.25 * (mat[1] + mat[3]) / quat[2];
|
|
quat[3] = 0.25 * (mat[5] + mat[7]) / quat[2];
|
|
}
|
|
|
|
// q3 largest
|
|
else {
|
|
quat[3] = 0.5 * mju_sqrt(1 - mat[0] - mat[4] + mat[8]);
|
|
quat[0] = 0.25 * (mat[3] - mat[1]) / quat[3];
|
|
quat[1] = 0.25 * (mat[2] + mat[6]) / quat[3];
|
|
quat[2] = 0.25 * (mat[5] + mat[7]) / quat[3];
|
|
}
|
|
|
|
mju_normalize4(quat);
|
|
}
|
|
|
|
|
|
|
|
// time-derivative of quaternion, given 3D rotational velocity
|
|
void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]) {
|
|
res[0] = 0.5*(-vel[0]*quat[1] - vel[1]*quat[2] - vel[2]*quat[3]);
|
|
res[1] = 0.5*( vel[0]*quat[0] + vel[1]*quat[3] - vel[2]*quat[2]);
|
|
res[2] = 0.5*(-vel[0]*quat[3] + vel[1]*quat[0] + vel[2]*quat[1]);
|
|
res[3] = 0.5*( vel[0]*quat[2] - vel[1]*quat[1] + vel[2]*quat[0]);
|
|
}
|
|
|
|
|
|
|
|
// integrate quaternion given 3D angular velocity
|
|
void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) {
|
|
mjtNum angle, tmp[4], qrot[4];
|
|
|
|
// form local rotation quaternion, apply
|
|
mju_copy3(tmp, vel);
|
|
angle = scale * mju_normalize3(tmp);
|
|
mju_axisAngle2Quat(qrot, tmp, angle);
|
|
mju_mulQuat(quat, quat, qrot);
|
|
mju_normalize4(quat);
|
|
}
|
|
|
|
|
|
|
|
// compute quaternion performing rotation from z-axis to given vector
|
|
void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) {
|
|
mjtNum axis[3], a, vn[3] = {vec[0], vec[1], vec[2]}, z[3] = {0, 0, 1};
|
|
|
|
// set default result to no-rotation quaternion
|
|
quat[0] = 1;
|
|
mju_zero3(quat+1);
|
|
|
|
// normalize vector; if too small, no rotation
|
|
if (mju_normalize3(vn) < mjMINVAL) {
|
|
return;
|
|
}
|
|
|
|
// compute angle and axis
|
|
mju_cross(axis, z, vn);
|
|
a = mju_normalize3(axis);
|
|
|
|
// almost parallel
|
|
if (fabs(a) < mjMINVAL) {
|
|
// opposite: 180 deg rotation around x axis
|
|
if (mju_dot3(vn, z) < 0) {
|
|
quat[0] = 0;
|
|
quat[1] = 1;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
// make quaternion from angle and axis
|
|
a = mju_atan2(a, mju_dot3(vn, z));
|
|
mju_axisAngle2Quat(quat, axis, a);
|
|
}
|
|
|
|
|
|
|
|
//------------------------------ pose operations (quat, pos) ---------------------------------------
|
|
|
|
// multiply two poses
|
|
void mju_mulPose(mjtNum posres[3], mjtNum quatres[4],
|
|
const mjtNum pos1[3], const mjtNum quat1[4],
|
|
const mjtNum pos2[3], const mjtNum quat2[4]) {
|
|
// quatres = quat1*quat2
|
|
mju_mulQuat(quatres, quat1, quat2);
|
|
mju_normalize4(quatres);
|
|
|
|
// posres = quat1*pos2 + pos1
|
|
mju_rotVecQuat(posres, pos2, quat1);
|
|
mju_addTo3(posres, pos1);
|
|
}
|
|
|
|
|
|
|
|
// negate pose
|
|
void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], const mjtNum quat[4]) {
|
|
// qres = neg(quat)
|
|
mju_negQuat(quatres, quat);
|
|
|
|
// pres = -neg(quat)*pos
|
|
mju_rotVecQuat(posres, pos, quatres);
|
|
mju_scl3(posres, posres, -1);
|
|
}
|
|
|
|
|
|
|
|
// transform vector by pose
|
|
void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], const mjtNum vec[3]) {
|
|
// res = quat*vec + pos
|
|
mju_rotVecQuat(res, vec, quat);
|
|
mju_addTo3(res, pos);
|
|
}
|
|
|
|
|
|
|
|
//------------------------------ spatial algebra ---------------------------------------------------
|
|
|
|
// vector cross-product, 3D
|
|
void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]) {
|
|
mjtNum tmp[3] = {
|
|
a[1]*b[2] - a[2]*b[1],
|
|
a[2]*b[0] - a[0]*b[2],
|
|
a[0]*b[1] - a[1]*b[0]
|
|
};
|
|
res[0] = tmp[0];
|
|
res[1] = tmp[1];
|
|
res[2] = tmp[2];
|
|
}
|
|
|
|
|
|
|
|
// cross-product for motion vector
|
|
void mju_crossMotion(mjtNum res[6], const mjtNum vel[6], const mjtNum v[6]) {
|
|
res[0] = -vel[2]*v[1] + vel[1]*v[2];
|
|
res[1] = vel[2]*v[0] - vel[0]*v[2];
|
|
res[2] = -vel[1]*v[0] + vel[0]*v[1];
|
|
res[3] = -vel[2]*v[4] + vel[1]*v[5];
|
|
res[4] = vel[2]*v[3] - vel[0]*v[5];
|
|
res[5] = -vel[1]*v[3] + vel[0]*v[4];
|
|
|
|
res[3] += -vel[5]*v[1] + vel[4]*v[2];
|
|
res[4] += vel[5]*v[0] - vel[3]*v[2];
|
|
res[5] += -vel[4]*v[0] + vel[3]*v[1];
|
|
}
|
|
|
|
|
|
|
|
// cross-product for force vectors
|
|
void mju_crossForce(mjtNum res[6], const mjtNum vel[6], const mjtNum f[6]) {
|
|
res[0] = -vel[2]*f[1] + vel[1]*f[2];
|
|
res[1] = vel[2]*f[0] - vel[0]*f[2];
|
|
res[2] = -vel[1]*f[0] + vel[0]*f[1];
|
|
res[3] = -vel[2]*f[4] + vel[1]*f[5];
|
|
res[4] = vel[2]*f[3] - vel[0]*f[5];
|
|
res[5] = -vel[1]*f[3] + vel[0]*f[4];
|
|
|
|
res[0] += -vel[5]*f[4] + vel[4]*f[5];
|
|
res[1] += vel[5]*f[3] - vel[3]*f[5];
|
|
res[2] += -vel[4]*f[3] + vel[3]*f[4];
|
|
}
|
|
|
|
|
|
|
|
// express inertia in com-based frame
|
|
void mju_inertCom(mjtNum res[10], const mjtNum inert[3], const mjtNum mat[9],
|
|
const mjtNum dif[3], mjtNum mass) {
|
|
// tmp = diag(inert) * mat' (mat is local-to-global rotation)
|
|
mjtNum tmp[9] = {mat[0]*inert[0], mat[3]*inert[0], mat[6]*inert[0],
|
|
mat[1]*inert[1], mat[4]*inert[1], mat[7]*inert[1],
|
|
mat[2]*inert[2], mat[5]*inert[2], mat[8]*inert[2]};
|
|
|
|
// res_rot = mat * diag(inert) * mat'
|
|
res[0] = mat[0]*tmp[0] + mat[1]*tmp[3] + mat[2]*tmp[6];
|
|
res[1] = mat[3]*tmp[1] + mat[4]*tmp[4] + mat[5]*tmp[7];
|
|
res[2] = mat[6]*tmp[2] + mat[7]*tmp[5] + mat[8]*tmp[8];
|
|
res[3] = mat[0]*tmp[1] + mat[1]*tmp[4] + mat[2]*tmp[7];
|
|
res[4] = mat[0]*tmp[2] + mat[1]*tmp[5] + mat[2]*tmp[8];
|
|
res[5] = mat[3]*tmp[2] + mat[4]*tmp[5] + mat[5]*tmp[8];
|
|
|
|
// res_rot -= mass * dif_cross * dif_cross
|
|
res[0] += mass*(dif[1]*dif[1] + dif[2]*dif[2]);
|
|
res[1] += mass*(dif[0]*dif[0] + dif[2]*dif[2]);
|
|
res[2] += mass*(dif[0]*dif[0] + dif[1]*dif[1]);
|
|
res[3] -= mass*dif[0]*dif[1];
|
|
res[4] -= mass*dif[0]*dif[2];
|
|
res[5] -= mass*dif[1]*dif[2];
|
|
|
|
// res_tran = mass * dif
|
|
res[6] = mass*dif[0];
|
|
res[7] = mass*dif[1];
|
|
res[8] = mass*dif[2];
|
|
|
|
// res_mass = mass
|
|
res[9] = mass;
|
|
}
|
|
|
|
|
|
|
|
// multiply 6D vector (rotation, translation) by 6D inertia matrix
|
|
void mju_mulInertVec(mjtNum res[6], const mjtNum i[10], const mjtNum v[6]) {
|
|
res[0] = i[0]*v[0] + i[3]*v[1] + i[4]*v[2] - i[8]*v[4] + i[7]*v[5];
|
|
res[1] = i[3]*v[0] + i[1]*v[1] + i[5]*v[2] + i[8]*v[3] - i[6]*v[5];
|
|
res[2] = i[4]*v[0] + i[5]*v[1] + i[2]*v[2] - i[7]*v[3] + i[6]*v[4];
|
|
res[3] = i[8]*v[1] - i[7]*v[2] + i[9]*v[3];
|
|
res[4] = i[6]*v[2] - i[8]*v[0] + i[9]*v[4];
|
|
res[5] = i[7]*v[0] - i[6]*v[1] + i[9]*v[5];
|
|
}
|
|
|
|
|
|
|
|
// express motion axis in com-based frame
|
|
void mju_dofCom(mjtNum res[6], const mjtNum axis[3], const mjtNum offset[3]) {
|
|
// hinge
|
|
if (offset) {
|
|
mju_copy3(res, axis);
|
|
mju_cross(res+3, axis, offset);
|
|
}
|
|
|
|
// slide
|
|
else {
|
|
mju_zero3(res);
|
|
mju_copy3(res+3, axis);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// multiply dof matrix (6-by-n, transposed) by vector (n-by-1)
|
|
void mju_mulDofVec(mjtNum* res, const mjtNum* dof, const mjtNum* vec, int n) {
|
|
if (n == 1) {
|
|
mju_scl(res, dof, vec[0], 6);
|
|
} else if (n <= 0) {
|
|
mju_zero(res, 6);
|
|
} else {
|
|
mju_mulMatTVec(res, dof, vec, n, 6);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// transform 6D motion or force vector between frames
|
|
// rot is 3-by-3 matrix; flg_force determines vector type (motion or force)
|
|
void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force,
|
|
const mjtNum newpos[3], const mjtNum oldpos[3],
|
|
const mjtNum rotnew2old[9]) {
|
|
mjtNum cros[3], dif[3], tran[6];
|
|
|
|
// apply translation
|
|
mju_copy(tran, vec, 6);
|
|
mju_sub3(dif, newpos, oldpos);
|
|
if (flg_force) {
|
|
mju_cross(cros, dif, vec+3);
|
|
mju_sub3(tran, vec, cros);
|
|
} else {
|
|
mju_cross(cros, dif, vec);
|
|
mju_sub3(tran+3, vec+3, cros);
|
|
}
|
|
|
|
// apply rotation if provided
|
|
if (rotnew2old) {
|
|
mju_rotVecMatT(res, tran, rotnew2old);
|
|
mju_rotVecMatT(res+3, tran+3, rotnew2old);
|
|
}
|
|
|
|
// otherwise copy
|
|
else {
|
|
mju_copy(res, tran, 6);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// make 3D frame given X axis (normal) and possibly Y axis (tangent 1)
|
|
void mju_makeFrame(mjtNum frame[9]) {
|
|
mjtNum tmp[3];
|
|
|
|
// normalize xaxis
|
|
if (mju_normalize3(frame) < 0.5) {
|
|
mjERROR("xaxis of contact frame undefined");
|
|
}
|
|
|
|
// if yaxis undefined, set yaxis to (0,1,0) if possible, otherwise (0,0,1)
|
|
if (mju_norm3(frame+3) < 0.5) {
|
|
mju_zero3(frame+3);
|
|
|
|
if (frame[1] < 0.5 && frame[1] > -0.5) {
|
|
frame[4] = 1;
|
|
} else {
|
|
frame[5] = 1;
|
|
}
|
|
}
|
|
|
|
// make yaxis orthogonal to xaxis
|
|
mju_scl3(tmp, frame, mju_dot3(frame, frame+3));
|
|
mju_subFrom3(frame+3, tmp);
|
|
mju_normalize3(frame+3);
|
|
|
|
// zaxis = cross(xaxis, yaxis)
|
|
mju_cross(frame+6, frame, frame+3);
|
|
}
|