From fad7ed30d21a2046a0a4b6303328f04e0a3e94f6 Mon Sep 17 00:00:00 2001 From: Vyankatesh Ashtekar Date: Sun, 25 Feb 2024 21:09:11 +0530 Subject: [PATCH 1/6] added function mj_subtreeAngMomMat() --- src/engine/engine_support.c | 71 +++++++++++++++++++++++++++++++++++++ src/engine/engine_support.h | 3 ++ 2 files changed, 74 insertions(+) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 38df9a14..977f602f 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -775,6 +775,77 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain, } +// compute subtree angular momentum matrix +void mj_subtreeAngMomMat(const mjModel *m, mjData *d, mjtNum *agm, int body) +{ + int nv = m->nv; + mj_markStack(d); + mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv); + mjtNum* jacr_b = mj_stackAllocNum(d, 3*nv); + mjtNum r_com_b[3]; + mjtNum R_com_b[9]; + mjtNum r_com[3]; + mjtNum Ib[9]; + mjtNum Xbcom[9]; + mjtNum temp1[9]; + mjtNum temp2[9]; + mjtNum* term1 = mj_stackAllocNum(d, 3*nv); + mjtNum* term2 = mj_stackAllocNum(d, 3*nv); + + // clear output and other matrices + mju_zero(agm, 3*nv); + mju_zero(R_com_b, 9); + mju_zero(Ib, 9); + + // save the location of the subtree COM + mju_copy3(r_com, d->subtree_com+3*body); + + for (int b=body; b < m->nbody; b++) + { + // end of body subtree, break from the loop + if (b > body && m->body_parentid[b] < body) + { + break; + } + + // linear and angular velocity Jacobian of the body COM (inertial frame) + mj_jacBodyCom(m, d, jacp_b, jacr_b, b); + + // orientation of the COM (intertial) frame of b-th body + mju_copy(Xbcom, d->ximat+9*b, 9); + + // save the inertia matrix of b-th body + Ib[0] = m->body_inertia[3*b]; // Ib(1,1) + Ib[4] = m->body_inertia[3*b+1]; // Ib(2,2) + Ib[8] = m->body_inertia[3*b+2]; // Ib(3,3) + + // compute the body angular momentum about self COM in world frame + mju_mulMatMat(temp1, Xbcom, Ib, 3, 3, 3); // Xbcom*Ib + mju_mulMatMatT(temp2, temp1, Xbcom, 3, 3, 3); // Xbcom*Ib*Xbcom^T + mju_mulMatMat(term1, temp2, jacr_b, 3, 3, nv); // Xbcom*Ib*Xbcom^T*Jr + + // compute the location of body COM w.r.t. subtree COM + mju_sub3(r_com_b, (d->xipos+3*b), r_com); + + // skew symm matrix representing r_com_b vector + R_com_b[1] = -r_com_b[2]; + R_com_b[2] = r_com_b[1]; + R_com_b[3] = r_com_b[2]; + R_com_b[5] = -r_com_b[0]; + R_com_b[6] = -r_com_b[1]; + R_com_b[7] = r_com_b[0]; + + // moment of linear momentum + mju_mulMatMat(term2, R_com_b, jacp_b, 3, 3, nv); // R_com_b*Jp + mju_scl(term2, term2, m->body_mass[b], 3 * nv); // R_com_b*Jp*mi + + // amg += Xbcom*Ib*Xbcom^T*Jr + R_com_b*Jp*m + mju_addTo(agm, term1, 3*nv); + mju_addTo(agm, term2, 3*nv); + } +} + + //-------------------------- name functions -------------------------------------------------------- diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index abb8a4ff..ce27a83e 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -107,6 +107,9 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain, int n, const int* body, const mjtNum* weight, const mjtNum point[3], mjtNum* jac, int flg_rot); +// compute subtree angular momentum matrix +MJAPI void mj_subtreeAngMomMat(const mjModel *m, mjData *d, mjtNum *agm, int body); + //-------------------------- name functions -------------------------------------------------------- From fe9b057dfe13d405bf6329f7ba963d2a06570736 Mon Sep 17 00:00:00 2001 From: Vyankatesh Ashtekar Date: Sun, 25 Feb 2024 21:17:44 +0530 Subject: [PATCH 2/6] minor style corrections --- src/engine/engine_support.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 977f602f..64be0fba 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -800,11 +800,9 @@ void mj_subtreeAngMomMat(const mjModel *m, mjData *d, mjtNum *agm, int body) // save the location of the subtree COM mju_copy3(r_com, d->subtree_com+3*body); - for (int b=body; b < m->nbody; b++) - { + for (int b=body; b < m->nbody; b++) { // end of body subtree, break from the loop - if (b > body && m->body_parentid[b] < body) - { + if (b > body && m->body_parentid[b] < body) { break; } From 41e41b538d49f208187b17c4b67753a0e11de13a Mon Sep 17 00:00:00 2001 From: Vyankatesh Ashtekar Date: Mon, 26 Feb 2024 01:23:55 +0530 Subject: [PATCH 3/6] 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(); From 063edf5a091bf0d959dc15805aaf36977c7740a1 Mon Sep 17 00:00:00 2001 From: Vyankatesh Date: Mon, 26 Feb 2024 12:03:42 +0530 Subject: [PATCH 4/6] corrections --- src/engine/engine_support.c | 123 ++++++++++++++++++------------------ 1 file changed, 62 insertions(+), 61 deletions(-) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 64be0fba..4a6145ab 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -776,71 +776,72 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain, // compute subtree angular momentum matrix -void mj_subtreeAngMomMat(const mjModel *m, mjData *d, mjtNum *agm, int body) -{ - int nv = m->nv; - mj_markStack(d); - mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv); - mjtNum* jacr_b = mj_stackAllocNum(d, 3*nv); - mjtNum r_com_b[3]; - mjtNum R_com_b[9]; - mjtNum r_com[3]; - mjtNum Ib[9]; - mjtNum Xbcom[9]; - mjtNum temp1[9]; - mjtNum temp2[9]; - mjtNum* term1 = mj_stackAllocNum(d, 3*nv); - mjtNum* term2 = mj_stackAllocNum(d, 3*nv); +void mj_subtreeAngMomMat(const mjModel *m, mjData *d, mjtNum *agm, int body) { + int nv = m->nv; + mj_markStack(d); + mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv); + mjtNum* jacr_b = mj_stackAllocNum(d, 3*nv); + mjtNum r_com_b[3]; + mjtNum R_com_b[9]; + mjtNum r_com[3]; + mjtNum Ib[9]; + mjtNum Xbcom[9]; + mjtNum temp1[9]; + mjtNum temp2[9]; + mjtNum* term1 = mj_stackAllocNum(d, 3*nv); + mjtNum* term2 = mj_stackAllocNum(d, 3*nv); - // clear output and other matrices - mju_zero(agm, 3*nv); - mju_zero(R_com_b, 9); - mju_zero(Ib, 9); + // clear output and other matrices + mju_zero(agm, 3*nv); + mju_zero(R_com_b, 9); + mju_zero(Ib, 9); - // save the location of the subtree COM - mju_copy3(r_com, d->subtree_com+3*body); + // save the location of the subtree COM + mju_copy3(r_com, d->subtree_com+3*body); - for (int b=body; b < m->nbody; b++) { - // end of body subtree, break from the loop - if (b > body && m->body_parentid[b] < body) { - break; - } - - // linear and angular velocity Jacobian of the body COM (inertial frame) - mj_jacBodyCom(m, d, jacp_b, jacr_b, b); - - // orientation of the COM (intertial) frame of b-th body - mju_copy(Xbcom, d->ximat+9*b, 9); - - // save the inertia matrix of b-th body - Ib[0] = m->body_inertia[3*b]; // Ib(1,1) - Ib[4] = m->body_inertia[3*b+1]; // Ib(2,2) - Ib[8] = m->body_inertia[3*b+2]; // Ib(3,3) - - // compute the body angular momentum about self COM in world frame - mju_mulMatMat(temp1, Xbcom, Ib, 3, 3, 3); // Xbcom*Ib - mju_mulMatMatT(temp2, temp1, Xbcom, 3, 3, 3); // Xbcom*Ib*Xbcom^T - mju_mulMatMat(term1, temp2, jacr_b, 3, 3, nv); // Xbcom*Ib*Xbcom^T*Jr - - // compute the location of body COM w.r.t. subtree COM - mju_sub3(r_com_b, (d->xipos+3*b), r_com); - - // skew symm matrix representing r_com_b vector - R_com_b[1] = -r_com_b[2]; - R_com_b[2] = r_com_b[1]; - R_com_b[3] = r_com_b[2]; - R_com_b[5] = -r_com_b[0]; - R_com_b[6] = -r_com_b[1]; - R_com_b[7] = r_com_b[0]; - - // moment of linear momentum - mju_mulMatMat(term2, R_com_b, jacp_b, 3, 3, nv); // R_com_b*Jp - mju_scl(term2, term2, m->body_mass[b], 3 * nv); // R_com_b*Jp*mi - - // amg += Xbcom*Ib*Xbcom^T*Jr + R_com_b*Jp*m - mju_addTo(agm, term1, 3*nv); - mju_addTo(agm, term2, 3*nv); + for (int b=body; b < m->nbody; b++) { + // end of body subtree, break from the loop + if (b > body && m->body_parentid[b] < body) { + break; } + + // linear and angular velocity Jacobian of the body COM (inertial frame) + mj_jacBodyCom(m, d, jacp_b, jacr_b, b); + + // orientation of the COM (intertial) frame of b-th body + mju_copy(Xbcom, d->ximat+9*b, 9); + + // save the inertia matrix of b-th body + Ib[0] = m->body_inertia[3*b]; // Ib(1,1) + Ib[4] = m->body_inertia[3*b+1]; // Ib(2,2) + Ib[8] = m->body_inertia[3*b+2]; // Ib(3,3) + + // compute the body angular momentum about self COM in world frame + mju_mulMatMat(temp1, Xbcom, Ib, 3, 3, 3); // Xbcom*Ib + mju_mulMatMatT(temp2, temp1, Xbcom, 3, 3, 3); // Xbcom*Ib*Xbcom^T + mju_mulMatMat(term1, temp2, jacr_b, 3, 3, nv); // Xbcom*Ib*Xbcom^T*Jr + + // compute the location of body COM w.r.t. subtree COM + mju_sub3(r_com_b, (d->xipos+3*b), r_com); + + // skew symm matrix representing r_com_b vector + R_com_b[1] = -r_com_b[2]; + R_com_b[2] = r_com_b[1]; + R_com_b[3] = r_com_b[2]; + R_com_b[5] = -r_com_b[0]; + R_com_b[6] = -r_com_b[1]; + R_com_b[7] = r_com_b[0]; + + // moment of linear momentum + mju_mulMatMat(term2, R_com_b, jacp_b, 3, 3, nv); // R_com_b*Jp + mju_scl(term2, term2, m->body_mass[b], 3 * nv); // R_com_b*Jp*mi + + // amg += Xbcom*Ib*Xbcom^T*Jr + R_com_b*Jp*m + mju_addTo(agm, term1, 3*nv); + mju_addTo(agm, term2, 3*nv); + } + + mj_freeStack(d); } From ebf9d637022d5aa1504bd31e38ac3e8c2393e7a2 Mon Sep 17 00:00:00 2001 From: Vyankatesh Date: Mon, 26 Feb 2024 14:48:47 +0530 Subject: [PATCH 5/6] 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); From 3fd0fe7884b7cf2e400a0cdb851b7513d80133e6 Mon Sep 17 00:00:00 2001 From: Vyankatesh Date: Mon, 26 Feb 2024 20:15:22 +0530 Subject: [PATCH 6/6] corrections --- test/engine/engine_support_test.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index daa5ea38..eedb9741 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -137,26 +137,26 @@ TEST_F(AngMomMatTest, CompareAngMomMats) { mjtNum agmf[3], agmb[3]; for(int i=0; i < nv; i++) { // reset vel, forward nudge i-th dof, update data->qvel, reset nudge - mju_copy(dd->qvel, qvel0, mm->nv); + mju_copy(data->qvel, qvel0, model->nv); nudge[i] = 1; - mju_addToScl(dd->qvel, nudge, eps, mm->nv); + mju_addToScl(data->qvel, nudge, eps, model->nv); nudge[i] = 0; // new value of angmom mj_forward(model, data); mj_subtreeVel(model, data); - mju_copy3(agmf, dd->subtree_angmom+3*bodyid); + mju_copy3(agmf, data->subtree_angmom+3*bodyid); // reset vel, backward nudge i-th dof, update data->qvel, reset nudge - mju_copy(dd->qvel, qvel0, mm->nv); + mju_copy(data->qvel, qvel0, model->nv); nudge[i] = -1; - mju_addToScl(dd->qvel, nudge, eps, mm->nv); + mju_addToScl(data->qvel, nudge, eps, model->nv); nudge[i] = 0; // new value of angmom mj_forward(model, data); mj_subtreeVel(model, data); - mju_copy3(agmb, dd->subtree_angmom+3*bodyid); + mju_copy3(agmb, data->subtree_angmom+3*bodyid); for(int j=0; j < 3; j++) { angmom_mat_fdm[nv*j+i] = (agmf[j] - agmb[j]) / (2 * eps);