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

Fix frame accumulation order for <frame> meta-element.

Browse Source 
PiperOrigin-RevId: 588803425
Change-Id: Id0289c34d1a6cfb6007540a29cc1f32d879e43ed
This commit is contained in:
Alessio Quaglino
2023-12-07 08:35:46 -08:00
committed by Copybara-Service
parent d12fe594d9
commit 1db0a9946f
4 changed files with 82 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+7 -7
View File
@@ -993,7 +993,7 @@ void mjCBody::Compile(void) {
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
}
// accumulate rbound, contype, conaffinity over geoms
@@ -1101,7 +1101,7 @@ void mjCFrame::Compile() {
// compile parents and accumulate result
if (frame) {
frame->Compile();
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
}
mjuu_normvec(quat, 4);
@@ -1263,7 +1263,7 @@ int mjCJoint::Compile(void) {
mjuu_zerovec(pos, 3);
} else if (frame) {
double qunit[4] = {1, 0, 0, 0};
mjuu_frameaccum(pos, qunit, frame->pos, frame->quat);
mjuu_frameaccumChild(frame->pos, frame->quat, pos, qunit);
}
// convert reference angles to radians for hinge joints
@@ -1878,7 +1878,7 @@ void mjCGeom::Compile(void) {
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
}
}
@@ -1990,7 +1990,7 @@ void mjCSite::Compile(void) {
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
}
// normalize quaternion
@@ -2055,7 +2055,7 @@ void mjCCamera::Compile(void) {
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
}
// normalize quaternion
@@ -2159,7 +2159,7 @@ void mjCLight::Compile(void) {
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
}
// normalize direction, make sure it is not zero
src/user/user_util.cc
+21 -10
View File
@@ -367,8 +367,8 @@ void mjuu_frame2quat(double* quat, const double* x, const double* y, const doubl
// invert frame transformation
void mjuu_frameinvert(double* newpos, double* newquat,
const double* oldpos, const double* oldquat) {
void mjuu_frameinvert(double newpos[3], double newquat[4],
const double oldpos[3], const double oldquat[4]) {
// position
mjuu_localaxis(newpos, oldpos, oldquat);
newpos[0] = -newpos[0];
@@ -384,28 +384,39 @@ void mjuu_frameinvert(double* newpos, double* newquat,
// accumulate frame transformations (forward kinematics)
void mjuu_frameaccum(double* pos, double* quat,
const double* addpos, const double* addquat) {
void mjuu_frameaccum(double pos[3], double quat[4],
const double childpos[3], const double childquat[4]) {
double mat[9], vec[3], qtmp[4];
mjuu_quat2mat(mat, quat);
mjuu_mulvecmat(vec, addpos, mat);
mjuu_mulvecmat(vec, childpos, mat);
pos[0] += vec[0];
pos[1] += vec[1];
pos[2] += vec[2];
mjuu_mulquat(qtmp, quat, addquat);
mjuu_mulquat(qtmp, quat, childquat);
mjuu_copyvec(quat, qtmp, 4);
}
// accumulate frame transformation in second frame
void mjuu_frameaccumChild(const double pos[3], const double quat[4],
double childpos[3], double childquat[4]) {
double p[] = {pos[0], pos[1], pos[2]};
double q[] = {quat[0], quat[1], quat[2], quat[3]};
mjuu_frameaccum(p, q, childpos, childquat);
mjuu_copyvec(childpos, p, 3);
mjuu_copyvec(childquat, q, 4);
}
// invert frame accumulation
void mjuu_frameaccuminv(double* pos, double* quat,
const double* addpos, const double* addquat) {
void mjuu_frameaccuminv(double pos[3], double quat[4],
const double childpos[3], const double childquat[4]) {
double mat[9], vec[3], qtmp[4];
double qneg[4] = {addquat[0], -addquat[1], -addquat[2], -addquat[3]};
double qneg[4] = {childquat[0], -childquat[1], -childquat[2], -childquat[3]};
mjuu_mulquat(qtmp, quat, qneg);
mjuu_copyvec(quat, qtmp, 4);
mjuu_quat2mat(mat, quat);
mjuu_mulvecmat(vec, addpos, mat);
mjuu_mulvecmat(vec, childpos, mat);
pos[0] -= vec[0];
pos[1] -= vec[1];
pos[2] -= vec[2];
src/user/user_util.h
+11 -7
View File
@@ -104,16 +104,20 @@ void mjuu_z2quat(double* quat, const double* vec);
void mjuu_frame2quat(double* quat, const double* x, const double* y, const double* z);
// invert frame transformation
void mjuu_frameinvert(double* newpos, double* newquat,
const double* oldpos, const double* oldquat);
void mjuu_frameinvert(double newpos[3], double newquat[4],
const double oldpos[3], const double oldquat[4]);
// accumulate frame transformations
void mjuu_frameaccum(double* pos, double* quat,
const double* addpos, const double* addquat);
// accumulate frame transformation into parent frame
void mjuu_frameaccum(double pos[3], double quat[4],
const double childpos[3], const double childquat[4]);
// accumulate frame transformation into child frame
void mjuu_frameaccumChild(const double pos[3], const double quat[4],
double childpos[3], double childquat[4]);
// invert frame accumulation
void mjuu_frameaccuminv(double* pos, double* quat,
const double* addpos, const double* addquat);
void mjuu_frameaccuminv(double pos[3], double quat[4],
const double childpos[3], const double childquat[4]);
// convert local_inertia[3] to global_inertia[6]
void mjuu_globalinertia(double* global, const double* local, const double* quat);