From c931565fdceb9b6ed044cd2499dc4d038334766f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 25 Mar 2025 09:35:14 -0700 Subject: [PATCH] Add internal function`mj_tendonDot`: time derivative of tendon Jacobian for one tendon. Notes: - Currently always uses dense math, even for sparse models. This should be easy to change in the future. - Does not support geom wrapping. This is possible but harder, requires derivatives of mju_wrap. PiperOrigin-RevId: 740378741 Change-Id: Id39ef2c4bfbb7ee11ec33c97d7d83140441cdab2 --- src/engine/engine_core_smooth.c | 122 ++++++++++++++++++++ src/engine/engine_core_smooth.h | 3 + test/engine/engine_core_smooth_test.cc | 54 +++++++++ test/engine/testdata/core_smooth/ten_J0.xml | 26 +++++ test/engine/testdata/core_smooth/ten_J1.xml | 29 +++++ test/engine/testdata/core_smooth/ten_J2.xml | 34 ++++++ test/engine/testdata/core_smooth/ten_J3.xml | 66 +++++++++++ 7 files changed, 334 insertions(+) create mode 100644 test/engine/testdata/core_smooth/ten_J0.xml create mode 100644 test/engine/testdata/core_smooth/ten_J1.xml create mode 100644 test/engine/testdata/core_smooth/ten_J2.xml create mode 100644 test/engine/testdata/core_smooth/ten_J3.xml diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 9b453c3e..009860eb 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -861,6 +861,128 @@ void mj_tendon(const mjModel* m, mjData* d) { +// compute time derivative of dense tendon Jacobian for one tendon +void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { + int nv = m->nv; + + // allocate stack arrays + mjtNum *jac1, *jac2, *jacdif, *tmp; + mj_markStack(d); + jac1 = mjSTACKALLOC(d, 3*nv, mjtNum); + jac2 = mjSTACKALLOC(d, 3*nv, mjtNum); + jacdif = mjSTACKALLOC(d, 3*nv, mjtNum); + tmp = mjSTACKALLOC(d, nv, mjtNum); + + // return if tendon id is invalid + if (id < 0 || id >= m->ntendon) { + return; + } + + // clear output + mju_zero(Jdot, nv); + + // fixed tendon has zero Jdot: return + int adr = m->tendon_adr[id]; + if (m->wrap_type[adr] == mjWRAP_JOINT) { + return; + } + + // process spatial tendon + mjtNum divisor = 1; + int wraptype, j = 0; + int num = m->tendon_num[id]; + while (j < num-1) { + // get 1st and 2nd object + int type0 = m->wrap_type[adr+j+0]; + int type1 = m->wrap_type[adr+j+1]; + int id0 = m->wrap_objid[adr+j+0]; + int id1 = m->wrap_objid[adr+j+1]; + + // pulley + if (type0 == mjWRAP_PULLEY || type1 == mjWRAP_PULLEY) { + // get divisor, insert obj=-2 + if (type0 == mjWRAP_PULLEY) { + divisor = m->wrap_prm[adr+j]; + } + + // move to next + j++; + continue; + } + + // init sequence; assume it starts with site + mjtNum wpnt[6]; + mju_copy3(wpnt, d->site_xpos+3*id0); + mjtNum vel[6]; + mj_objectVelocity(m, d, mjOBJ_SITE, id0, vel, /*flg_local=*/0); + mjtNum wvel[6] = {vel[3], vel[4], vel[5], 0, 0, 0}; + int wbody[2]; + wbody[0] = m->site_bodyid[id0]; + + // second object is geom: process site-geom-site + if (type1 == mjWRAP_SPHERE || type1 == mjWRAP_CYLINDER) { + // TODO(tassa) support geom wrapping (requires derivatives of mju_wrap) + mjERROR("geom wrapping not supported"); + } else { + wraptype = mjWRAP_NONE; + } + + // complete sequence + wbody[1] = m->site_bodyid[id1]; + mju_copy3(wpnt+3, d->site_xpos+3*id1); + mj_objectVelocity(m, d, mjOBJ_SITE, id1, vel, /*flg_local=*/0); + mju_copy3(wvel+3, vel+3); + + // accumulate moments if consecutive points are in different bodies + if (wbody[0] != wbody[1]) { + // dpnt = 3D position difference, normalize + mjtNum dpnt[3]; + mju_sub3(dpnt, wpnt+3, wpnt); + mjtNum norm = mju_norm3(dpnt); + mju_scl3(dpnt, dpnt, 1/norm); + + // dvel = d / dt (dpnt) + mjtNum dvel[3]; + mju_sub3(dvel, wvel+3, wvel); + mjtNum dot = mju_dot3(dpnt, dvel); + mju_addToScl3(dvel, dpnt, -dot); + mju_scl3(dvel, dvel, 1/norm); + + // TODO(tassa ) write sparse branch, requires mj_jacDotSparse + // if (mj_isSparse(m)) { ... } + + // get endpoint JacobianDots, subtract + mj_jacDot(m, d, jac1, 0, wpnt, wbody[0]); + mj_jacDot(m, d, jac2, 0, wpnt+3, wbody[1]); + mju_sub(jacdif, jac2, jac1, 3*nv); + + // chain rule, first term: Jdot += d/dt(jac2 - jac1) * dpnt + mju_mulMatTVec(tmp, jacdif, dpnt, 3, nv); + + // add to existing + mju_addToScl(Jdot, tmp, 1/divisor, nv); + + // get endpoint Jacobians, subtract + mj_jac(m, d, jac1, 0, wpnt, wbody[0]); + mj_jac(m, d, jac2, 0, wpnt+3, wbody[1]); + mju_sub(jacdif, jac2, jac1, 3*nv); + + // chain rule, second term: Jdot += (jac2 - jac1) * d/dt(dpnt) + mju_mulMatTVec(tmp, jacdif, dvel, 3, nv); + + // add to existing + mju_addToScl(Jdot, tmp, 1/divisor, nv); + } + + // advance + j += (wraptype != mjWRAP_NONE ? 2 : 1); + } + + mj_freeStack(d); +} + + + // compute actuator/transmission lengths and moments void mj_transmission(const mjModel* m, mjData* d) { int nv = m->nv, nu = m->nu; diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index 7f1b9fb3..41d06775 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -39,6 +39,9 @@ MJAPI void mj_flex(const mjModel* m, mjData* d); // compute tendon lengths, velocities and moment arms MJAPI void mj_tendon(const mjModel* m, mjData* d); +// compute time derivative of dense tendon Jacobian for one tendon +MJAPI void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot); + // compute actuator transmission lengths and moments MJAPI void mj_transmission(const mjModel* m, mjData* d); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 3263acc5..8f24a99a 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -159,6 +159,60 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) { mj_deleteModel(model); } +static const char* const kTen_J0 = "engine/testdata/core_smooth/ten_J0.xml"; +static const char* const kTen_J1 = "engine/testdata/core_smooth/ten_J1.xml"; +static const char* const kTen_J2 = "engine/testdata/core_smooth/ten_J2.xml"; +static const char* const kTen_J3 = "engine/testdata/core_smooth/ten_J3.xml"; + +TEST_F(CoreSmoothTest, TendonJdot) { + for (const char* local_path : {kTen_J0, kTen_J1, kTen_J2, kTen_J3}) { + const std::string xml_path = GetTestDataFilePath(local_path); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + int nv = m->nv; + ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; + EXPECT_EQ(m->ntendon, 1); + mjData* d = mj_makeData(m); + + for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) { + m->opt.jacobian = sparsity; + + if (m->nkey) { + mj_resetDataKeyframe(m, d, 0); + } else { + mj_resetData(m, d); + while (d->time < 1) { + mj_step(m, d); + } + } + + mj_forward(m, d); + + // get current J and Jdot for the tendon + vector ten_J(d->ten_J, d->ten_J + nv); + vector ten_Jdot(nv, 0); + mj_tendonDot(m, d, 0, ten_Jdot.data()); + + // compute finite-differenced Jdot + mjtNum h = 1e-7; + mj_integratePos(m, d->qpos, d->qvel, h); + mj_kinematics(m, d); + mj_comPos(m, d); + mj_tendon(m, d); + vector ten_Jh(d->ten_J, d->ten_J + nv); + mju_subFrom(ten_Jh.data(), ten_J.data(), nv); + mju_scl(ten_Jh.data(), ten_Jh.data(), 1.0 / h, nv); + + // expect analytic and FD derivatives to be similar to eps precision + mjtNum eps = 1e-6; + EXPECT_THAT(ten_Jdot, Pointwise(DoubleNear(eps), ten_Jh)); + } + + mj_deleteData(d); + mj_deleteModel(m); + } +} + // --------------------------- connect constraint ------------------------------ // test that bodies hanging on connects lead to expected force sensor readings diff --git a/test/engine/testdata/core_smooth/ten_J0.xml b/test/engine/testdata/core_smooth/ten_J0.xml new file mode 100644 index 00000000..b4fec9ba --- /dev/null +++ b/test/engine/testdata/core_smooth/ten_J0.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/core_smooth/ten_J1.xml b/test/engine/testdata/core_smooth/ten_J1.xml new file mode 100644 index 00000000..d5c3cf93 --- /dev/null +++ b/test/engine/testdata/core_smooth/ten_J1.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/core_smooth/ten_J2.xml b/test/engine/testdata/core_smooth/ten_J2.xml new file mode 100644 index 00000000..e5777029 --- /dev/null +++ b/test/engine/testdata/core_smooth/ten_J2.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/core_smooth/ten_J3.xml b/test/engine/testdata/core_smooth/ten_J3.xml new file mode 100644 index 00000000..520969d2 --- /dev/null +++ b/test/engine/testdata/core_smooth/ten_J3.xml @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +