From 41e41b538d49f208187b17c4b67753a0e11de13a Mon Sep 17 00:00:00 2001 From: Vyankatesh Ashtekar Date: Mon, 26 Feb 2024 01:23:55 +0530 Subject: [PATCH] added two tests for the mj_subtreeAngMomMat() function --- test/engine/engine_support_test.cc | 115 +++++++++++++++++++++++++++++ 1 file changed, 115 insertions(+) diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index a2b3acda..fa2d53f2 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -37,6 +37,121 @@ using ::testing::ContainsRegex; using ::testing::MatchesRegex; using ::testing::Pointwise; using ::testing::ElementsAreArray; +using AngMomMatTest = MujocoTest; + +static constexpr char AngMomTestingModel[] = R"( + + + )"; + +// compare subtree angular momentum computed in two ways +TEST_F(AngMomMatTest, CompareAngMom) { + mjModel* model = LoadModelFromString(AngMomTestingModel); + int nv = model->nv; + int bodyid = mj_name2id(model, mjOBJ_BODY, "link1"); + mjData* data = mj_makeData(model); + + // let the mechanism move and generate some angular momentum + for (int i=0; i < 500; i++) { + mj_step(model, data); + } + + // get the reference value of angular momentum + mj_subtreeVel(model,data); + mjtNum angmom_ref[3]; + mju_copy3(angmom_ref, data->subtree_angmom+3*bodyid); + + // compute angular momentum using the angular momentum matrix + mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + mj_subtreeAngMomMat(model, data, angmom_mat, bodyid); + mjtNum angmom_test[3]; + mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv); + + // compare the two angular momentum values + static const mjtNum tol = 1e-3; + for(int i=0; i<3; i++) { + EXPECT_THAT(angmom_ref[i], DoubleNear(angmom_test[i], tol)); + } + + mju_free(angmom_mat); + mj_deleteData(data); + mj_deleteModel(model); +} + +// compare subtree angular momentum matrix computed in two ways: analytical and fdm +TEST_F(AngMomMatTest, CompareAngMomMats) { + mjModel* model = LoadModelFromString(AngMomTestingModel); + int nv = model->nv; + int bodyid = mj_name2id(model, mjOBJ_BODY, "link1"); + mjData* data = mj_makeData(model); + mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + mjtNum* angmom_mat_fdm = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + + // let the mechanism move and generate some angular momentum + for (int i=0; i < 500; i++) { + mj_step(model, data); + } + + // compute the angular momentum matrix using the analytical method + mj_subtreeAngMomMat(model, data, angmom_mat, bodyid); + + // compute the angular momentum matrix using finite differences + static const mjtNum eps = 1e-3; + static const mjtNum tol = 1e-4; + + // backup original qvel and save the angular momentum (H) + mjtNum* qvel0 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv); + mju_copy(qvel0, data->qvel, nv); + mj_subtreeVel(model, data); + mjtNum agm0[3]; + mju_copy3(agm0, data->subtree_angmom+3*bodyid); + + // acceleration nudge + mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv); + mju_zero(nudge, nv); + + // H = angmomMat * qvel + // dH = angmomMat * dqvel + // the following proves that angmomMat is only a function of qpos + for(int i=0; i < nv; i++) { + // reset qvel, nudge i-th dof, update data->qvel, reset nudge + mju_copy(data->qvel, qvel0, nv); + nudge[i] = 1; + mju_addToScl(data->qvel, nudge, eps, nv); + nudge[i] = 0; + + // compute new value of H + mj_forward(model, data); + mj_subtreeVel(model, data); + + for(int j=0; j < 3; j++) { + angmom_mat_fdm[nv*j+i] = (data->subtree_angmom[3*bodyid+j] - agm0[j]) / (1 * eps); + EXPECT_THAT(angmom_mat_fdm[nv*j+i], DoubleNear(angmom_mat[nv*j+i], tol)); + } + } + + // restore original qvel (doesn't in the test here) + mju_copy(data->qvel, qvel0, nv); + + mju_free(nudge); + mju_free(qvel0); + mju_free(angmom_mat_fdm); + mju_free(angmom_mat); + mj_deleteData(data); + mj_deleteModel(model); +} + using JacobianTest = MujocoTest; static const mjtNum max_abs_err = std::numeric_limits::epsilon();