From f713e81297246c405aead50c001a1be626e80ce6 Mon Sep 17 00:00:00 2001
From: Nimrod Gileadi <nimrod@google.com>
Date: Wed, 25 May 2022 09:21:21 -0700
Subject: [PATCH] Explicitly reset xipos for worldbody.

Before this change, xipos[0,1,2] was only set to 0 by mj_resetData zeroing the entire buffer.

When using mj_resetDataDebug, the value would be wrong.

PiperOrigin-RevId: 450939814
Change-Id: I887dd977161c7d132a80b766db7be23350ffb50b
---
 src/engine/engine_core_smooth.c        |  1 +
 test/engine/engine_core_smooth_test.cc | 21 +++++++++++++++++++++
 2 files changed, 22 insertions(+)

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<mjtNum> GetVector(const mjtNum* array, int length) {
   return std::vector<mjtNum>(array, array + length);
 }
 
+// --------------------------- mj_kinematics -----------------------------------
+
+TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+    </worldbody>
+  </mujoco>
+  )";
+  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) {