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
This commit is contained in:
Nimrod Gileadi
2022-05-25 09:21:21 -07:00
committed by Copybara-Service
parent 4973f0d7f8
commit f713e81297
2 changed files with 22 additions and 0 deletions
+1
View File
@@ -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;