Fix quaternion typos.
PiperOrigin-RevId: 453203844 Change-Id: I7d788c21bb00b7746f941fb2cf755f0b046e35f2
This commit is contained in:
committed by
Copybara-Service
parent
16baac9353
commit
53d3220fcc
@@ -49,7 +49,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
|
||||
// normalize all quaternions in qpos
|
||||
mj_normalizeQuat(m, d->qpos);
|
||||
|
||||
// normalize mocap quaterions
|
||||
// normalize mocap quaternions
|
||||
for (int i=0; i<m->nmocap; i++) {
|
||||
mju_normalize4(d->mocap_quat+4*i);
|
||||
}
|
||||
|
||||
@@ -72,7 +72,7 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) {
|
||||
rnd[0] = mju_standardNormal(rnd+1);
|
||||
rnd[2] = mju_standardNormal(rnd+3);
|
||||
|
||||
// scale angle, normalize axis, make quaterion
|
||||
// scale angle, normalize axis, make quaternion
|
||||
rnd[0] *= noise;
|
||||
mju_normalize3(rnd+1);
|
||||
mju_axisAngle2Quat(quat, rnd+1, rnd[0]);
|
||||
|
||||
@@ -1199,7 +1199,7 @@ void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum
|
||||
|
||||
|
||||
|
||||
// normalize all quaterions in qpos-type vector
|
||||
// normalize all quaternions in qpos-type vector
|
||||
void mj_normalizeQuat(const mjModel* m, mjtNum* qpos) {
|
||||
// find quaternion fields and normalize
|
||||
for (int i=0; i<m->njnt; i++) {
|
||||
|
||||
@@ -165,7 +165,7 @@ void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) {
|
||||
|
||||
|
||||
|
||||
// convert 3D rotation matrix to quaterion
|
||||
// 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) {
|
||||
@@ -214,7 +214,7 @@ void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]) {
|
||||
|
||||
|
||||
|
||||
// integrate quaterion given 3D angular velocity
|
||||
// integrate quaternion given 3D angular velocity
|
||||
void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) {
|
||||
mjtNum angle, tmp[4], qrot[4];
|
||||
|
||||
@@ -257,7 +257,7 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) {
|
||||
return;
|
||||
}
|
||||
|
||||
// make quaterion from angle and axis
|
||||
// make quaternion from angle and axis
|
||||
a = mju_atan2(a, mju_dot3(vn, z));
|
||||
mju_axisAngle2Quat(quat, axis, a);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user