diff --git a/doc/changelog.rst b/doc/changelog.rst index 471e7105..e03ebdd4 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -12,6 +12,11 @@ General for the optimization. This allows to decrease the number of initial points needed for finding the contacts and is more robust for very small or large geom sizes. +Bug fixes +^^^^^^^^^ +- Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped. + Before this fix such actuators could lead to non-conservation of momentum. + Version 3.0.1 (November 15, 2023) --------------------------------- diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index a8194d82..d86997a4 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1032,6 +1032,34 @@ void mj_transmission(const mjModel* m, mjData* d) { int refid = m->actuator_trnid[2*i+1]; if (!jacref) jacref = mj_stackAllocNum(d, 3*nv); + // intialize last dof address for each body + int b0 = m->body_weldid[m->site_bodyid[id]]; + int b1 = m->body_weldid[m->site_bodyid[refid]]; + int dofadr0 = m->body_dofadr[b0] + m->body_dofnum[b0] - 1; + int dofadr1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1; + + // find common ancestral dof, if any + int dofadr_common = -1; + if (dofadr0 >= 0 && dofadr1 >= 0) { + // traverse up the tree until common ancestral dof is found + while (dofadr0 != dofadr1) { + if (dofadr0 < dofadr1) { + dofadr1 = m->dof_parentid[dofadr1]; + } else { + dofadr0 = m->dof_parentid[dofadr0]; + } + if (dofadr0 == -1 || dofadr1 == -1) { + // reached tree root, no common ancestral dof + break; + } + } + + // found common ancestral dof + if (dofadr0 == dofadr1) { + dofadr_common = dofadr0; + } + } + // clear moment mju_zero(moment+i*nv, nv); @@ -1050,6 +1078,15 @@ void mj_transmission(const mjModel* m, mjData* d) { // subtract jacref from jac mju_subFrom(jac, jacref, 3*nv); + // if common ancestral dof was found, clear the columns of its parental chain + int da = dofadr_common; + while (da >= 0) { + jac[nv*0 + da] = 0; + jac[nv*1 + da] = 0; + jac[nv*2 + da] = 0; + da = m->dof_parentid[da]; + } + // wrench: translational gear expressed in global frame mju_rotVecMat(wrench, gear, d->site_xmat+9*refid); @@ -1077,6 +1114,15 @@ void mj_transmission(const mjModel* m, mjData* d) { // subtract jacref from jacS mju_subFrom(jacS, jacref, 3*nv); + // if common ancestral dof was found, clear the columns of its parental chain + int da = dofadr_common; + while (da >= 0) { + jacS[nv*0 + da] = 0; + jacS[nv*1 + da] = 0; + jacS[nv*2 + da] = 0; + da = m->dof_parentid[da]; + } + // wrench: rotational gear expressed in global frame mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 52f95e50..dcb123d0 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -294,6 +294,30 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) { mj_deleteModel(model); } +// Test Cartesian position control w.r.t moving refsite +TEST_F(CoreSmoothTest, RefsiteConservesMomentum) { + constexpr char kRefsitePath[] = "engine/testdata/actuation/refsite_free.xml"; + const std::string xml_path = GetTestDataFilePath(kRefsitePath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + + data->ctrl[0] = 1; + data->ctrl[1] = -1; + + // simulate, assert that momentum is conserved + mjtNum eps = 1e-9; + while (data->time < 1) { + mj_step(model, data); + for (int i=0; i < 6; i++) { + EXPECT_LT(mju_abs(data->sensordata[i]), eps); + } + } + + mj_deleteData(data); + mj_deleteModel(model); +} + static const char* const kIlslandEfcPath = "engine/testdata/island/island_efc.xml"; diff --git a/test/engine/testdata/actuation/refsite_free.xml b/test/engine/testdata/actuation/refsite_free.xml new file mode 100644 index 00000000..f61e8f1a --- /dev/null +++ b/test/engine/testdata/actuation/refsite_free.xml @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +