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

Add tendon armature

Browse Source 
PiperOrigin-RevId: 743939992
Change-Id: I587214f5d6fabbc0cc273c33d82decbe9ad8f919
This commit is contained in:
Yuval Tassa
2025-04-04 07:42:40 -07:00
committed by Copybara-Service
parent e1f5ceb65a
commit d05251af2a
22 changed files with 735 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_core_smooth_test.cc
+178
View File
@@ -17,6 +17,7 @@
#include "src/engine/engine_core_smooth.h"
#include "src/engine/engine_util_sparse.h"
#include <algorithm>
#include <string>
#include <string_view>
#include <vector>
@@ -213,6 +214,183 @@ TEST_F(CoreSmoothTest, TendonJdot) {
}
}
static const char* const kTen_offtree =
"engine/testdata/core_smooth/ten_armature_offtree.xml";
TEST_F(CoreSmoothTest, TendonArmature) {
const std::string xml_path = GetTestDataFilePath(kTen_offtree);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
int nv = m->nv;
mjData* d = mj_makeData(m);
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
m->opt.jacobian = sparsity;
mj_forward(m, d);
// get full M, includes both CRB and tendon inertia
vector<mjtNum> M(nv*nv);
mj_fullM(m, M.data(), d->qM);
// put only CRB inertia in M2
mj_crb(m, d);
vector<mjtNum> M2(nv*nv);
mj_fullM(m, M2.data(), d->qM);
vector<mjtNum> ten_J(nv); // tendon Jacobian
vector<mjtNum> ten_M(nv*nv); // tendon inertia
// add tendon inertias to M2 using outer product
for (int j=0; j < m->ntendon; j++) {
// get tendon Jacobian
if (mj_isSparse(m)) {
int rowadr = d->ten_J_rowadr[j];
int* rownnz = d->ten_J_rownnz + j;
int zero = 0;
mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv,
rownnz, &zero, d->ten_J_colind + rowadr);
} else {
mju_copy(ten_J.data(), d->ten_J + j*nv, nv);
}
// get tendon inertia only, using outer product
mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv);
mju_scl(ten_M.data(), ten_M.data(), m->tendon_armature[j], nv * nv);
// manually add values, at nonzeros only
for (int i=0; i < nv*nv; i++) {
if (M[i]) M2[i] += ten_M[i];
}
}
// expect matrices to match
EXPECT_THAT(M2, Pointwise(DoubleNear(1e-9), M));
}
mj_deleteData(d);
mj_deleteModel(m);
}
static const char* const kTen_i0 =
"engine/testdata/core_smooth/ten_armature_0.xml";
static const char* const kTen_i1 =
"engine/testdata/core_smooth/ten_armature_1.xml";
static const char* const kTen_i2 =
"engine/testdata/core_smooth/ten_armature_2.xml";
static const char* const kTen_i3 =
"engine/testdata/core_smooth/ten_armature_3.xml";
static const char* const kTen_i4 =
"engine/testdata/core_smooth/ten_armature_4.xml";
TEST_F(CoreSmoothTest, TendonArmatureConservesEnergy) {
for (const char* local_path : {kTen_i0, kTen_i1, kTen_i2, kTen_i3, kTen_i4}) {
const std::string xml_path = GetTestDataFilePath(local_path);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
mjData* d = mj_makeData(m);
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
m->opt.jacobian = sparsity;
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
double energy_0 = d->energy[0] + d->energy[1];
double eps = std::max(energy_0, 1.0) * 1e-5;
while (d->time < 1) {
mj_step(m, d);
double energy_t = d->energy[0] + d->energy[1];
EXPECT_THAT(energy_t, DoubleNear(energy_0, eps));
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
}
TEST_F(CoreSmoothTest, TendonArmatureConservesMomentum) {
const std::string xml_path = GetTestDataFilePath(kTen_i4);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
mjData* d = mj_makeData(m);
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
m->opt.jacobian = sparsity;
mj_resetData(m, d);
mj_forward(m, d);
// this model contains subtreelinvel and subtreeangmom sensors
vector<mjtNum> sdata_0 = AsVector(d->sensordata, m->nsensordata);
EXPECT_THAT(sdata_0, Each(Eq(0)));
double eps = 1e-5;
while (d->time < 1) {
mj_step(m, d);
vector<mjtNum> sdata_t = AsVector(d->sensordata, m->nsensordata);
EXPECT_THAT(sdata_t, Pointwise(DoubleNear(eps), sdata_0));
}
// momentum is conserved nontrivially (velocities are non-zero)
EXPECT_GT(d->energy[1], 0);
}
mj_deleteData(d);
mj_deleteModel(m);
}
static const char* const kTen_i0_equiv =
"engine/testdata/core_smooth/ten_armature_0_equiv.xml";
static const char* const kTen_i1_equiv =
"engine/testdata/core_smooth/ten_armature_1_equiv.xml";
TEST_F(CoreSmoothTest, TendonInertiaEquivalent) {
for (const char* lpath : {kTen_i0, kTen_i1}) {
// load tendon model
const std::string path = GetTestDataFilePath(lpath);
char error[1024];
mjModel* m = mj_loadXML(path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
int gid = mj_name2id(m, mjOBJ_GEOM, "query");
mjData* d = mj_makeData(m);
if (m->nkey) mj_resetDataKeyframe(m, d, 0);
// load equivalent model
const char* lpath_e = lpath == kTen_i0 ? kTen_i0_equiv : kTen_i1_equiv;
const std::string path_e = GetTestDataFilePath(lpath_e);
mjModel* m_e = mj_loadXML(path_e.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
int gid_e = mj_name2id(m_e, mjOBJ_GEOM, "query");
mjData* d_e = mj_makeData(m_e);
if (m_e->nkey) mj_resetDataKeyframe(m_e, d_e, 0);
// the equality constraint in kTen_i1_equiv reduces precision
double eps = lpath == kTen_i0 ? 1e-6 : 1e-3;
while (d->time < 1) {
mj_step(m, d);
vector<mjtNum> xpos = AsVector(d->geom_xpos + 3*gid, 3);
mj_step(m_e, d_e);
vector<mjtNum> xpos_e = AsVector(d_e->geom_xpos + 3*gid_e, 3);
EXPECT_THAT(xpos, Pointwise(DoubleNear(eps), xpos_e));
}
mj_deleteData(d);
mj_deleteModel(m);
mj_deleteData(d_e);
mj_deleteModel(m_e);
}
}
// --------------------------- connect constraint ------------------------------
// test that bodies hanging on connects lead to expected force sensor readings
test/engine/testdata/core_smooth/ten_armature_0.xml
Vendored
+28
View File
@@ -0,0 +1,28 @@
<mujoco>
<option integrator="RK4">
<flag energy="enable"/>
</option>
<default>
<geom rgba="0.8 0.6 .4 1"/>
<site rgba="0.4 0.8 .6 .5"/>
</default>
<worldbody>
<site name="decoration" type="cylinder" zaxis="0 -1 0" size="1 .02" pos="0 .1 0"/>
<site name="world"/>
<body pos="1 0 0">
<joint name="x" type="slide" axis="1 0 0"/>
<joint name="z" type="slide" axis="0 0 1"/>
<geom name="query" type="sphere" size=".1" mass="1"/>
<site name="body"/>
</body>
</worldbody>
<tendon>
<spatial width=".01" rgba=".2 .2 1 1" springlength=".5" stiffness="100" armature="5">
<site site="world"/>
<site site="body"/>
</spatial>
</tendon>
</mujoco>
test/engine/testdata/core_smooth/ten_armature_0_compare.xml
Vendored
+17
View File
@@ -0,0 +1,17 @@
<mujoco>
<asset>
<model name="tendon_model" file="ten_armature_0.xml"/>
<model name="equivalent_model" file="ten_armature_0_equiv.xml"/>
</asset>
<option integrator="RK4">
<flag energy="enable"/>
</option>
<worldbody>
<attach model="tendon_model" prefix="t_"/>
<frame pos="2.2 0 0">
<attach model="equivalent_model" prefix="e_"/>
</frame>
</worldbody>
</mujoco>
test/engine/testdata/core_smooth/ten_armature_0_equiv.xml
Vendored
+20
View File
@@ -0,0 +1,20 @@
<mujoco>
<option integrator="RK4">
<flag energy="enable"/>
</option>
<default>
<geom rgba="0.8 0.6 .4 1"/>
<site rgba="0.4 0.8 .6 .5"/>
</default>
<worldbody>
<site name="decoration" type="cylinder" zaxis="0 -1 0" size="1 .02" pos="0 .1 0"/>
<body>
<inertial pos="1 0 0" mass="1" diaginertia="1e-8 1e-8 1e-8"/>
<joint name="hinge" axis="0 -1 0"/>
<joint name="slide" type="slide" axis="1 0 0" springref="-.5" stiffness="100" armature="5"/>
<geom name="query" type="sphere" size=".1" pos="1 0 0"/>
</body>
</worldbody>
</mujoco>
test/engine/testdata/core_smooth/ten_armature_1.xml
Vendored
+28
View File
@@ -0,0 +1,28 @@
<mujoco>
<option integrator="RK4" timestep="0.0001">
<flag contact="disable" gravity="disable" energy="enable"/>
</option>
<default>
<geom rgba="0.8 0.6 .4 1"/>
<site rgba="0.4 0.8 .6 .5"/>
</default>
<worldbody>
<site name="tendon" type="box" size=".03 .03 .03" pos=".5 0 -.5"/>
<body name="link1" pos="0 0 0">
<joint name="link1" axis="0 -1 0" stiffness="100" springref="90"/>
<geom type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
<geom name="query" type="sphere" size=".08" pos="0 0 -1"/>
<site name="link1" type="box" size=".03 .03 .03" pos="0 0 -.5"/>
<site name="acc1" pos="0 0 -1"/>
</body>
</worldbody>
<tendon>
<spatial width=".01" rgba=".2 .2 1 1" armature="9">
<site site="link1"/>
<site site="tendon"/>
</spatial>
</tendon>
</mujoco>
test/engine/testdata/core_smooth/ten_armature_1_compare.xml
Vendored
+26
View File
@@ -0,0 +1,26 @@
<mujoco>
<visual>
<global elevation="-20"/>
<headlight ambient=".3 .3 .3" diffuse=".8 .8 .8"/>
</visual>
<asset>
<model name="tendon_model" file="ten_armature_1.xml"/>
<model name="equivalent_model" file="ten_armature_1_equiv.xml"/>
</asset>
<visual>
<global elevation="0"/>
</visual>
<option integrator="RK4" timestep="0.0001">
<flag contact="disable" gravity="disable" energy="enable"/>
</option>
<worldbody>
<attach model="tendon_model" prefix="t_"/>
<frame pos="-1.2 0 0">
<attach model="equivalent_model" prefix="e_"/>
</frame>
</worldbody>
</mujoco>
test/engine/testdata/core_smooth/ten_armature_1_equiv.xml
Vendored
+34
View File
@@ -0,0 +1,34 @@
<mujoco>
<option integrator="RK4" timestep="0.0001">
<flag contact="disable" gravity="disable" energy="enable"/>
</option>
<default>
<geom rgba="0.8 0.6 .4 1"/>
<site rgba="0.4 0.8 .6 .5"/>
</default>
<worldbody>
<site name="connect" type="box" size=".03 .03 .03" pos=".5 0 -.5"/>
<body name="link2" pos="0 0 0">
<joint name="link2" axis="0 -1 0" stiffness="100" springref="90"/>
<geom type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
<geom name="query" type="sphere" size=".08" pos="0 0 -1"/>
<body name="link3" pos="0 0 -.5" euler="0 90 0">
<joint name="link3" axis="0 -1 0"/>
<geom name="link3" type="box" size=".02 .02 .02" mass="1e-3"/>
<geom type="capsule" size=".015" fromto="0 0 0 0 0 .2" mass="0"/>
<body name="link4">
<joint name="link4" type="slide" axis="0 0 1" armature="9"/>
<geom type="capsule" size=".015" fromto="0 0 .3 0 0 .5" mass="0"/>
<geom type="sphere" size=".001" pos="0 0 .5" mass=".1"/>
<site name="link4" type="box" size=".03 .03 .03" pos="0 0 .5"/>
</body>
</body>
</body>
</worldbody>
<equality>
<connect site1="connect" site2="link4" solimp=".99 .99 .001" solref="0.0001 1"/>
</equality>
</mujoco>
test/engine/testdata/core_smooth/ten_armature_2.xml
Vendored
+29
View File
@@ -0,0 +1,29 @@
<mujoco>
<option integrator="RK4" timestep="1e-4">
<flag contact="disable" gravity="disable" energy="enable"/>
</option>
<worldbody>
<site name="tendon" type="box" size=".03 .03 .03" pos="-.5 0 0"/>
<body name="link1" pos="-1 0 0">
<joint name="link1" axis="0 -1 0" damping="0" stiffness="50"/>
<geom name="link1" type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
<body name="link2" pos="0 0 -1">
<joint name="link2" axis="0 -1 0" damping="0" stiffness="1" springref="-180"/>
<geom name="link2" type="capsule" size=".03" fromto="0 0 0 .6 0 0"/>
<site name="link2" type="box" size=".03 .03 .03" pos=".5 0 0"/>
</body>
</body>
</worldbody>
<tendon>
<spatial width=".01" rgba=".2 .2 1 1" armature="9">
<site site="link2"/>
<site site="tendon"/>
</spatial>
</tendon>
<keyframe>
<key qpos="-1 0" qvel="1 1"/>
</keyframe>
</mujoco>
test/engine/testdata/core_smooth/ten_armature_3.xml
Vendored
+39
View File
@@ -0,0 +1,39 @@
<mujoco>
<option integrator="RK4">
<flag contact="disable" gravity="disable" energy="enable"/>
</option>
<worldbody>
<body name="distractor">
<freejoint/>
<geom size=".1"/>
</body>
<body name="thigh" pos="-1 0 0">
<site name="0" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
<joint axis="1 0 0" stiffness="50"/>
<joint axis="0 1 0" stiffness="50"/>
<joint axis="0 0 1" stiffness="50"/>
<geom type="capsule" size=".04" fromto="0 0 0 0 0 -1"/>
<geom type="capsule" size=".02" fromto="0 0 -1 .05 -.08 -1.1"/>
<site name="1" type="box" size=".03 .03 .03" pos=".05 -.08 -1.1"/>
<body name="calf" pos="0 0 -1">
<joint axis="0 -1 0" damping="0" stiffness="20" springref="90"/>
<geom type="capsule" size=".03" fromto="0 0 0 -.6 0 0"/>
<body name="foot" pos="-.6 0 0">
<joint type="ball" stiffness="20" armature=".2"/>
<site name="2" type="box" size=".03 .03 .03" pos=".03 .1 .15"/>
<geom name="foot" type="box" size=".03 .1 .15" pos="0 0 -.05"/>
</body>
</body>
</body>
</worldbody>
<tendon>
<spatial name="ten" width=".01" rgba=".2 .2 1 1" armature="9">
<site site="0"/>
<site site="1"/>
<site site="2"/>
</spatial>
</tendon>
</mujoco>
test/engine/testdata/core_smooth/ten_armature_4.xml
Vendored
+47
View File
@@ -0,0 +1,47 @@
<mujoco>
<option integrator="RK4">
<flag contact="disable" gravity="disable" energy="enable"/>
</option>
<default>
<geom rgba="0.8 0.6 .4 1"/>
<site rgba="0.4 0.8 .6 .5"/>
</default>
<worldbody>
<body name="distractor">
<freejoint/>
<geom size=".1"/>
</body>
<body name="thigh" pos="-1 0 0">
<site name="0" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
<freejoint/>
<geom type="capsule" size=".04" fromto="0 0 0 0 0 -1"/>
<geom type="capsule" size=".02" fromto="0 0 -1 .05 -.08 -1.1"/>
<site name="1" type="box" size=".03 .03 .03" pos=".05 -.08 -1.1"/>
<body name="calf" pos="0 0 -1">
<joint axis="0 -1 0" damping="0" stiffness="20" springref="90"/>
<geom type="capsule" size=".03" fromto="0 0 0 -.6 0 0"/>
<body name="foot" pos="-.6 0 0">
<joint type="ball" stiffness="20" armature=".2"/>
<site name="2" type="box" size=".03 .03 .03" pos=".03 .1 .15"/>
<geom name="foot" type="box" size=".03 .1 .15" pos="0 0 -.05"/>
</body>
</body>
</body>
</worldbody>
<tendon>
<spatial name="ten" width=".01" rgba=".2 .2 1 1" armature="100" stiffness="10">
<site site="0"/>
<site site="1"/>
<site site="2"/>
</spatial>
</tendon>
<sensor>
<subtreelinvel body="thigh"/>
<subtreeangmom body="thigh"/>
</sensor>
</mujoco>
test/engine/testdata/core_smooth/ten_armature_offtree.xml
Vendored
+77
View File
@@ -0,0 +1,77 @@
<mujoco>
<!-- this is an example of a "bad" model where the tendon spans multiple branches of the tree -->
<option integrator="RK4">
<flag contact="disable" gravity="disable" energy="enable"/>
</option>
<default>
<geom rgba="0.8 0.6 .4 1"/>
<site rgba="0.4 0.8 .6 .5"/>
</default>
<worldbody>
<body name="distractor1">
<freejoint/>
<geom size=".1"/>
<site name="a"/>
</body>
<body>
<site name="0" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
<joint type="hinge" axis="0 -1 0"/>
<geom type="capsule" size=".04" fromto="0 0 0 0 0 -1"/>
<site name="1" type="box" size=".03 .03 .03" pos=".05 -.08 -1.1"/>
<body pos="0 0 -1">
<joint axis="0 -1 0" damping="0" stiffness="20" springref="90"/>
<geom type="capsule" size=".03" fromto="0 0 0 -.6 0 0"/>
<body pos="-.6 0 0">
<joint type="ball" stiffness="20" armature=".6"/>
<site name="2" type="box" size=".03 .03 .03" pos=".03 .1 .15"/>
<geom type="box" size=".03 .1 .15" pos="0 0 -.05"/>
</body>
</body>
</body>
<body name="distractor2" pos="0 0 1">
<freejoint/>
<geom size=".1"/>
<site name="b"/>
</body>
<body pos="1 0 0">
<site name="3" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
<joint type="hinge" axis="0 -1 0"/>
<geom type="capsule" size=".04" fromto="0 0 0 0 0 -1"/>
<site name="4" type="box" size=".03 .03 .03" pos=".05 -.08 -1.1"/>
<body pos="0 0 -1">
<joint axis="0 -1 0" damping="0" stiffness="20" springref="90"/>
<geom type="capsule" size=".03" fromto="0 0 0 -.6 0 0"/>
<body pos="-.6 0 0">
<joint type="ball" stiffness="20" armature=".2"/>
<site name="5" type="box" size=".03 .03 .03" pos=".03 .1 .15"/>
<geom type="box" size=".03 .1 .15" pos="0 0 -.05"/>
</body>
</body>
</body>
</worldbody>
<tendon>
<spatial armature="4">
<site site="b"/>
<site site="a"/>
</spatial>
<spatial width=".01" rgba=".2 .2 1 1" armature="10" stiffness="10">
<site site="2"/>
<site site="1"/>
<site site="0"/>
<site site="3"/>
<site site="4"/>
<site site="5"/>
</spatial>
<spatial width=".01" rgba=".2 .2 1 1" armature="10" stiffness="10">
<site site="0"/>
<site site="2"/>
<site site="1"/>
</spatial>
</tendon>
</mujoco>