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
doc/XMLreference.rst
+20 -2
View File
@@ -4706,12 +4706,30 @@ length X, as in the clip on the right of `this example model
joint damping which is integrated implicitly by the Euler method, tendon damping is not integrated implicitly, thus
joint damping should be used if possible.
.. TODO(tassa): Update here once the feature is implemented.
.. image:: images/XMLreference/tendon_armature.gif
:width: 30%
:align: right
:class: only-light
:target: https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/core_smooth/ten_armature_1_compare.xml
.. image:: images/XMLreference/tendon_armature_dark.gif
:width: 30%
:align: right
:class: only-dark
:target: https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/core_smooth/ten_armature_1_compare.xml
.. _tendon-spatial-armature:
:at:`armature`: :at-val:`real, "0"`
Inertia associated with tendon. This feature is not yet implemented.
Inertia associated with changes in tendon length. Setting this attribute to a positive value :math:`m` adds a kinetic
energy term :math:`\frac{1}{2}mv^2`, where :math:`v` is the tendon velocity. Tendon inertia is most valuable
when modeling the :ref:`armature<body-joint-armature>` inertia in a linear actuator which contains a spinning element
or the inertial motion of a fluid in a linear hydraulic actuator. In the illustration, we compare (*left*) a 3-dof
system with a "tendon" implemented with a rotational joint and a slider joint with
:ref:`armature<body-joint-armature>`, attached to the world with a :ref:`connect<equality-connect>` constraint and
(*right*) an equivalent 1-dof model with an armature-bearing tendon. Like joint :ref:`armature<body-joint-armature>`,
this added inertia is only associated with changes in tendon length, and would not affect the dynamics of a moving
fixed-length tendon. Because the tendon Jacobian :math:`J` is position-dependent, tendon armature leads to an
additional bias-force term :math:`c = m J \dot{J}^T \dot{q}`.
.. _tendon-spatial-user:
doc/changelog.rst
+1
View File
@@ -15,6 +15,7 @@ Upcoming version (not yet released)
General
^^^^^^^
- Added :ref:`tendon armature<tendon-spatial-armature>`: inertia associated with changes in tendon length.
- Added the :ref:`compiler/saveinertial<compiler-saveinertial>` flag, writing explicit inertial clauses for all
bodies when saving to XML.
- Added :ref:`orientation<body-composite-quat>` attribute to :ref:`composite<body-composite>`. Moreover, allow the
doc/images/XMLreference/tendon_armature.gif
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 123 KiB

doc/images/XMLreference/tendon_armature_dark.gif
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 133 KiB

src/engine/engine_core_smooth.c
+115 -9
View File
@@ -865,15 +865,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
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
// tendon id is invalid: return
if (id < 0 || id >= m->ntendon) {
return;
}
@@ -887,6 +879,13 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) {
return;
}
// allocate stack arrays
mj_markStack(d);
mjtNum* jac1 = mjSTACKALLOC(d, 3*nv, mjtNum);
mjtNum* jac2 = mjSTACKALLOC(d, 3*nv, mjtNum);
mjtNum* jacdif = mjSTACKALLOC(d, 3*nv, mjtNum);
mjtNum* tmp = mjSTACKALLOC(d, nv, mjtNum);
// process spatial tendon
mjtNum divisor = 1;
int wraptype, j = 0;
@@ -1470,6 +1469,63 @@ void mj_transmission(const mjModel* m, mjData* d) {
//-------------------------- inertia ---------------------------------------------------------------
// add tendon armature to qM
void mj_tendonArmature(const mjModel* m, mjData* d) {
TM_START;
int nv = m->nv, ntendon = m->ntendon, issparse = mj_isSparse(m);
for (int k=0; k < ntendon; k++) {
mjtNum armature = m->tendon_armature[k];
if (!armature) {
continue;
}
// dense
if (!issparse) {
mjtNum* ten_J = d->ten_J + nv*k;
for (int i=0; i < m->nv; i++) {
int Madr = m->dof_Madr[i];
for (int j = i; j >= 0; j = m->dof_parentid[j]) {
d->qM[Madr++] += armature * ten_J[j] * ten_J[i];
}
}
}
// sparse
else {
// get sparse info for tendon k
int rowadr = d->ten_J_rowadr[k];
int rownnz = d->ten_J_rownnz[k];
const int* colind = d->ten_J_colind + rowadr;
mjtNum* ten_J = d->ten_J + rowadr;
// iterate forward on nonzero rows i
for (int adr_i=0; adr_i < rownnz; adr_i++) {
int i = colind[adr_i];
int Madr = m->dof_Madr[i];
int adr_j = rownnz - 1;
// iterate backward on ancestors of i, find matching column j
for (int j = i; j >= 0; j = m->dof_parentid[j]) {
// reduce adr_j until column index is no bigger than j
while (colind[adr_j] > j && adr_j >= 0) {
adr_j--;
}
// found match, update qM
if (colind[adr_j] == j) {
d->qM[Madr++] += armature * ten_J[adr_j] * ten_J[adr_i];
}
}
}
}
}
TM_END(mjTIMER_POS_INERTIA);
}
// composite rigid body inertia algorithm
void mj_crb(const mjModel* m, mjData* d) {
TM_START;
@@ -2321,3 +2377,53 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
mju_addTo(d->cfrc_int+6*m->body_parentid[j], d->cfrc_int+6*j, 6);
}
}
// add bias force due to tendon armature
void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) {
int ntendon = m->ntendon, nv = m->nv, issparse = mj_isSparse(m);
mjtNum* ten_Jdot = NULL;
mj_markStack(d);
// add bias term due to tendon armature
for (int i=0; i < ntendon; i++) {
mjtNum armature = m->tendon_armature[i];
// no armature: skip
if (!armature) {
continue;
}
// allocate if required
if (!ten_Jdot) {
ten_Jdot = mjSTACKALLOC(d, nv, mjtNum);
}
// get dense d/dt(tendon Jacobian) for tendon i
mj_tendonDot(m, d, i, ten_Jdot);
// add bias term: qfrc += ten_J * armature * dot(ten_Jdot, qvel)
mjtNum coef = armature * mju_dot(ten_Jdot, d->qvel, nv);
if (coef) {
// dense
if (!issparse) {
mju_addToScl(qfrc, d->ten_J + nv*i, coef, nv);
}
// sparse
else {
int nnz = d->ten_J_rownnz[i];
int adr = d->ten_J_rowadr[i];
const int* colind = d->ten_J_colind + adr;
const mjtNum* ten_J = d->ten_J + adr;
for (int j=0; j < nnz; j++) {
qfrc[colind[j]] += coef * ten_J[j];
}
}
}
}
mj_freeStack(d);
}
src/engine/engine_core_smooth.h
+9
View File
@@ -51,6 +51,9 @@ MJAPI void mj_transmission(const mjModel* m, mjData* d);
// composite rigid body inertia algorithm
MJAPI void mj_crb(const mjModel* m, mjData* d);
// add tendon armature to qM
MJAPI void mj_tendonArmature(const mjModel* m, mjData* d);
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd (legacy implementation)
MJAPI void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M,
mjtNum* qLD, mjtNum* qLDiagInv);
@@ -99,6 +102,12 @@ MJAPI void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result);
// RNE with complete data: compute cacc, cfrc_ext, cfrc_int
MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d);
//-------------------------- tendon bias -----------------------------------------------------------
// add bias force due to tendon armature
MJAPI void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc);
#ifdef __cplusplus
}
#endif
src/engine/engine_forward.c
+10 -5
View File
@@ -114,8 +114,9 @@ typedef struct mjFwdPositionArgs_ mjFwdPositionArgs;
// wrapper for mj_crb and mj_factorM
void* mj_inertialThreaded(void* args) {
mjFwdPositionArgs* forward_args = (mjFwdPositionArgs*) args;
mj_crb(forward_args->m, forward_args->d); // timed internally (POS_INERTIA)
mj_factorM(forward_args->m, forward_args->d); // timed internally (POS_INERTIA)
mj_crb(forward_args->m, forward_args->d); // timed internally (POS_INERTIA)
mj_tendonArmature(forward_args->m, forward_args->d); // timed internally (POS_INERTIA)
mj_factorM(forward_args->m, forward_args->d); // timed internally (POS_INERTIA)
return NULL;
}
@@ -142,9 +143,10 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
// no threadpool: inertia and collision on main thread
if (!d->threadpool) {
mj_crb(m, d); // timed internally (POS_INERTIA)
mj_factorM(m, d); // timed internally (POS_INERTIA)
mj_collision(m, d); // timed internally (POS_COLLISION)
mj_crb(m, d); // timed internally (POS_INERTIA)
mj_tendonArmature(m, d); // timed internally (POS_INERTIA)
mj_factorM(m, d); // timed internally (POS_INERTIA)
mj_collision(m, d); // timed internally (POS_COLLISION)
}
// have threadpool: inertia and collision on separate threads
@@ -222,6 +224,9 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// compute qfrc_bias with abbreviated RNE (without acceleration)
mj_rne(m, d, 0, d->qfrc_bias);
// add bias force due to tendon armature
mj_tendonBias(m, d, d->qfrc_bias);
TM_END(mjTIMER_VELOCITY);
}
src/engine/engine_inverse.c
+3 -2
View File
@@ -45,8 +45,9 @@ void mj_invPosition(const mjModel* m, mjData* d) {
mj_tendon(m, d);
TM_END(mjTIMER_POS_KINEMATICS);
mj_crb(m, d); // timed internally (POS_INERTIA)
mj_factorM(m, d); // timed internally (POS_INERTIA)
mj_crb(m, d); // timed internally (POS_INERTIA)
mj_tendonArmature(m, d); // timed internally (POS_INERTIA)
mj_factorM(m, d); // timed internally (POS_INERTIA)
mj_collision(m, d); // timed internally (POS_COLLISION)
src/engine/engine_setconst.c
+2 -1
View File
@@ -102,10 +102,11 @@ static void set0(mjModel* m, mjData* d) {
memset(m->flex_rigid, 0, m->nflex);
// run remaining computations
mj_tendon(m, d);
mj_crb(m, d);
mj_tendonArmature(m, d);
mj_factorM(m, d);
mj_flex(m, d);
mj_tendon(m, d);
mj_transmission(m, d);
// restore flex rigidity
src/user/user_model.cc
+51 -25
View File
@@ -2769,31 +2769,6 @@ void mjCModel::CopyTree(mjModel* m) {
}
}
m->nB = nB;
// set dof_simplenum
int count = 0;
for (int i=nv-1; i >= 0; i--) {
if (m->body_simple[m->dof_bodyid[i]]) {
count++; // increment counter
} else {
count = 0; // reset
}
m->dof_simplenum[i] = count;
}
// compute nC
int nOD = 0; // number of off-diagonal (non-simple) parent dofs
for (int i=0; i < nv; i++) {
// count ancestor (off-diagonal) dofs
if (!m->dof_simplenum[i]) {
int j = i;
while (j >= 0) {
if (j != i) nOD++;
j = m->dof_parentid[j];
}
}
}
m->nC = nC = nOD + nv;
}
// copy plugin data
@@ -3564,6 +3539,54 @@ void mjCModel::CopyObjects(mjModel* m) {
// finalize simple bodies/dofs including tendon information
void mjCModel::FinalizeSimple(mjModel* m) {
// demote bodies affected by inertia-bearing tendon to non-simple
for (int i=0; i < ntendon; i++) {
if (m->tendon_armature[i] == 0) {
continue;
}
int adr = m->tendon_adr[i];
int num = m->tendon_num[i];
for (int j=adr; j < adr+num; j++) {
int objid = m->wrap_objid[j];
if (m->wrap_type[j] == mjWRAP_SITE) {
m->body_simple[m->site_bodyid[objid]] = 0;
}
if (m->wrap_type[j] == mjWRAP_CYLINDER || m->wrap_type[j] == mjWRAP_SPHERE) {
m->body_simple[m->geom_bodyid[objid]] = 0;
}
}
}
// set dof_simplenum
int count = 0;
for (int i=nv-1; i >= 0; i--) {
if (m->body_simple[m->dof_bodyid[i]]) {
count++; // increment counter
} else {
count = 0; // reset
}
m->dof_simplenum[i] = count;
}
// compute nC
int nOD = 0; // number of off-diagonal (non-simple) parent dofs
for (int i=0; i < nv; i++) {
// count ancestor (off-diagonal) dofs
if (!m->dof_simplenum[i]) {
int j = i;
while (j >= 0) {
if (j != i) nOD++;
j = m->dof_parentid[j];
}
}
}
m->nC = nC = nOD + nv;
}
// save the current state
template <class T>
void mjCModel::SaveState(const std::string& state_name, const T* qpos, const T* qvel, const T* act,
@@ -4509,6 +4532,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// copy objects outsite kinematic tree (including keyframes)
CopyObjects(m);
// finalize simple bodies/dofs including tendon information
FinalizeSimple(m);
// compute non-zeros in actuator_moment
m->nJmom = nJmom = CountNJmom(m);
src/user/user_model.h
+1
View File
@@ -349,6 +349,7 @@ class mjCModel : public mjCModel_, private mjSpec {
void CopyPaths(mjModel*); // copy paths, compute path addresses
void CopyObjects(mjModel*); // copy objects outside kinematic tree
void CopyTree(mjModel*); // copy objects inside kinematic tree
void FinalizeSimple(mjModel* m); // finalize simple bodies/dofs including tendon information
void CopyPlugins(mjModel*); // copy plugin data
int CountNJmom(const mjModel* m); // compute number of non-zeros in actuator_moment matrix
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>