chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Add mj_jacSubtreeCom for computing subtree center-of-mass Jacobians.

Browse Source 
PiperOrigin-RevId: 464075727
Change-Id: If679c0fdd05b7dc84154cec00963aeea7bbdacc3
This commit is contained in:
Yuval Tassa
2022-07-29 07:43:52 -07:00
committed by Copybara-Service
parent cd489fc72d
commit fcf4131771
7 changed files with 211 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference.rst
+11
View File
@@ -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
doc/changelog.rst
+4 -2
View File
@@ -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)
-----------------------------
include/mujoco/mujoco.h
+3
View File
@@ -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);
introspect/functions.py
+30
View File
@@ -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',
src/engine/engine_support.c
+29
View File
@@ -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; b<m->nbody; 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]);
src/engine/engine_support.h
+3
View File
@@ -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);
test/engine/engine_support_test.cc
+131 -2
View File
@@ -16,14 +16,143 @@
#include <string_view>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
#include <gtest/gtest.h>
namespace mujoco {
namespace {
using ::testing::DoubleNear;
using JacobianTest = MujocoTest;
static const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
static constexpr char kJacobianTestingModel[] = R"(
<mujoco>
<worldbody>
<body name="distractor1" pos="0 0 .3">
<freejoint/>
<geom size=".1"/>
</body>
<body name="main">
<freejoint/>
<geom size=".1"/>
<body pos=".1 0 0">
<joint axis="0 1 0"/>
<geom type="capsule" size=".03" fromto="0 0 0 .2 0 0"/>
</body>
<body pos="0 .1 0">
<joint type="ball"/>
<geom type="capsule" size=".03" fromto="0 0 0 0 .2 0"/>
<body pos="0 .2 0">
<joint type="slide" axis="1 1 1"/>
<geom size=".05"/>
</body>
</body>
</body>
<body name="distractor2" pos="0 0 -.3">
<freejoint/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
// 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; i<nv; i++) {
// reset qpos, nudge i-th dof, update data->qpos, 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<nv; c++) {
mjtNum expected = c-body_dofadr==r ? invtreemass : 0.0;
EXPECT_THAT(jac_subtree[nv*r+c], DoubleNear(expected, max_abs_err));
}
}
mju_free(jac_subtree);
mj_deleteData(data);
mj_deleteModel(model);
}
using VersionTest = MujocoTest;
const char *const kExpectedVersionString = "2.2.1";