From ebf9d637022d5aa1504bd31e38ac3e8c2393e7a2 Mon Sep 17 00:00:00 2001 From: Vyankatesh Date: Mon, 26 Feb 2024 14:48:47 +0530 Subject: [PATCH] changed the model, using central differences in CompareAngMomMats test --- test/engine/engine_support_test.cc | 65 ++++++++++++++++++++---------- 1 file changed, 44 insertions(+), 21 deletions(-) diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index fa2d53f2..daa5ea38 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -43,13 +43,22 @@ static constexpr char AngMomTestingModel[] = R"( @@ -62,8 +71,8 @@ TEST_F(AngMomMatTest, CompareAngMom) { 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++) { + // let the mechanism move for 1 sec and gain some angular momentum + for (int i=0; i < 1000; i++) { mj_step(model, data); } @@ -79,7 +88,7 @@ TEST_F(AngMomMatTest, CompareAngMom) { mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv); // compare the two angular momentum values - static const mjtNum tol = 1e-3; + static const mjtNum tol = 1e-4; for(int i=0; i<3; i++) { EXPECT_THAT(angmom_ref[i], DoubleNear(angmom_test[i], tol)); } @@ -98,8 +107,8 @@ TEST_F(AngMomMatTest, CompareAngMomMats) { 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++) { + // let the mechanism move for 1 sec and gain some angular momentum + for (int i=0; i < 1000; i++) { mj_step(model, data); } @@ -107,10 +116,10 @@ TEST_F(AngMomMatTest, CompareAngMomMats) { 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; + static const mjtNum eps = 1e-6; + static const mjtNum tol = 1e-5; - // backup original qvel and save the angular momentum (H) + // save current qvel and computed angular momentum (H) mjtNum* qvel0 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv); mju_copy(qvel0, data->qvel, nv); mj_subtreeVel(model, data); @@ -124,24 +133,38 @@ TEST_F(AngMomMatTest, CompareAngMomMats) { // H = angmomMat * qvel // dH = angmomMat * dqvel // the following proves that angmomMat is only a function of qpos + // using centre difference method + mjtNum agmf[3], agmb[3]; for(int i=0; i < nv; i++) { - // reset qvel, nudge i-th dof, update data->qvel, reset nudge - mju_copy(data->qvel, qvel0, nv); + // reset vel, forward nudge i-th dof, update data->qvel, reset nudge + mju_copy(dd->qvel, qvel0, mm->nv); nudge[i] = 1; - mju_addToScl(data->qvel, nudge, eps, nv); + mju_addToScl(dd->qvel, nudge, eps, mm->nv); nudge[i] = 0; - // compute new value of H + // new value of angmom mj_forward(model, data); mj_subtreeVel(model, data); + mju_copy3(agmf, dd->subtree_angmom+3*bodyid); + + // reset vel, backward nudge i-th dof, update data->qvel, reset nudge + mju_copy(dd->qvel, qvel0, mm->nv); + nudge[i] = -1; + mju_addToScl(dd->qvel, nudge, eps, mm->nv); + nudge[i] = 0; + + // new value of angmom + mj_forward(model, data); + mj_subtreeVel(model, data); + mju_copy3(agmb, dd->subtree_angmom+3*bodyid); for(int j=0; j < 3; j++) { - angmom_mat_fdm[nv*j+i] = (data->subtree_angmom[3*bodyid+j] - agm0[j]) / (1 * eps); + angmom_mat_fdm[nv*j+i] = (agmf[j] - agmb[j]) / (2 * 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) + // restore original qvel (doesn't matter in the test here) mju_copy(data->qvel, qvel0, nv); mju_free(nudge);