diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc
index 009ecebc..84a9c8c8 100644
--- a/src/user/user_objects.cc
+++ b/src/user/user_objects.cc
@@ -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
diff --git a/src/user/user_util.cc b/src/user/user_util.cc
index 4c75ccce..35e5568d 100644
--- a/src/user/user_util.cc
+++ b/src/user/user_util.cc
@@ -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];
diff --git a/src/user/user_util.h b/src/user/user_util.h
index fa230137..2cede571 100644
--- a/src/user/user_util.h
+++ b/src/user/user_util.h
@@ -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);
diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc
index 37154b13..28750da0 100644
--- a/test/user/user_objects_test.cc
+++ b/test/user/user_objects_test.cc
@@ -1404,38 +1404,47 @@ TEST_F(MujocoTest, Frame) {
-
+
-
-
-
+
+
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
)";
+ constexpr mjtNum eps = 1e-14;
std::array error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, testing::NotNull()) << error.data();
- EXPECT_EQ(m->nbody, 4);
+ EXPECT_EQ(m->nbody, 5);
// geom quat transformed to euler = 0 0 50
EXPECT_NEAR(m->geom_quat[0], mju_cos(25. * mjPI / 180.), 1e-3);
@@ -1444,15 +1453,15 @@ TEST_F(MujocoTest, Frame) {
EXPECT_NEAR(m->geom_quat[3], mju_sin(25. * mjPI / 180.), 1e-3);
// geom transformed to frame 0 1 0, 0 0 1, 1 0 0
- EXPECT_NEAR(m->geom_quat[4], .5, 1e-6);
- EXPECT_NEAR(m->geom_quat[5], .5, 1e-6);
- EXPECT_NEAR(m->geom_quat[6], .5, 1e-6);
- EXPECT_NEAR(m->geom_quat[7], .5, 1e-6);
+ EXPECT_NEAR(m->geom_quat[4], .5, eps);
+ EXPECT_NEAR(m->geom_quat[5], .5, eps);
+ EXPECT_NEAR(m->geom_quat[6], .5, eps);
+ EXPECT_NEAR(m->geom_quat[7], .5, eps);
// geom pos transformed from 0 1 0 to 0 2 0
- EXPECT_EQ(m->geom_pos[6], 0);
- EXPECT_EQ(m->geom_pos[7], 2);
- EXPECT_EQ(m->geom_pos[8], 0);
+ EXPECT_EQ(m->geom_pos[ 9], 0);
+ EXPECT_EQ(m->geom_pos[10], 2);
+ EXPECT_EQ(m->geom_pos[11], 0);
// body pos transformed from 1 0 0 to 1 1 0
EXPECT_EQ(m->body_pos[6], 1);
@@ -1460,16 +1469,29 @@ TEST_F(MujocoTest, Frame) {
EXPECT_EQ(m->body_pos[8], 0);
// nested geom pos not transformed
- EXPECT_EQ(m->geom_pos[ 9], 0);
- EXPECT_EQ(m->geom_pos[10], 0);
- EXPECT_EQ(m->geom_pos[11], 1);
+ EXPECT_EQ(m->geom_pos[12], 0);
+ EXPECT_EQ(m->geom_pos[13], 0);
+ EXPECT_EQ(m->geom_pos[14], 1);
// joint axis transformed to 0 -1 0
- EXPECT_NEAR(m->jnt_axis[0], 0, 1e-6);
- EXPECT_NEAR(m->jnt_axis[1], -1, 1e-6);
- EXPECT_NEAR(m->jnt_axis[2], 0, 1e-6);
+ EXPECT_NEAR(m->jnt_axis[0], 0, eps);
+ EXPECT_NEAR(m->jnt_axis[1], -1, eps);
+ EXPECT_NEAR(m->jnt_axis[2], 0, eps);
+
+ mjData* d = mj_makeData(m);
+ mj_kinematics(m, d);
+
+ // body and frame equivalence geom 2 vs 6
+ EXPECT_NEAR(d->geom_xpos[6], d->geom_xpos[18], eps);
+ EXPECT_NEAR(d->geom_xpos[7], d->geom_xpos[19], eps);
+ EXPECT_NEAR(d->geom_xpos[8], d->geom_xpos[20], eps);
+ EXPECT_NEAR(d->geom_xmat[18], d->geom_xmat[54], eps);
+ EXPECT_NEAR(d->geom_xmat[19], d->geom_xmat[55], eps);
+ EXPECT_NEAR(d->geom_xmat[20], d->geom_xmat[56], eps);
+ EXPECT_NEAR(d->geom_xmat[21], d->geom_xmat[57], eps);
mj_deleteModel(m);
+ mj_deleteData(d);
}
} // namespace