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

Refactor islands to be memory contiguous.

Browse Source 
PiperOrigin-RevId: 755803476
Change-Id: I41972b07e0d5ef5d0117c94f565b93367b87458b
This commit is contained in:
Yuval Tassa
2025-05-07 05:05:34 -07:00
committed by Copybara-Service
parent 449de73430
commit ecb769fc3a
30 changed files with 1742 additions and 1116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_core_constraint_test.cc
+54 -240
View File
@@ -25,6 +25,7 @@
#include <mujoco/mujoco.h>
#include "src/engine/engine_core_constraint.h"
#include "src/engine/engine_support.h"
#include "src/engine/engine_util_misc.h"
#include "test/fixture.h"
namespace mujoco {
@@ -284,205 +285,15 @@ TEST_F(CoreConstraintTest, EqualityBodySite) {
mj_deleteModel(model);
}
static const char* const kIlslandEfcPath =
"engine/testdata/island/island_efc.xml";
TEST_F(CoreConstraintTest, MulJacVecIsland) {
// validate mj_constraintUpdate_impl
TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
// allocate vec_nv, fill with arbitrary values
mjtNum* vec_nv = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv);
for (int i=0; i < model->nv; i++) {
vec_nv[i] = 0.2 + 0.3*i;
}
// iterate through dense and sparse
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
model->opt.jacobian = sparsity;
// simulate for 0.2 seconds
mj_resetData(model, data);
while (data->time < 0.2) {
mj_step(model, data);
}
mj_forward(model, data);
// multiply by Jacobian: vec_nefc = J * vec_nv
mjtNum* vec_nefc = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc);
mj_mulJacVec(model, data, vec_nefc, vec_nv);
mjtNum* vec_nefc_tmp = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc);
// iterate over islands
for (int i=0; i < data->nisland; i++) {
// allocate dof and efc vectors for island
int dofnum = data->island_dofnum[i];
mjtNum* vec_nvi = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
int efcnum = data->island_efcnum[i];
mjtNum* vec_nefci = (mjtNum*)mju_malloc(sizeof(mjtNum) * efcnum);
// get indices
int* dofind = data->island_dofind + data->island_dofadr[i];
int* efcind = data->island_efcind + data->island_efcadr[i];
// copy values into vec_nvi
for (int j=0; j < dofnum; j++) {
vec_nvi[j] = vec_nv[dofind[j]];
}
// ===== both compressed
int flg_resunc = 0;
int flg_vecunc = 0;
mju_zero(vec_nefci, efcnum); // clear output
mj_mulJacVec_island(model, data, vec_nefci, vec_nvi,
i, flg_resunc, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < efcnum; j++) {
EXPECT_THAT(vec_nefci[j], DoubleNear(vec_nefc[efcind[j]], 1e-12));
}
// ===== input uncompressed: read from vec_nv
flg_resunc = 0;
flg_vecunc = 1;
mju_zero(vec_nefci, efcnum); // clear output
mj_mulJacVec_island(model, data, vec_nefci, vec_nv,
i, flg_resunc, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < efcnum; j++) {
EXPECT_THAT(vec_nefci[j], DoubleNear(vec_nefc[efcind[j]], 1e-12));
}
// ===== output uncompressed: write to vec_nefc_tmp
flg_resunc = 1;
flg_vecunc = 0;
mju_zero(vec_nefc_tmp, data->nefc); // clear output
mj_mulJacVec_island(model, data, vec_nefc_tmp, vec_nvi,
i, flg_resunc, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < efcnum; j++) {
EXPECT_THAT(vec_nefc_tmp[efcind[j]],
DoubleNear(vec_nefc[efcind[j]], 1e-12));
}
mju_free(vec_nvi);
mju_free(vec_nefci);
}
mju_free(vec_nefc_tmp);
mju_free(vec_nefc);
}
mju_free(vec_nv);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(CoreConstraintTest, MulJacTVecIsland) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
// allocate vec_nv
mjtNum* vec_nv = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv);
mjtNum* vec_nv_tmp = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv);
// iterate through dense and sparse
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
model->opt.jacobian = sparsity;
// simulate for 0.3 seconds
mj_resetData(model, data);
while (data->time < 0.3) {
mj_step(model, data);
}
mj_forward(model, data);
// allocate vec_nefc, fill with arbitrary values
mjtNum* vec_nefc = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc);
for (int i=0; i < data->nefc; i++) {
vec_nefc[i] = 0.2 + 0.3*i;
}
// multiply by Jacobian: vec_nv = J^T * vec_nefc
mj_mulJacTVec(model, data, vec_nv, vec_nefc);
// iterate over islands
for (int i=0; i < data->nisland; i++) {
// allocate dof and efc vectors for island
int dofnum = data->island_dofnum[i];
mjtNum* vec_nvi = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
int efcnum = data->island_efcnum[i];
mjtNum* vec_nefci = (mjtNum*)mju_malloc(sizeof(mjtNum) * efcnum);
// get indices
int* efcind = data->island_efcind + data->island_efcadr[i];
int* dofind = data->island_dofind + data->island_dofadr[i];
// copy values into vec_nefci
for (int j=0; j < efcnum; j++) {
vec_nefci[j] = vec_nefc[efcind[j]];
}
// ==== both compressed
int flg_resunc = 0;
int flg_vecunc = 0;
mju_zero(vec_nvi, dofnum); // clear output
mj_mulJacTVec_island(model, data, vec_nvi, vec_nefci,
i, flg_resunc, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(vec_nvi[j], DoubleNear(vec_nv[dofind[j]], 1e-12));
}
// ===== input uncompressed: read from vec_nefc
flg_resunc = 0;
flg_vecunc = 1;
mju_zero(vec_nvi, dofnum); // clear output
mj_mulJacTVec_island(model, data, vec_nvi, vec_nefc,
i, flg_resunc, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(vec_nvi[j], DoubleNear(vec_nv[dofind[j]], 1e-12));
}
// ===== output uncompressed: write to vec_nv_tmp
flg_resunc = 1;
flg_vecunc = 0;
mju_zero(vec_nv_tmp, model->nv); // clear output
mj_mulJacTVec_island(model, data, vec_nv_tmp, vec_nefci,
i, flg_resunc, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(vec_nv_tmp[dofind[j]],
DoubleNear(vec_nv[dofind[j]], 1e-12));
}
mju_free(vec_nvi);
mju_free(vec_nefci);
}
mju_free(vec_nefc);
}
mju_free(vec_nv_tmp);
mju_free(vec_nv);
mj_deleteData(data);
mj_deleteModel(model);
}
// compare mj_constraintUpdate and mj_constraintUpdate_island
TEST_F(CoreConstraintTest, ConstraintUpdateIsland) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data1 = mj_makeData(model);
mjData* data2 = mj_makeData(model);
mjData* d1 = mj_makeData(model);
mjData* d2 = mj_makeData(model);
// iterate over sparsity and cone
for (mjtJacobian sparsity : {mjJAC_SPARSE, mjJAC_DENSE}) {
@@ -491,81 +302,84 @@ TEST_F(CoreConstraintTest, ConstraintUpdateIsland) {
model->opt.cone = cone;
// simulate for 0.2 seconds
mj_resetData(model, data1);
mj_resetData(model, data2);
while (data1->time < 0.2) {
mj_step(model, data1);
mj_step(model, data2);
mj_resetData(model, d1);
mj_resetData(model, d2);
while (d1->time < 0.2) {
mj_step(model, d1);
mj_step(model, d2);
}
mj_forward(model, data1);
mj_forward(model, data2);
mj_forward(model, d1);
mj_forward(model, d2);
// get sizes
int nefc = data1->nefc;
int nefc = d1->nefc;
int nv = model->nv;
int nisland = data1->nisland;
int nisland = d1->nisland;
EXPECT_GT(nisland, 0);
// get jar = J*a - aref
mjtNum* jar = (mjtNum*)mju_malloc(sizeof(mjtNum) * nefc);
mj_mulJacVec(model, data1, jar, data1->qacc);
mju_subFrom(jar, data1->efc_aref, nefc);
mj_mulJacVec(model, d1, jar, d1->qacc);
mju_subFrom(jar, d1->efc_aref, nefc);
// constraint update for data1 given jar
mjtNum cost1;
mj_constraintUpdate(model, data1, jar, &cost1, /*flg_coneHessian=*/1);
mj_constraintUpdate(model, d1, jar, &cost1, /*flg_coneHessian=*/1);
// iterate over islands, check match
mjtNum cost2 = 0;
for (int island=0; island < nisland; island++) {
// clear outputs from data2
for (int i=0; i < nefc; i++) data2->efc_state[i] = -1;
mju_zero(data2->efc_force, nefc);
mju_zero(data2->qfrc_constraint, nv);
for (int i=0; i < data2->ncon; i++) mju_zero(data2->contact[i].H, 36);
for (int i=0; i < nefc; i++) d2->efc_state[i] = -1;
mju_zero(d2->efc_force, nefc);
mju_zero(d2->qfrc_constraint, nv);
for (int i=0; i < d2->ncon; i++) mju_zero(d2->contact[i].H, 36);
// sizes and indices, in this island
int dofnum = data2->island_dofnum[island];
int efcnum = data2->island_efcnum[island];
int* dofind = data2->island_dofind + data2->island_dofadr[island];
int* efcind = data2->island_efcind + data2->island_efcadr[island];
int efcnum = d2->island_nefc[island];
// get jar restricted to island
// gather values into jari
mjtNum* jari = (mjtNum*)mju_malloc(sizeof(mjtNum) * efcnum);
for (int c=0; c < efcnum; c++) {
jari[c] = jar[efcind[c]];
}
int* map2efc = d2->map_iefc2efc + d2->island_iefcadr[island];
mju_gather(jari, jar, map2efc, efcnum);
// update constraints for this island
mjtNum cost2i;
mj_constraintUpdate_island(model, data2, jari, &cost2i,
/*flg_coneHessian=*/1, island);
int ne = d2->island_ne[island];
int nf = d2->island_nf[island];
int adr = d2->island_iefcadr[island];
int* state = d2->iefc_state + adr;
mjtNum *force = d2->iefc_force + adr;
mj_constraintUpdate_impl(ne, nf, efcnum,
d2->iefc_D + adr,
d2->iefc_R + adr,
d2->iefc_frictionloss + adr,
jari,
d2->iefc_type + adr,
d2->iefc_id + adr,
d2->contact,
state,
force,
&cost2i,
/*flg_coneHessian=*/1);
// compare nefc vectors
for (int c=0; c < efcnum; c++) {
int i = efcind[c];
EXPECT_EQ(data2->efc_island[i], island);
EXPECT_EQ(data2->efc_state[i], data1->efc_state[i]);
EXPECT_THAT(data2->efc_force[i],
DoubleNear(data1->efc_force[i], 1e-12));
}
// compare qfrc_constraint
for (int c=0; c < dofnum; c++) {
int i = dofind[c];
EXPECT_THAT(data2->qfrc_constraint[i],
DoubleNear(data1->qfrc_constraint[i], 1e-12));
int i = map2efc[c];
EXPECT_EQ(d2->efc_island[i], island);
EXPECT_EQ(state[c], d1->efc_state[i]);
EXPECT_THAT(force[c], DoubleNear(d1->efc_force[i], 1e-12));
}
// compare cone Hessians
if (cone == mjCONE_ELLIPTIC) {
for (int c=0; c < data2->ncon; c++) {
int efcadr = data2->contact[c].efc_address;
if (data2->efc_island[efcadr] == island &&
data2->efc_state[efcadr] == mjCNSTRSTATE_CONE) {
for (int c=0; c < d2->ncon; c++) {
int efcadr = d2->contact[c].efc_address;
if (d2->efc_island[efcadr] == island &&
d2->efc_state[efcadr] == mjCNSTRSTATE_CONE) {
for (int j=0; j < 36; j++) {
EXPECT_THAT(data2->contact[c].H[j],
DoubleNear(data1->contact[c].H[j], 1e-12));
EXPECT_THAT(d2->contact[c].H[j],
DoubleNear(d1->contact[c].H[j], 1e-12));
}
}
}
@@ -584,8 +398,8 @@ TEST_F(CoreConstraintTest, ConstraintUpdateIsland) {
}
}
mj_deleteData(data2);
mj_deleteData(data1);
mj_deleteData(d2);
mj_deleteData(d1);
mj_deleteModel(model);
}
test/engine/engine_core_smooth_test.cc
-60
View File
@@ -634,66 +634,6 @@ TEST_F(CoreSmoothTest, RefsiteConservesMomentum) {
mj_deleteModel(model);
}
static const char* const kIlslandEfcPath =
"engine/testdata/island/island_efc.xml";
static const char* const kModelPath =
"testdata/model.xml";
TEST_F(CoreSmoothTest, SolveMIsland) {
for (auto model_path : {kModelPath, kIlslandEfcPath}) {
const std::string xml_path = GetTestDataFilePath(model_path);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
int nv = model->nv;
// allocate vec, fill with arbitrary values, copy to sol
mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv);
mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv);
for (int i=0; i < nv; i++) {
vec[i] = 0.2 + 0.3*i;
}
mju_copy(res, vec, nv);
if (model->nkey > 0) mj_resetDataKeyframe(model, data, 0);
for (int i=0; i < 6; i++) {
mj_step(model, data);
}
mj_forward(model, data);
// divide by mass matrix: sol = M^-1 * vec
mj_solveM(model, data, res, res, 1);
// iterate over islands
for (int i=0; i < data->nisland; i++) {
// allocate dof vectors for island
int dofnum = data->island_dofnum[i];
mjtNum* res_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
// copy values into sol_i
int* dofind = data->island_dofind + data->island_dofadr[i];
for (int j=0; j < dofnum; j++) {
res_i[j] = vec[dofind[j]];
}
// divide by mass matrix, for this island
mj_solveM_island(model, data, res_i, i);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(res_i[j], DoubleNear(res[dofind[j]], 1e-12));
}
mju_free(res_i);
}
mju_free(res);
mju_free(vec);
mj_deleteData(data);
mj_deleteModel(model);
}
}
static const char* const kInertiaPath = "engine/testdata/inertia.xml";
TEST_F(CoreSmoothTest, FactorI) {
test/engine/engine_island_test.cc
+185 -28
View File
@@ -208,17 +208,19 @@ TEST_F(IslandTest, Abacus) {
int nv = model->nv;
int nefc = data->nefc;
int nisland = data->nisland;
int nidof = data->nidof;
// 4 dofs, 12 constraints, 2 islands
EXPECT_EQ(nv, 4);
EXPECT_EQ(nidof, 3);
EXPECT_EQ(nefc, 12); // 3 pyramidal contacts
EXPECT_EQ(nisland, 2);
// the islands begin at dofs 0 and 1
EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 1));
EXPECT_THAT(AsVector(data->island_idofadr, nisland), ElementsAre(0, 1));
// number of dofs in the 2 islands
EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(1, 2));
EXPECT_THAT(AsVector(data->island_nv, nisland), ElementsAre(1, 2));
// dof 0 in island 0
// dof 1 in no island
@@ -228,19 +230,19 @@ TEST_F(IslandTest, Abacus) {
// dof 0 constitutes first island
// dofs 2, 3 are the second island
// last index is unassigned since dof 1 is unconstrained
EXPECT_THAT(AsVector(data->island_dofind, nv), ElementsAre(0, 2, 3, -1));
EXPECT_THAT(AsVector(data->map_idof2dof, nv), ElementsAre(0, 2, 3, 1));
// dof 0 constitutes first island
// dofs 1 is unassigned
// dofs 2, 3 are second island
EXPECT_THAT(AsVector(data->dof_islandind, nv), ElementsAre(0, -1, 0, 1));
EXPECT_THAT(AsVector(data->map_dof2idof, nv), ElementsAre(0, 3, 1, 2));
// island 0 starts at constraint 0
// island 1 starts at constraint 4
EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 4));
EXPECT_THAT(AsVector(data->island_iefcadr, nisland), ElementsAre(0, 4));
// number of constraints in the 2 islands
EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(4, 8));
EXPECT_THAT(AsVector(data->island_nefc, nisland), ElementsAre(4, 8));
// first contact (4 constraints) is in island 0
// second contact (8 constraints) is in island 1
@@ -248,7 +250,7 @@ TEST_F(IslandTest, Abacus) {
ElementsAre(0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1));
// index lists for islands 0 and 1
EXPECT_THAT(AsVector(data->island_efcind, nefc),
EXPECT_THAT(AsVector(data->map_iefc2efc, nefc),
ElementsAre(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11));
// reset, push 0 to the left, 3 to the right, 1,2 to the middle
@@ -266,18 +268,20 @@ TEST_F(IslandTest, Abacus) {
// local variables
nefc = data->nefc;
nisland = data->nisland;
nidof = data->nidof;
EXPECT_EQ(nisland, 3);
EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 1, 3));
EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(1, 2, 1));
EXPECT_EQ(nidof, 4);
EXPECT_THAT(AsVector(data->island_idofadr, nisland), ElementsAre(0, 1, 3));
EXPECT_THAT(AsVector(data->island_nv, nisland), ElementsAre(1, 2, 1));
EXPECT_THAT(AsVector(data->dof_island, nv), ElementsAre(0, 1, 1, 2));
EXPECT_THAT(AsVector(data->island_dofind, nv), ElementsAre(0, 1, 2, 3));
EXPECT_THAT(AsVector(data->dof_islandind, nv), ElementsAre(0, 0, 1, 0));
EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 4, 8));
EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(4, 4, 4));
EXPECT_THAT(AsVector(data->map_idof2dof, nv), ElementsAre(0, 1, 2, 3));
EXPECT_THAT(AsVector(data->map_dof2idof, nv), ElementsAre(0, 1, 2, 3));
EXPECT_THAT(AsVector(data->island_iefcadr, nisland), ElementsAre(0, 4, 8));
EXPECT_THAT(AsVector(data->island_nefc, nisland), ElementsAre(4, 4, 4));
EXPECT_THAT(AsVector(data->efc_island, nefc),
ElementsAre(0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2));
EXPECT_THAT(AsVector(data->island_efcind, nefc),
EXPECT_THAT(AsVector(data->map_iefc2efc, nefc),
ElementsAre(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11));
mj_deleteData(data);
@@ -311,27 +315,30 @@ TEST_F(IslandTest, DenseSparse) {
int nisland = data1->nisland;
// expect sparse and dense to be identical
EXPECT_EQ(data1->nidof, data2->nidof);
EXPECT_EQ(data1->nefc, data2->nefc);
EXPECT_EQ(data1->nisland, data2->nisland);
EXPECT_EQ(data1->nefc, data2->nefc);
EXPECT_EQ(AsVector(data1->island_dofadr, nisland),
AsVector(data2->island_dofadr, nisland));
EXPECT_EQ(AsVector(data1->island_dofnum, nisland),
AsVector(data2->island_dofnum, nisland));
EXPECT_EQ(AsVector(data1->island_idofadr, nisland),
AsVector(data2->island_idofadr, nisland));
EXPECT_EQ(AsVector(data1->island_nv, nisland),
AsVector(data2->island_nv, nisland));
EXPECT_EQ(AsVector(data1->dof_island, nv),
AsVector(data2->dof_island, nv));
EXPECT_EQ(AsVector(data1->island_dofind, nv),
AsVector(data2->island_dofind, nv));
EXPECT_EQ(AsVector(data1->dof_islandind, nv),
AsVector(data2->dof_islandind, nv));
EXPECT_EQ(AsVector(data1->island_efcadr, nisland),
AsVector(data2->island_efcadr, nisland));
EXPECT_EQ(AsVector(data1->island_efcnum, nisland),
AsVector(data2->island_efcnum, nisland));
EXPECT_EQ(AsVector(data1->map_idof2dof, nv),
AsVector(data2->map_idof2dof, nv));
EXPECT_EQ(AsVector(data1->map_dof2idof, nv),
AsVector(data2->map_dof2idof, nv));
EXPECT_EQ(AsVector(data1->island_iefcadr, nisland),
AsVector(data2->island_iefcadr, nisland));
EXPECT_EQ(AsVector(data1->island_nefc, nisland),
AsVector(data2->island_nefc, nisland));
EXPECT_EQ(AsVector(data1->efc_island, nefc),
AsVector(data2->efc_island, nefc));
EXPECT_EQ(AsVector(data1->island_efcind, nefc),
AsVector(data2->island_efcind, nefc));
EXPECT_EQ(AsVector(data1->map_iefc2efc, nefc),
AsVector(data2->map_iefc2efc, nefc));
EXPECT_EQ(AsVector(data1->map_efc2iefc, nefc),
AsVector(data2->map_efc2iefc, nefc));
mj_deleteData(data2);
mj_deleteData(data1);
@@ -361,6 +368,156 @@ TEST_F(IslandTest, IslandEfc) {
mj_deleteModel(model);
}
static const char* const k2H100Path = "engine/testdata/island/2humanoid100.xml";
TEST_F(IslandTest, IslandJacobian) {
for (const char* local_path : {kIlslandEfcPath, k2H100Path}) {
const std::string xml_path = GetTestDataFilePath(local_path);
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
int jac0 = m->opt.jacobian;
mjData* d = mj_makeData(m);
for (mjtNum t_stop : {0.0, 0.2, 2.0}) {
while (d->time < t_stop) {
mj_step(m, d);
}
for (mjtJacobian jac : {mjJAC_DENSE, mjJAC_SPARSE}) {
m->opt.jacobian = jac;
mj_forward(m, d);
int nv = m->nv;
int nefc = d->nefc;
int nisland = d->nisland;
int nidof = d->nidof;
mjtNum* J = (mjtNum*)mju_malloc(sizeof(mjtNum) * nefc * nv);
mjtNum* iJ = (mjtNum*)mju_malloc(sizeof(mjtNum) * nefc * nidof);
// get local dense Jacobian
if (jac == mjJAC_DENSE) {
mju_copy(J, d->efc_J, nefc * nv);
mju_copy(iJ, d->iefc_J, nefc * nidof);
} else {
mju_sparse2dense(J, d->efc_J, nefc, nv, d->efc_J_rownnz,
d->efc_J_rowadr, d->efc_J_colind);
}
// compare random access in efc_J to contiguous memory in iefc_J
for (int island=0; island < nisland; island++) {
int idof = d->island_idofadr[island];
int iefc = d->island_iefcadr[island];
int nefc_island = d->island_nefc[island];
int nv_island = d->island_nv[island];
// === test J
// get pointer to J_island, dense (nefc_island x nv_island) submatrix
mjtNum* J_island;
if (jac == mjJAC_DENSE) {
// point to starting address of island in efc_J
J_island = iJ + iefc * nidof;
} else {
// dense copy of island in iJ (here used as scratch)
mju_sparse2dense(iJ, d->iefc_J, nefc_island, nv_island,
d->iefc_J_rownnz + iefc,
d->iefc_J_rowadr + iefc,
d->iefc_J_colind);
J_island = iJ;
}
// sequential memory in J_island equals random access memory in J
for (int i=0; i < nefc_island; i++) {
for (int j=0; j < nv_island; j++) {
int efc = d->map_iefc2efc[iefc + i];
int dof = d->map_idof2dof[idof + j];
EXPECT_EQ(J_island[i * nv_island + j], J[efc * nv + dof]);
}
}
// === test JT (if sparse)
// get pointer to J_island, dense (nefc_island x nv_island) submatrix
if (jac == mjJAC_SPARSE) {
// dense copy of island in iJ (here used as scratch)
mju_sparse2dense(iJ, d->iefc_JT, nv_island, nefc_island,
d->iefc_JT_rownnz + idof,
d->iefc_JT_rowadr + idof,
d->iefc_JT_colind);
J_island = iJ;
// sequential memory in J_island equals random access memory in J
for (int i=0; i < nv_island; i++) {
for (int j=0; j < nefc_island; j++) {
int dof = d->map_idof2dof[idof + i];
int efc = d->map_iefc2efc[iefc + j];
EXPECT_EQ(J_island[i * nefc_island + j], J[efc * nv + dof]);
}
}
}
}
mju_free(iJ);
mju_free(J);
}
// reset opt.jacobian to initial value
m->opt.jacobian = jac0;
}
mj_deleteData(d);
mj_deleteModel(m);
}
}
TEST_F(IslandTest, IslandInertia) {
for (const char* local_path : {kIlslandEfcPath, k2H100Path}) {
const std::string xml_path = GetTestDataFilePath(local_path);
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
int nv = m->nv;
mjData* d = mj_makeData(m);
mjtNum* M = (mjtNum*)mju_malloc(sizeof(mjtNum) * nv * nv);
for (mjtNum t_stop : {0.0, 0.2, 2.0}) {
while (d->time < t_stop) {
mj_step(m, d);
}
mj_forward(m, d);
int nisland = d->nisland;
// get dense inertia (lower only)
mj_fullM(m, M, d->qM);
// compare iM sub-matrix to full M
for (int island=0; island < nisland; island++) {
int nvi = d->island_nv[island];
mjtNum* Mi = (mjtNum*)mju_malloc(sizeof(mjtNum) * nvi * nvi);
int adr = d->island_idofadr[island];
mju_sparse2dense(Mi, d->iM, nvi, nvi,
d->iM_rownnz + adr,
d->iM_rowadr + adr,
d->iM_colind);
// compare Mi to M (lower triangle only)
for (int i=0; i < nvi; i++) {
for (int j=0; j <= i; j++) {
int dofi = d->map_idof2dof[adr + j];
int dofj = d->map_idof2dof[adr + i];
EXPECT_EQ(Mi[i * nvi + j], M[dofi * nv + dofj]);
}
}
mju_free(Mi);
}
}
mju_free(M);
mj_deleteData(d);
mj_deleteModel(m);
}
}
TEST_F(IslandTest, IslandEfcElliptic) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
test/engine/engine_solver_test.cc
-85
View File
@@ -17,7 +17,6 @@
#include <algorithm>
#include <cstdlib>
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
@@ -29,19 +28,9 @@ namespace {
using ::testing::DoubleNear;
using ::testing::NotNull;
using ::std::vector;
using ::std::abs;
using ::std::max;
// compare two vectors, relative error (increase tolerance for large elements)
inline void ExpectEqRel(vector<mjtNum> v1, vector<mjtNum> v2, mjtNum rtol) {
ASSERT_TRUE(v1.size() == v2.size());
for (int i = 0; i < v1.size(); i++) {
mjtNum scale = 0.5 * max(2.0, abs(v1[i]) + abs(v2[i]));
EXPECT_THAT(v1[i], DoubleNear(v2[i], scale*rtol));
}
}
using SolverTest = MujocoTest;
static const char* const kModelPath =
@@ -169,79 +158,5 @@ TEST_F(SolverTest, IslandsEquivalentForward) {
mj_deleteModel(model);
}
static const char* const kIlslandEfcPath =
"engine/testdata/island/island_efc.xml";
// compare qacc from 1 iteration of monolithic CG solver and one big island
TEST_F(SolverTest, OneBigIsland) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
ASSERT_THAT(model, NotNull());
model->opt.solver = mjSOL_CG; // use CG solver
model->opt.disableflags |= mjDSBL_WARMSTART; // disable warmstart
model->opt.tolerance = 0; // set tolerance to 0
model->opt.enableflags &= ~mjENBL_ISLAND; // disable islands
int state_size = mj_stateSize(model, mjSTATE_INTEGRATION);
mjtNum* state = (mjtNum*) mju_malloc(sizeof(mjtNum)*state_size);
mjData* data_island = mj_makeData(model);
mjData* data_noisland = mj_makeData(model);
int nv = model->nv;
mjtNum rtol = 1e-7;
// save current (default) iterations
int iterations_default = model->opt.iterations;
while (data_noisland->time < .2) {
// step and copy the state to data_island
mj_step(model, data_noisland);
mj_getState(model, data_noisland, state, mjSTATE_INTEGRATION);
mj_setState(model, data_island, state, mjSTATE_INTEGRATION);
// set small number of iterations
model->opt.iterations = 1;
// call forward on data_noisland
mj_forward(model, data_noisland);
// enable islands
model->opt.enableflags |= mjENBL_ISLAND;
// call forward (just for smooth dynamics and to allocate islands)
mj_forward(model, data_island);
// overwrite island structure with one big island
data_island->nisland = 1;
data_island->island_dofnum[0] = nv;
data_island->island_dofadr[0] = 0;
for (int i = 0; i < nv; i++) {
data_island->island_dofind[i] = data_island->dof_islandind[i] = i;
}
int nefc = data_island->nefc;
data_island->island_efcnum[0] = nefc;
data_island->island_efcadr[0] = 0;
for (int i = 0; i < nefc; i++) data_island->island_efcind[i] = i;
// solve using using one big island
mj_fwdConstraint(model, data_island);
// re-disable islands and reset iterations
model->opt.enableflags &= ~mjENBL_ISLAND;
model->opt.iterations = iterations_default;
// compare accelerations (relative error)
ExpectEqRel(AsVector(data_noisland->qacc, nv),
AsVector(data_island->qacc, nv), rtol);
}
mj_deleteData(data_noisland);
mj_deleteData(data_island);
mju_free(state);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_support_test.cc
-71
View File
@@ -830,77 +830,6 @@ TEST_F(InertiaTest, mulM2) {
mj_deleteModel(model);
}
static const char* const kIlslandEfcPath =
"engine/testdata/island/island_efc.xml";
TEST_F(SupportTest, MulMIsland) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
// allocate vec, fill with arbitrary values
mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv);
for (int i=0; i < model->nv; i++) {
vec[i] = 0.2 + 0.3*i;
}
// simulate for 0.2 seconds
mj_resetData(model, data);
while (data->time < 0.2) {
mj_step(model, data);
}
mj_forward(model, data);
// multiply by Mass matrix: Mvec = M * vec
mjtNum* Mvec = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc);
mj_mulM(model, data, Mvec, vec);
// iterate over islands
for (int i=0; i < data->nisland; i++) {
// allocate dof vectors for island
int dofnum = data->island_dofnum[i];
mjtNum* vec_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
mjtNum* Mvec_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
// copy values into vec_i
int* dofind = data->island_dofind + data->island_dofadr[i];
for (int j=0; j < dofnum; j++) {
vec_i[j] = vec[dofind[j]];
}
// === compressed: use vec_i
// multiply by Jacobian, for this island
int flg_vecunc = 0;
mj_mulM_island(model, data, Mvec_i, vec_i, i, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(Mvec_i[j], DoubleNear(Mvec[dofind[j]], 1e-12));
}
// === uncompressed: use vec
mju_zero(Mvec_i, dofnum); // clear output
// multiply by Jacobian, for this island
flg_vecunc = 1;
mj_mulM_island(model, data, Mvec_i, vec, i, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(Mvec_i[j], DoubleNear(Mvec[dofind[j]], 1e-12));
}
mju_free(vec_i);
mju_free(Mvec_i);
}
mju_free(Mvec);
mju_free(vec);
mj_deleteData(data);
mj_deleteModel(model);
}
static constexpr char GeomDistanceTestingModel[] = R"(
<mujoco>
<option>
test/engine/testdata/island/2humanoid100.xml
Vendored
+123
View File
@@ -0,0 +1,123 @@
<mujoco model="2 Humanoids and 100 objects">
<!--
Model designed for a maximally-elaborate island structure.
More horizontal gravity leads to larger, fewer islands.
-->
<option timestep="0.005" solver="CG" gravity="-1 -1 -10">
<flag island="enable"/>
</option>
<size memory="100M"/>
<default>
<geom solimp=".9 .9 .01"/>
<default class="capsule">
<geom type="capsule" material="capsule" size="0.1 0.05"/>
</default>
<default class="ellipsoid">
<geom type="ellipsoid" material="ellipsoid" size="0.15 0.1 0.07"/>
</default>
<default class="box">
<geom type="box" material="box" size="0.15 0.1 0.05"/>
</default>
<default class="cylinder">
<geom type="cylinder" material="cylinder" size="0.1 0.05" condim="4" friction="1 .01 .01"/>
</default>
<default class="sphere">
<geom type="sphere" material="sphere" size="0.1"/>
</default>
<default class="border">
<geom type="capsule" size="0.4" rgba=".4 .4 .4 1"/>
</default>
<default class="borderpost">
<geom type="box" size="0.41 0.41 0.41" rgba=".55 .55 .55 1"/>
</default>
</default>
<asset>
<model file="humanoid.xml"/>
<texture type="skybox" builtin="gradient" width="512" height="512" rgb1=".4 .6 .8" rgb2="0 0 0"/>
<texture name="texgeom" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.6 0.6 0.6" rgb2="0.6 0.6 0.6" markrgb="1 1 1"/>
<texture name="texplane" type="2d" builtin="checker" rgb1=".4 .4 .4" rgb2=".6 .6 .6" width="512" height="512"/>
<material name="MatPlane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true" rgba=".7 .7 .7 1"/>
<material name="capsule" texture="texgeom" texuniform="true" rgba=".4 .9 .6 1"/>
<material name="ellipsoid" texture="texgeom" texuniform="true" rgba=".4 .6 .9 1"/>
<material name="box" texture="texgeom" texuniform="true" rgba=".4 .9 .9 1"/>
<material name="cylinder" texture="texgeom" texuniform="true" rgba=".8 .6 .8 1"/>
<material name="sphere" texture="texgeom" texuniform="true" rgba=".9 .1 .1 1"/>
</asset>
<visual>
<quality shadowsize="4096" offsamples="8"/>
<map znear="0.1" force="0.05"/>
</visual>
<statistic extent="4"/>
<worldbody>
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
<geom name="floor" type="plane" size="3 3 .5" material="MatPlane"/>
<geom class="border" fromto="-3 3 0 3 3 0"/>
<geom class="border" fromto="-3 -3 0 3 -3 0"/>
<geom class="border" fromto="3 3 0 3 -3 0"/>
<geom class="border" fromto="-3 3 0 -3 -3 0"/>
<geom class="borderpost" pos="3 3 0"/>
<geom class="borderpost" pos="-3 3 0"/>
<geom class="borderpost" pos="3 -3 0"/>
<geom class="borderpost" pos="-3 -3 0"/>
<replicate count="4" euler="0 0 90">
<geom type="plane" size=".5 3 .05" zaxis="1 0 0" pos="-3 0 0.4"/>
</replicate>
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="30 40 0">
<freejoint/>
<geom class="capsule"/>
</body>
</replicate>
<attach model="Humanoid" body="torso" prefix="1_"/>
<frame euler="0 0 72">
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="20 40 60">
<freejoint/>
<geom class="ellipsoid"/>
</body>
</replicate>
</frame>
<frame euler="0 0 144">
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="30 70 110">
<freejoint/>
<geom class="box"/>
</body>
</replicate>
</frame>
<frame pos="1 1 0" euler="0 0 144">
<attach model="Humanoid" body="torso" prefix="2_"/>
</frame>
<frame euler="0 0 216">
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="60 30 0">
<freejoint/>
<geom class="cylinder"/>
</body>
</replicate>
</frame>
<frame euler="0 0 288">
<replicate count="20" offset="0 0 0.2" euler="0 0 20">
<body pos="-2 0 0.5" euler="60 30 0">
<freejoint/>
<geom class="sphere"/>
</body>
</replicate>
</frame>
</worldbody>
</mujoco>
test/engine/testdata/island/humanoid.xml
Vendored
+252
View File
@@ -0,0 +1,252 @@
<mujoco model="Humanoid">
<option timestep="0.005"/>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
</visual>
<statistic center="0 0 0.7"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<default class="body">
<!-- geoms -->
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" group="1"/>
<default class="thigh">
<geom size=".06"/>
</default>
<default class="shin">
<geom fromto="0 0 0 0 0 -.3" size=".049"/>
</default>
<default class="foot">
<geom size=".027"/>
<default class="foot1">
<geom fromto="-.07 -.01 0 .14 -.03 0"/>
</default>
<default class="foot2">
<geom fromto="-.07 .01 0 .14 .03 0"/>
</default>
</default>
<default class="arm_upper">
<geom size=".04"/>
</default>
<default class="arm_lower">
<geom size=".031"/>
</default>
<default class="hand">
<geom type="sphere" size=".04"/>
</default>
<!-- joints -->
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="joint_big">
<joint damping="5" stiffness="10"/>
<default class="hip_x">
<joint range="-30 10"/>
</default>
<default class="hip_z">
<joint range="-60 35"/>
</default>
<default class="hip_y">
<joint axis="0 1 0" range="-150 20"/>
</default>
<default class="joint_big_stiff">
<joint stiffness="20"/>
</default>
</default>
<default class="knee">
<joint pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
</default>
<default class="ankle">
<joint range="-50 50"/>
<default class="ankle_y">
<joint pos="0 0 .08" axis="0 1 0" stiffness="6"/>
</default>
<default class="ankle_x">
<joint pos="0 0 .04" stiffness="3"/>
</default>
</default>
<default class="shoulder">
<joint range="-85 60"/>
</default>
<default class="elbow">
<joint range="-100 50" stiffness="0"/>
</default>
</default>
</default>
<worldbody>
<geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>
<light name="spotlight" mode="targetbodycom" target="torso" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 -6 4" cutoff="30"/>
<light name="top" pos="0 0 2" mode="trackcom"/>
<body name="torso" pos="0 0 1.282" childclass="body">
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="root"/>
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="waist_upper" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="head" pos="0 0 .19">
<geom name="head" type="sphere" size=".09"/>
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="waist_lower" pos="-.01 0 -.26">
<geom name="waist_lower" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="joint_big_stiff"/>
<joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="joint_big"/>
<body name="pelvis" pos="0 0 -.165">
<joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="joint_big"/>
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="thigh_right" pos="0 -.1 -.04">
<joint name="hip_x_right" axis="1 0 0" class="hip_x"/>
<joint name="hip_z_right" axis="0 0 1" class="hip_z"/>
<joint name="hip_y_right" class="hip_y"/>
<geom name="thigh_right" fromto="0 0 0 0 .01 -.34" class="thigh"/>
<body name="shin_right" pos="0 .01 -.4">
<joint name="knee_right" class="knee"/>
<geom name="shin_right" class="shin"/>
<body name="foot_right" pos="0 0 -.39">
<joint name="ankle_y_right" class="ankle_y"/>
<joint name="ankle_x_right" class="ankle_x" axis="1 0 .5"/>
<geom name="foot1_right" class="foot1"/>
<geom name="foot2_right" class="foot2"/>
</body>
</body>
</body>
<body name="thigh_left" pos="0 .1 -.04">
<joint name="hip_x_left" axis="-1 0 0" class="hip_x"/>
<joint name="hip_z_left" axis="0 0 -1" class="hip_z"/>
<joint name="hip_y_left" class="hip_y"/>
<geom name="thigh_left" fromto="0 0 0 0 -.01 -.34" class="thigh"/>
<body name="shin_left" pos="0 -.01 -.4">
<joint name="knee_left" class="knee"/>
<geom name="shin_left" fromto="0 0 0 0 0 -.3" class="shin"/>
<body name="foot_left" pos="0 0 -.39">
<joint name="ankle_y_left" class="ankle_y"/>
<joint name="ankle_x_left" class="ankle_x" axis="-1 0 -.5"/>
<geom name="foot1_left" class="foot1"/>
<geom name="foot2_left" class="foot2"/>
</body>
</body>
</body>
</body>
</body>
<body name="upper_arm_right" pos="0 -.17 .06">
<joint name="shoulder1_right" axis="2 1 1" class="shoulder"/>
<joint name="shoulder2_right" axis="0 -1 1" class="shoulder"/>
<geom name="upper_arm_right" fromto="0 0 0 .16 -.16 -.16" class="arm_upper"/>
<body name="lower_arm_right" pos=".18 -.18 -.18">
<joint name="elbow_right" axis="0 -1 1" class="elbow"/>
<geom name="lower_arm_right" fromto=".01 .01 .01 .17 .17 .17" class="arm_lower"/>
<body name="hand_right" pos=".18 .18 .18">
<geom name="hand_right" zaxis="1 1 1" class="hand"/>
</body>
</body>
</body>
<body name="upper_arm_left" pos="0 .17 .06">
<joint name="shoulder1_left" axis="-2 1 -1" class="shoulder"/>
<joint name="shoulder2_left" axis="0 -1 -1" class="shoulder"/>
<geom name="upper_arm_left" fromto="0 0 0 .16 .16 -.16" class="arm_upper"/>
<body name="lower_arm_left" pos=".18 .18 -.18">
<joint name="elbow_left" axis="0 -1 -1" class="elbow"/>
<geom name="lower_arm_left" fromto=".01 -.01 .01 .17 -.17 .17" class="arm_lower"/>
<body name="hand_left" pos=".18 -.18 .18">
<geom name="hand_left" zaxis="1 -1 1" class="hand"/>
</body>
</body>
</body>
</body>
</worldbody>
<contact>
<exclude body1="waist_lower" body2="thigh_right"/>
<exclude body1="waist_lower" body2="thigh_left"/>
</contact>
<tendon>
<fixed name="hamstring_right" limited="true" range="-0.3 2">
<joint joint="hip_y_right" coef=".5"/>
<joint joint="knee_right" coef="-.5"/>
</fixed>
<fixed name="hamstring_left" limited="true" range="-0.3 2">
<joint joint="hip_y_left" coef=".5"/>
<joint joint="knee_left" coef="-.5"/>
</fixed>
</tendon>
<actuator>
<motor name="abdomen_z" gear="40" joint="abdomen_z"/>
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
<motor name="abdomen_x" gear="40" joint="abdomen_x"/>
<motor name="hip_x_right" gear="40" joint="hip_x_right"/>
<motor name="hip_z_right" gear="40" joint="hip_z_right"/>
<motor name="hip_y_right" gear="120" joint="hip_y_right"/>
<motor name="knee_right" gear="80" joint="knee_right"/>
<motor name="ankle_y_right" gear="20" joint="ankle_y_right"/>
<motor name="ankle_x_right" gear="20" joint="ankle_x_right"/>
<motor name="hip_x_left" gear="40" joint="hip_x_left"/>
<motor name="hip_z_left" gear="40" joint="hip_z_left"/>
<motor name="hip_y_left" gear="120" joint="hip_y_left"/>
<motor name="knee_left" gear="80" joint="knee_left"/>
<motor name="ankle_y_left" gear="20" joint="ankle_y_left"/>
<motor name="ankle_x_left" gear="20" joint="ankle_x_left"/>
<motor name="shoulder1_right" gear="20" joint="shoulder1_right"/>
<motor name="shoulder2_right" gear="20" joint="shoulder2_right"/>
<motor name="elbow_right" gear="40" joint="elbow_right"/>
<motor name="shoulder1_left" gear="20" joint="shoulder1_left"/>
<motor name="shoulder2_left" gear="20" joint="shoulder2_left"/>
<motor name="elbow_left" gear="40" joint="elbow_left"/>
</actuator>
<keyframe>
<!--
The values below are split into rows for readibility:
torso position
torso orientation
spinal
right leg
left leg
arms
-->
<key name="squat"
qpos="0 0 0.596
0.988015 0 0.154359 0
0 0.4 0
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
0 0 0 0 0 0"/>
<key name="stand_on_left_leg"
qpos="0 0 1.21948
0.971588 -0.179973 0.135318 -0.0729076
-0.0516 -0.202 0.23
-0.24 -0.007 -0.34 -1.76 -0.466 -0.0415
-0.08 -0.01 -0.37 -0.685 -0.35 -0.09
0.109 -0.067 -0.7 -0.05 0.12 0.16"/>
<key name="prone"
qpos="0.4 0 0.0757706
0.7325 0 0.680767 0
0 0.0729 0
0.0077 0.0019 -0.026 -0.351 -0.27 0
0.0077 0.0019 -0.026 -0.351 -0.27 0
0.56 -0.62 -1.752
0.56 -0.62 -1.752"/>
<key name="supine"
qpos="-0.4 0 0.08122
0.722788 0 -0.69107 0
0 -0.25 0
0.0182 0.0142 0.3 0.042 -0.44 -0.02
0.0182 0.0142 0.3 0.042 -0.44 -0.02
0.186 -0.73 -1.73
0.186 -0.73 -1.73"/>
</keyframe>
</mujoco>