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

Merge pull request #1450 from v-r-a:angmomMat

Browse Source 
PiperOrigin-RevId: 612835207
Change-Id: I8e7b01cc433563556a3ae4bbdd0c53612cc15e97
This commit is contained in:
Copybara-Service
2024-03-05 07:35:15 -08:00
parent 49ddb7ca2e 93f9154d44
commit 6b7d714291
12 changed files with 279 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_support_test.cc
+121 -1
View File
@@ -16,6 +16,7 @@
#include "src/engine/engine_support.h"
#include <limits>
#include <random>
#include <string>
#include <vector>
@@ -37,6 +38,125 @@ using ::testing::ContainsRegex;
using ::testing::MatchesRegex;
using ::testing::Pointwise;
using ::testing::ElementsAreArray;
using AngMomMatTest = MujocoTest;
static constexpr char AngMomTestingModel[] = R"(
<mujoco>
<option>
<flag gravity="disable"/>
</option>
<worldbody>
<body name="link1" pos="0 0 0.5">
<freejoint/>
<geom type="ellipsoid" size="0.15 0.17 0.19" quat="1 .2 .3 .4"/>
<body name="link2" >
<joint type="hinge" axis="1 0 0" />
<geom type="capsule" size="0.05" fromto="0 0 0 0 0.5 0"/>
<body pos="0 0.6 0">
<joint type="slide" axis="1 0 0"/>
<geom type="capsule" size="0.05 0.2" quat="0.707 0 0.707 0"/>
<body name="link3">
<joint type="ball" pos="0.2 0 0"/>
<geom type="capsule" pos="0.2 0 0" size="0.03 0.4"/>
</body>
</body>
</body>
</body>
</worldbody>
<keyframe>
<key qvel="0 0 0 .1 .2 .3 .4 .5 .4 .3 .2"/>
</keyframe>
</mujoco>
)";
// 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);
// reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0);
mj_forward(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_angmomMat(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-8;
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: analytical and findiff
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_fd = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
// reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
// compute the angular momentum matrix using the analytical method
mj_angmomMat(model, data, angmom_mat, bodyid);
// compute the angular momentum matrix using finite differences
static const mjtNum eps = 1e-6;
for (int i = 0; i < nv; i++) {
// reset vel, forward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] += eps;
mj_forward(model, data);
mj_subtreeVel(model, data);
mjtNum agmf[3];
mju_copy3(agmf, data->subtree_angmom+3*bodyid);
// reset vel, backward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] -= eps;
mj_forward(model, data);
mj_subtreeVel(model, data);
mjtNum agmb[3];
mju_copy3(agmb, data->subtree_angmom+3*bodyid);
// finite-difference the angmom matrix
for (int j = 0; j < 3; j++) {
angmom_mat_fd[nv*j+i] = (agmf[j] - agmb[j]) / (2 * eps);
}
}
// compare the two matrices
static const mjtNum tol = 1e-8;
for (int i = 0; i < 3*nv; i++) {
EXPECT_THAT(angmom_mat_fd[i], DoubleNear(angmom_mat[i], tol));
}
mju_free(angmom_mat_fd);
mju_free(angmom_mat);
mj_deleteData(data);
mj_deleteModel(model);
}
using JacobianTest = MujocoTest;
static const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
@@ -446,7 +566,7 @@ TEST_F(AddMTest, DenseSameAsSparse) {
rowadr.data(), colind.data());
// dense addM
mj_addM(m, d, dst_dense.data(), NULL, NULL, NULL);
mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr);
// dense comparison, should be same matrix
EXPECT_THAT(dst_dense, ElementsAreArray(dst_sparse));