diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 548377f0..6401f2b2 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -40,6 +40,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { // set world position and orientation mju_zero3(d->xpos); mju_unit4(d->xquat); + mju_zero3(d->xipos); mju_zero(d->xmat, 9); mju_zero(d->ximat, 9); d->xmat[0] = d->xmat[4] = d->xmat[8] = 1; diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index ad50d05e..6b28eb8b 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -33,6 +33,27 @@ static std::vector GetVector(const mjtNum* array, int length) { return std::vector(array, array + length); } +// --------------------------- mj_kinematics ----------------------------------- + +TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) { + constexpr char xml[] = R"( + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, testing::NotNull()); + mjData* data = mj_makeData(model); + + mj_resetDataDebug(model, data, 'd'); + mj_kinematics(model, data); + EXPECT_THAT(GetVector(&data->xipos[0], 3), ElementsAre(0, 0, 0)); + + mj_deleteData(data); + mj_deleteModel(model); +} + // --------------------------- connect constraint ------------------------------ TEST_F(CoreSmoothTest, RnePostConnectForceSlide) {