Implement multi-cell finite element method for interpolated flexes.
This change introduces a `flex_cellcount` field to `mjModel` to specify the number of cells in each dimension for interpolated flexes. The stiffness computation, passive force calculation, and Jacobian derivatives are updated to operate on a per-cell basis, significantly improving performance by localizing computations to the nodes within each cell. PiperOrigin-RevId: 901216393 Change-Id: Ic23132e609de11e71bb7fef8d1f139daad2ec264
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
8415dff307
commit
6c7ed66781
@@ -14,12 +14,9 @@
|
||||
|
||||
// Tests for engine/{engine_support.c and engine_core_util.c}
|
||||
|
||||
#include "src/engine/engine_core_util.h"
|
||||
#include "src/engine/engine_support.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <random>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
@@ -41,521 +38,9 @@ using ::testing::Ne;
|
||||
using ::testing::NotNull;
|
||||
using ::testing::Pointwise;
|
||||
|
||||
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) {
|
||||
char error[1024];
|
||||
mjModel* model =
|
||||
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
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
|
||||
for (int i = 0; i < 3; i++) {
|
||||
EXPECT_THAT(angmom_ref[i], MjNear(angmom_test[i], 1e-8, 1e-4));
|
||||
}
|
||||
|
||||
mju_free(angmom_mat);
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// compare subtree angular momentum matrix: analytical and findiff
|
||||
TEST_F(AngMomMatTest, CompareAngMomMats) {
|
||||
char error[1024];
|
||||
mjModel* model =
|
||||
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
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 constexpr mjtNum eps = MjTol(1e-6, 1e-3);
|
||||
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
|
||||
for (int i = 0; i < 3*nv; i++) {
|
||||
EXPECT_THAT(angmom_mat_fd[i], MjNear(angmom_mat[i], 1e-8, 2e-4));
|
||||
}
|
||||
|
||||
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();
|
||||
|
||||
static constexpr char kJacobianTestingModel[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body name="distractor1" pos="0 0 .3">
|
||||
<freejoint/>
|
||||
<geom size=".1"/>
|
||||
</body>
|
||||
<body name="main">
|
||||
<freejoint/>
|
||||
<geom size=".1"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint axis="0 1 0"/>
|
||||
<geom type="capsule" size=".03" fromto="0 0 0 .2 0 0"/>
|
||||
</body>
|
||||
<body pos="0 .1 0">
|
||||
<joint type="ball"/>
|
||||
<geom type="capsule" size=".03" fromto="0 0 0 0 .2 0"/>
|
||||
<body pos="0 .2 0">
|
||||
<joint type="slide" axis="1 1 1"/>
|
||||
<geom size=".05"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="distractor2" pos="0 0 -.3">
|
||||
<freejoint/>
|
||||
<geom size=".1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
// compare analytic and finite-differenced subtree-com Jacobian
|
||||
TEST_F(JacobianTest, SubtreeJac) {
|
||||
char error[1024];
|
||||
mjModel* model =
|
||||
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
int nv = model->nv;
|
||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
|
||||
mjData* data = mj_makeData(model);
|
||||
mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
|
||||
mjtNum* qpos = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nq);
|
||||
mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv);
|
||||
|
||||
// all we need for Jacobians are kinematics and CoM-related quantities
|
||||
mj_kinematics(model, data);
|
||||
mj_comPos(model, data);
|
||||
|
||||
// get subtree CoM Jacobian of free body
|
||||
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
|
||||
|
||||
// save current subtree-com and qpos, clear nudge
|
||||
mjtNum subtree_com[3];
|
||||
mju_copy3(subtree_com, data->subtree_com+3*bodyid);
|
||||
mju_copy(qpos, data->qpos, model->nq);
|
||||
mju_zero(nudge, nv);
|
||||
|
||||
// compare analytic Jacobian to finite-difference approximation
|
||||
static const mjtNum eps = 1e-6;
|
||||
for (int i=0; i < nv; i++) {
|
||||
// reset qpos, nudge i-th dof, update data->qpos, reset nudge
|
||||
mju_copy(data->qpos, qpos, model->nq);
|
||||
nudge[i] = 1;
|
||||
mj_integratePos(model, data->qpos, nudge, eps);
|
||||
nudge[i] = 0;
|
||||
|
||||
// kinematics and comPos to get nudged com
|
||||
mj_kinematics(model, data);
|
||||
mj_comPos(model, data);
|
||||
|
||||
// compare finite-differenced and analytic Jacobian
|
||||
for (int j=0; j < 3; j++) {
|
||||
mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps;
|
||||
EXPECT_THAT(jac_subtree[nv*j+i], MjNear(findiff, eps, 1e-2));
|
||||
}
|
||||
}
|
||||
|
||||
mju_free(nudge);
|
||||
mju_free(qpos);
|
||||
mju_free(jac_subtree);
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// confirm that applying linear forces via the subtree-com Jacobian only creates
|
||||
// the expected linear accelerations (no accelerations of internal joints)
|
||||
TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
|
||||
char error[1024];
|
||||
mjModel* model =
|
||||
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
int nv = model->nv;
|
||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
|
||||
mjData* data = mj_makeData(model);
|
||||
mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
|
||||
|
||||
// all we need for Jacobians are kinematics and CoM-related quantities
|
||||
mj_kinematics(model, data);
|
||||
mj_comPos(model, data);
|
||||
|
||||
// get subtree CoM Jacobian of free body
|
||||
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
|
||||
|
||||
// uncomment for debugging
|
||||
// mju_printMat(jac_subtree, 3, nv);
|
||||
|
||||
// call fwdPosition since we'll need the factorised mass matrix in the test
|
||||
mj_fwdPosition(model, data);
|
||||
|
||||
// treating the subtree Jacobian as the projection of 3 axis-aligned unit
|
||||
// forces into joint space, solve for the resulting accelerations in-place
|
||||
mj_solveM(model, data, jac_subtree, jac_subtree, 3);
|
||||
|
||||
// expect to find accelerations of magnitude 1/subtreemass in the first 3
|
||||
// coordinates of the free joint and 0s elsewhere, since applying forces to
|
||||
// the CoM should accelerate the whole mechanism without any internal motion
|
||||
int body_dofadr = model->body_dofadr[bodyid];
|
||||
mjtNum invtreemass = 1.0/model->body_subtreemass[bodyid];
|
||||
for (int r = 0; r < 3; r++) {
|
||||
for (int c = 0; c < nv; c++) {
|
||||
mjtNum expected = c - body_dofadr == r ? invtreemass : 0.0;
|
||||
EXPECT_THAT(jac_subtree[nv*r+c], MjNear(expected, max_abs_err, 1e-4));
|
||||
}
|
||||
}
|
||||
|
||||
mju_free(jac_subtree);
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
static constexpr char kQuat[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body name="query">
|
||||
<joint type="ball"/>
|
||||
<geom size="1"/>
|
||||
<site name="query" pos=".1 .2 .3"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<keyframe>
|
||||
<key qvel="2 3 5"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
static constexpr char kFreeBall[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body name="distractor1" pos="0 0 .3">
|
||||
<freejoint/>
|
||||
<geom size=".1"/>
|
||||
</body>
|
||||
<body name="main">
|
||||
<freejoint/>
|
||||
<geom size=".1"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint axis="0 1 0"/>
|
||||
<geom type="capsule" size=".03" fromto="0 0 0 .2 0 0"/>
|
||||
<body pos=".2 0 0">
|
||||
<joint type="ball" stiffness="20"/>
|
||||
<geom type="capsule" size=".03" fromto="0 0 0 0 .2 0"/>
|
||||
<body name="query" pos="0 .2 0">
|
||||
<joint type="slide" axis="1 1 1"/>
|
||||
<geom size=".05"/>
|
||||
<site name="query" pos=".1 .2 .3"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="distractor2" pos="0 0 -.3">
|
||||
<freejoint/>
|
||||
<geom size=".1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<keyframe>
|
||||
<key qvel="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
static constexpr char kQuatlessPendulum[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicit">
|
||||
<flag constraint="disable"/>
|
||||
</option>
|
||||
<worldbody>
|
||||
<body pos="0.15 0 0">
|
||||
<joint type="hinge" axis="0 1 0"/>
|
||||
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
|
||||
<body pos="0.1 0 0">
|
||||
<joint type="slide" axis="1 0 0" stiffness="200"/>
|
||||
<geom type="capsule" size="0.015" fromto="-.1 0 0 .1 0 0"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint axis="1 0 0"/>
|
||||
<joint axis="0 1 0"/>
|
||||
<joint axis="0 0 1"/>
|
||||
<geom type="box" size=".02" fromto="0 0 0 0 .1 0"/>
|
||||
<body name="query" pos="0 .1 0">
|
||||
<joint axis="1 0 0"/>
|
||||
<geom type="capsule" size="0.02" fromto="0 0 0 0 .1 0"/>
|
||||
<site name="query" pos=".1 0 0"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
static constexpr char kTelescope[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body>
|
||||
<joint type="ball"/>
|
||||
<geom type="capsule" size="0.02" fromto="0 .02 0 .1 .02 0"/>
|
||||
<body pos=".1 .02 0">
|
||||
<joint type="slide" axis="1 0 0"/>
|
||||
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
|
||||
<body pos=".1 .02 0">
|
||||
<joint type="slide" axis="1 0 0"/>
|
||||
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
|
||||
<body pos=".1 .02 0" name="query">
|
||||
<joint type="slide" axis="1 0 0"/>
|
||||
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
|
||||
<site name="query" pos=".1 0 0"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
<keyframe>
|
||||
<key qvel="1 1 1 1 1 1"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
static constexpr char kHinge[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body name="query">
|
||||
<joint name="link1" axis="0 1 0"/>
|
||||
<geom type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
|
||||
<site name="query" pos="0 0 -1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<keyframe>
|
||||
<key qpos="1" qvel="1"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
// compare mj_jacDot with finite-differenced mj_jac
|
||||
TEST_F(JacobianTest, JacDot) {
|
||||
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
|
||||
char error[1024];
|
||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
int nv = model->nv;
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
// load keyframe if present, step for a bit
|
||||
if (model->nkey) mj_resetDataKeyframe(model, data, 0);
|
||||
while (data->time < 0.1) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// minimal call required for mj_jacDot outputs to be valid
|
||||
mj_kinematics(model, data);
|
||||
mj_comPos(model, data);
|
||||
mj_comVel(model, data);
|
||||
|
||||
// get bodyid
|
||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "query");
|
||||
EXPECT_GT(bodyid, 0);
|
||||
|
||||
// get site position
|
||||
int siteid = mj_name2id(model, mjOBJ_SITE, "query");
|
||||
EXPECT_GT(siteid, -1);
|
||||
mjtNum point[3];
|
||||
mju_copy3(point, data->site_xpos+3*siteid);
|
||||
|
||||
// jac, jac_dot
|
||||
vector<mjtNum> jacp(3*nv);
|
||||
vector<mjtNum> jacr(3*nv);
|
||||
mj_jac(model, data, jacp.data(), jacr.data(), point, bodyid);
|
||||
vector<mjtNum> jacp_dot(3*nv);
|
||||
vector<mjtNum> jacr_dot(3*nv);
|
||||
mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid);
|
||||
|
||||
// jac_h: jacobian after integrating qpos with a timestep of h
|
||||
constexpr mjtNum h = MjTol(1e-7, 5e-4);
|
||||
mj_integratePos(model, data->qpos, data->qvel, h);
|
||||
mj_kinematics(model, data);
|
||||
mj_comPos(model, data);
|
||||
vector<mjtNum> jacp_h(3*nv);
|
||||
vector<mjtNum> jacr_h(3*nv);
|
||||
mju_copy3(point, data->site_xpos+3*siteid); // get updated site position
|
||||
mj_jac(model, data, jacp_h.data(), jacr_h.data(), point, bodyid);
|
||||
|
||||
// jac_dot_h finite-difference approximation
|
||||
vector<mjtNum> jacp_dot_h(3*nv);
|
||||
mju_sub(jacp_dot_h.data(), jacp_h.data(), jacp.data(), 3*nv);
|
||||
mju_scl(jacp_dot_h.data(), jacp_dot_h.data(), 1/h, 3*nv);
|
||||
vector<mjtNum> jacr_dot_h(3*nv);
|
||||
mju_sub(jacr_dot_h.data(), jacr_h.data(), jacr.data(), 3*nv);
|
||||
mju_scl(jacr_dot_h.data(), jacr_dot_h.data(), 1/h, 3*nv);
|
||||
|
||||
// compare finite-differenced and analytic
|
||||
mjtNum tol = 1e-5;
|
||||
EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h));
|
||||
EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h));
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
}
|
||||
|
||||
// compare mj_jacDotSparse with dense mj_jacDot
|
||||
TEST_F(JacobianTest, JacDotSparse) {
|
||||
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
|
||||
char error[1024];
|
||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
int nv = model->nv;
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
// load keyframe if present, step for a bit
|
||||
if (model->nkey) mj_resetDataKeyframe(model, data, 0);
|
||||
while (data->time < 0.1) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// minimal call required for mj_jacDot outputs to be valid
|
||||
mj_kinematics(model, data);
|
||||
mj_comPos(model, data);
|
||||
mj_comVel(model, data);
|
||||
|
||||
// get bodyid and site position
|
||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "query");
|
||||
EXPECT_GT(bodyid, 0);
|
||||
int siteid = mj_name2id(model, mjOBJ_SITE, "query");
|
||||
EXPECT_GT(siteid, -1);
|
||||
mjtNum point[3];
|
||||
mju_copy3(point, data->site_xpos+3*siteid);
|
||||
|
||||
// dense jacDot
|
||||
vector<mjtNum> jacp_dense(3*nv);
|
||||
vector<mjtNum> jacr_dense(3*nv);
|
||||
mj_jacDot(model, data, jacp_dense.data(), jacr_dense.data(), point, bodyid);
|
||||
|
||||
// compute body chain using public mjModel fields
|
||||
vector<int> chain(nv);
|
||||
int NV = 0;
|
||||
int weldbody = model->body_weldid[bodyid];
|
||||
if (weldbody) {
|
||||
int da = model->body_dofadr[weldbody] + model->body_dofnum[weldbody] - 1;
|
||||
while (da >= 0) {
|
||||
chain[NV++] = da;
|
||||
da = model->dof_parentid[da];
|
||||
}
|
||||
std::reverse(chain.begin(), chain.begin() + NV);
|
||||
}
|
||||
EXPECT_GT(NV, 0);
|
||||
|
||||
// sparse jacDot
|
||||
vector<mjtNum> jacp_sparse(3*NV);
|
||||
vector<mjtNum> jacr_sparse(3*NV);
|
||||
mj_jacDotSparse(model, data, jacp_sparse.data(), jacr_sparse.data(),
|
||||
point, bodyid, NV, chain.data());
|
||||
|
||||
// expand sparse to dense and compare
|
||||
vector<mjtNum> jacp_expanded(3*nv, 0);
|
||||
vector<mjtNum> jacr_expanded(3*nv, 0);
|
||||
for (int ci = 0; ci < NV; ci++) {
|
||||
int di = chain[ci];
|
||||
for (int r = 0; r < 3; r++) {
|
||||
jacp_expanded[di+r*nv] = jacp_sparse[ci+r*NV];
|
||||
jacr_expanded[di+r*nv] = jacr_sparse[ci+r*NV];
|
||||
}
|
||||
}
|
||||
|
||||
// expect bitwise equality
|
||||
EXPECT_EQ(jacp_expanded, jacp_dense);
|
||||
EXPECT_EQ(jacr_expanded, jacr_dense);
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
}
|
||||
|
||||
using Name2idTest = MujocoTest;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user