diff --git a/doc/APIreference.rst b/doc/APIreference.rst index 9bfaa2e4..9f4baa52 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -4358,6 +4358,17 @@ mj_jacBodyCom Compute body center-of-mass end-effector Jacobian. +.. _mj_jacSubtreeCom: + +mj_jacSubtreeCom +~~~~~~~~~~~~~~~~ + +.. code-block:: C + + void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body); + +Compute subtree center-of-mass end-effector Jacobian. ``jacp`` is 3 x nv. + .. _mj_jacGeom: mj_jacGeom diff --git a/doc/changelog.rst b/doc/changelog.rst index e57408ca..21b5bc4c 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,8 +2,10 @@ Changelog ========= -.. Upcoming version (not yet released) -.. ----------------------------------- +Upcoming version (not yet released) +----------------------------------- + +- Added :ref:`mj_jacSubtreeCom` for computing the translational Jacobian of the center-of-mass of a subtree. Version 2.2.1 (July 18, 2022) ----------------------------- diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 8434044a..55c15694 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -386,6 +386,9 @@ MJAPI void mj_jacBody(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* j // Compute body center-of-mass end-effector Jacobian. MJAPI void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body); +// Compute subtree center-of-mass end-effector Jacobian. +MJAPI void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body); + // Compute geom end-effector Jacobian. MJAPI void mj_jacGeom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int geom); diff --git a/introspect/functions.py b/introspect/functions.py index a08f494c..e06bfef8 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -2010,6 +2010,36 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Compute body center-of-mass end-effector Jacobian.', )), + ('mj_jacSubtreeCom', + FunctionDecl( + name='mj_jacSubtreeCom', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData'), + ), + ), + FunctionParameterDecl( + name='jacp', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + FunctionParameterDecl( + name='body', + type=ValueType(name='int'), + ), + ), + doc='Compute subtree center-of-mass end-effector Jacobian.', + )), ('mj_jacGeom', FunctionDecl( name='mj_jacGeom', diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index dd1b7ddf..72835b4b 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -154,6 +154,35 @@ void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr +// compute subtree-com Jacobian +void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { + int nv = m->nv; + mjMARKSTACK; + mjtNum* jacp_b = mj_stackAlloc(d, 3*nv); + + // clear output + mju_zero(jacp, 3*nv); + + // forward pass starting from body + for (int b=body; bnbody; b++) { + // end of body subtree, break from the loop + if (b > body && m->body_parentid[b] < body) { + break; + } + + // b is in the body subtree, add mass-weighted Jacobian into jacp + mj_jac(m, d, jacp_b, NULL, d->xipos+3*b, b); + mju_addToScl(jacp, jacp_b, m->body_mass[b], 3*nv); + } + + // normalize by subtree mass + mju_scl(jacp, jacp, 1/m->body_subtreemass[body], 3*nv); + + mjFREESTACK; +} + + + // compute geom Jacobian void mj_jacGeom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int geom) { mj_jac(m, d, jacp, jacr, d->geom_xpos + 3*geom, m->geom_bodyid[geom]); diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index 7c87a2d3..324b9759 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -43,6 +43,9 @@ MJAPI void mj_jacBody(const mjModel* m, const mjData* d, MJAPI void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body); +// compute subtree center-of-mass Jacobian +MJAPI void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body); + // compute geom Jacobian MJAPI void mj_jacGeom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int geom); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 4e9d57f2..2c51ff58 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -16,14 +16,143 @@ #include +#include +#include + #include #include "test/fixture.h" -#include - namespace mujoco { namespace { +using ::testing::DoubleNear; +using JacobianTest = MujocoTest; +static const mjtNum max_abs_err = std::numeric_limits::epsilon(); + +static constexpr char kJacobianTestingModel[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +// compare analytic and finite-differenced subtree-com Jacobian +TEST_F(JacobianTest, SubtreeJac) { + mjModel* model = LoadModelFromString(kJacobianTestingModel); + int nv = model->nv; + int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); + mjData* data = mj_makeData(model); + mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + mjtNum* qpos = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nq); + mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv); + + // all we need for Jacobians are kinematics and CoM-related quantitites + mj_kinematics(model, data); + mj_comPos(model, data); + + // get subtree CoM Jacobian of free body + mj_jacSubtreeCom(model, data, jac_subtree, bodyid); + + // save current subtree-com and qpos, clear nudge + mjtNum subtree_com[3]; + mju_copy3(subtree_com, data->subtree_com+3*bodyid); + mju_copy(qpos, data->qpos, model->nq); + mju_zero(nudge, nv); + + // compare analytic Jacobian to finite-difference approximation + static const mjtNum eps = 1e-6; + for (int i=0; iqpos, reset nudge + mju_copy(data->qpos, qpos, model->nq); + nudge[i] = 1; + mj_integratePos(model, data->qpos, nudge, eps); + nudge[i] = 0; + + // kinematics and comPos to get nudged com + mj_kinematics(model, data); + mj_comPos(model, data); + + // compare finite-differenced and analytic Jacobian + for (int j=0; j<3; j++) { + mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps; + EXPECT_THAT(jac_subtree[nv*j+i], DoubleNear(findiff, eps)); + } + } + + mju_free(nudge); + mju_free(qpos); + mju_free(jac_subtree); + mj_deleteData(data); + mj_deleteModel(model); +} + +// confirm that applying linear forces via the subtree-com Jacobian only creates +// the expected linear accelerations (no accelerations of internal joints) +TEST_F(JacobianTest, SubtreeJacNoInternalAcc) { + mjModel* model = LoadModelFromString(kJacobianTestingModel); + int nv = model->nv; + int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); + mjData* data = mj_makeData(model); + mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + + // all we need for Jacobians are kinematics and CoM-related quantitites + mj_kinematics(model, data); + mj_comPos(model, data); + + // get subtree CoM Jacobian of free body + mj_jacSubtreeCom(model, data, jac_subtree, bodyid); + + // uncomment for debugging + // mju_printMat(jac_subtree, 3, nv); + + // call fwdPosition since we'll need the factorised mass matrix in the test + mj_fwdPosition(model, data); + + // treating the subtree Jacobian as the projection of 3 axis-aligned unit + // forces into joint space, solve for the resulting accelerations in-place + mj_solveM(model, data, jac_subtree, jac_subtree, 3); + + // expect to find accelerations of magnitude 1/subtreemass in the first 3 + // coordinates of the free joint and 0s elsewhere, since applying forces to + // the CoM should accelerate the whole mechanism without any internal motion + int body_dofadr = model->body_dofadr[bodyid]; + mjtNum invtreemass = 1.0/model->body_subtreemass[bodyid]; + for (int r=0; r<3; r++) { + for (int c=0; c