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

Add CSR implementation of mj_factorI

Browse Source 
PiperOrigin-RevId: 712498431
Change-Id: I13b52e53482ed97da8788875d4d95e2beb5ca7c1
This commit is contained in:
Yuval Tassa
2025-01-06 05:47:13 -08:00
committed by Copybara-Service
parent 7eb8231fda
commit ac11e5faa6
9 changed files with 514 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+39 -7
View File
@@ -1440,10 +1440,9 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
}
}
// compute 1/diag(D), 1/sqrt(diag(D))
// compute 1/diag(D)
for (int i=0; i < nv; i++) {
mjtNum qLDi = qLD[dof_Madr[i]];
qLDiagInv[i] = 1.0/qLDi;
qLDiagInv[i] = 1.0 / qLD[dof_Madr[i]];
}
}
@@ -1458,6 +1457,40 @@ void mj_factorM(const mjModel* m, mjData* d) {
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd
// like mj_factorI, but using CSR representation
void mj_factorIs(mjtNum* mat, mjtNum* diaginv, int nv,
const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) {
// backward loop over rows
for (int k=nv-1; k >= 0; k--) {
// get row k's address, diagonal index, inverse diagonal value
int rowadr_k = rowadr[k];
int diag_k = rowadr_k + rownnz[k] - 1;
mjtNum invD = 1 / mat[diag_k];
if (diaginv) diaginv[k] = invD;
// skip if simple
if (diagnum[k]) {
continue;
}
// update triangle above row k, inclusive
for (int adr=diag_k - 1; adr >= rowadr_k; adr--) {
// tmp = L(k, i) / L(k, k)
mjtNum tmp = mat[adr] * invD;
// update row i < k: L(i, 0..i) -= L(i, 0..i) * L(k, i) / L(k, k)
int i = colind[adr];
mju_addToScl(mat + rowadr[i], mat + rowadr_k, -tmp, rownnz[i]);
// update ith element of row k: L(k, i) /= L(k, k)
mat[adr] = tmp;
}
}
}
// in-place sparse backsubstitution: x = inv(L'*D*L)*x
// L is in lower triangle of qLD; D is on diagonal of qLD
// handle n vectors at once
@@ -1575,8 +1608,7 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
// in-place sparse backsubstitution: x = inv(L'*D*L)*x
// like mj_solveLD, but using the CSR representation of L
void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv,
const int* rownnz, const int* rowadr, const int* diagind, const int* diagnum,
const int* colind) {
const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) {
// x <- L^-T x
for (int i=nv-1; i > 0; i--) {
// skip diagonal (simple) rows, exploit sparsity of input vector
@@ -1584,7 +1616,7 @@ void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv
continue;
}
int d = diagind[i];
int d = rownnz[i] - 1;
int adr_i = rowadr[i];
mjtNum x_i = x[i];
for (int j=0; j < d; j++) {
@@ -1607,7 +1639,7 @@ void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv
}
int adr = rowadr[i];
x[i] -= mju_dotSparse(qLDs+adr, x, diagind[i], colind+adr, /*flg_unc1=*/0);
x[i] -= mju_dotSparse(qLDs+adr, x, rownnz[i] - 1, colind+adr, /*flg_unc1=*/0);
}
}
src/engine/engine_core_smooth.h
+6 -2
View File
@@ -51,6 +51,11 @@ MJAPI void mj_crb(const mjModel* m, mjData* d);
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd
MJAPI void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv);
// sparse L'*D*L factorizaton of inertia-like matrix
// like mj_factorI, but using CSR representation
MJAPI void mj_factorIs(mjtNum* mat, mjtNum* diaginv, int nv,
const int* rownnz, const int* rowadr, const int* diagnum, const int* colind);
// sparse L'*D*L factorizaton of the inertia matrix M, assumed spd
MJAPI void mj_factorM(const mjModel* m, mjData* d);
@@ -61,8 +66,7 @@ MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n,
// in-place sparse backsubstitution: x = inv(L'*D*L)*x
// like mj_solveLD, but using the CSR representation of L
MJAPI void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv,
const int* rownnz, const int* rowadr, const int* diagind, const int* diagnum,
const int* colind);
const int* rownnz, const int* rowadr, const int* diagnum, const int* colind);
// sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d
MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n);
test/benchmark/CMakeLists.txt
+12
View File
@@ -43,6 +43,18 @@ mujoco_test(
ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers
)
mujoco_test(
factorI_benchmark_test
MAIN_TARGET benchmark::benchmark_main
ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers
)
mujoco_test(
inertia_benchmark_test
MAIN_TARGET benchmark::benchmark_main
ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers
)
mujoco_test(
solveLD_benchmark_test
MAIN_TARGET benchmark::benchmark_main
test/benchmark/factorI_benchmark_test.cc
+102
View File
@@ -0,0 +1,102 @@
// Copyright 2025 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// A benchmark for comparing different implementations of mj_factorI.
#include <benchmark/benchmark.h>
#include <absl/base/attributes.h>
#include <mujoco/mjdata.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_core_smooth.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
// number of steps to benchmark
static const int kNumBenchmarkSteps = 50;
// ----------------------------- benchmark ------------------------------------
static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
static mjModel* m;
if (coil) {
m = LoadModelFromPath("plugin/elasticity/coil.xml");
} else {
m = LoadModelFromPath("humanoid/humanoid100.xml");
}
mjData* d = mj_makeData(m);
mj_forward(m, d);
// allocate inputs and outputs
mj_markStack(d);
// CSR matrices
mjtNum* Ms = mj_stackAllocNum(d, m->nC);
mjtNum* LDs = mj_stackAllocNum(d, m->nC);
for (int i=0; i < m->nC; i++) {
Ms[i] = d->qM[d->mapM2C[i]];
}
// benchmark
while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
for (int i=0; i < kNumBenchmarkSteps; i++) {
if (legacy) {
mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv);
} else {
mju_copy(LDs, Ms, m->nC);
mj_factorIs(LDs, d->qLDiagInv, m->nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
}
}
}
// finalize
mj_freeStack(d);
mj_deleteData(d);
mj_deleteModel(m);
state.SetItemsProcessed(state.iterations());
}
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_factorI_COIL_LEGACY(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_factorI(state, /*legacy=*/true, /*coil=*/true);
}
BENCHMARK(BM_factorI_COIL_LEGACY);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_factorI_COIL_CSR(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_factorI(state, /*legacy=*/false, /*coil=*/true);
}
BENCHMARK(BM_factorI_COIL_CSR);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_factorI_H100_LEGACY(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_factorI(state, /*legacy=*/true, /*coil=*/false);
}
BENCHMARK(BM_factorI_H100_LEGACY);
void ABSL_ATTRIBUTE_NO_TAIL_CALL
BM_factorI_H100_CSR(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_factorI(state, /*legacy=*/false, /*coil=*/false);
}
BENCHMARK(BM_factorI_H100_CSR);
} // namespace
} // namespace mujoco
test/benchmark/inertia_benchmark_test.cc
+101
View File
@@ -0,0 +1,101 @@
// Copyright 2025 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// A benchmark for comparing legacy and two CSR implementations of inertia
// factor and then solve.
#include <benchmark/benchmark.h>
#include <absl/base/attributes.h>
#include <mujoco/mjdata.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_core_smooth.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
// number of steps to benchmark
static const int kNumBenchmarkSteps = 50;
// ----------------------------- benchmark ------------------------------------
enum class SolveType {
kLegacy = 0,
kCsr,
};
static void BM_solve(benchmark::State& state, SolveType type) {
static mjModel* m;
m = LoadModelFromPath("../test/benchmark/testdata/inertia.xml");
mjData* d = mj_makeData(m);
mj_forward(m, d);
// allocate input and output vectors
mj_markStack(d);
// make CSR matrix
mjtNum* Ms = mj_stackAllocNum(d, m->nC);
mjtNum* LDs = mj_stackAllocNum(d, m->nC);
for (int i=0; i < m->nC; i++) {
Ms[i] = d->qM[d->mapM2C[i]];
}
// arbitrary input vector
mjtNum *res = mj_stackAllocNum(d, m->nv);
mjtNum *vec = mj_stackAllocNum(d, m->nv);
for (int i=0; i < m->nv; i++) {
vec[i] = 0.2 + 0.3*i;
}
// benchmark
while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
for (int i=0; i < kNumBenchmarkSteps; i++) {
switch (type) {
case SolveType::kLegacy:
mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv);
mj_solveM(m, d, res, vec, 1);
break;
case SolveType::kCsr:
mju_copy(LDs, Ms, m->nC);
mj_factorIs(LDs, d->qLDiagInv, m->nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
mju_copy(res, vec, m->nv);
mj_solveLDs(res, LDs, d->qLDiagInv, m->nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
}
}
}
// finalize
mj_freeStack(d);
mj_deleteData(d);
mj_deleteModel(m);
state.SetItemsProcessed(state.iterations());
}
void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solve_LEGACY(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_solve(state, SolveType::kLegacy);
}
BENCHMARK(BM_solve_LEGACY);
void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solve_CSR(benchmark::State& state) {
MujocoErrorTestGuard guard;
BM_solve(state, SolveType::kCsr);
}
BENCHMARK(BM_solve_CSR);
} // namespace
} // namespace mujoco
test/benchmark/solveLD_benchmark_test.cc
+1 -2
View File
@@ -64,8 +64,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
} else {
mju_copy(res, vec, m->nv);
mj_solveLDs(res, LDs, d->qLDiagInv, m->nv,
d->C_rownnz, d->C_rowadr, d->C_diag, m->dof_simplenum,
d->C_colind);
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
}
}
}
test/benchmark/testdata/inertia.xml
Vendored
+199
View File
@@ -0,0 +1,199 @@
<mujoco model="Benchmark inertia test model">
<option timestep="0.005"/>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
</visual>
<statistic center="0 0 0.7"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<default class="body">
<!-- geoms -->
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" group="1"/>
<default class="thigh">
<geom size=".06"/>
</default>
<default class="shin">
<geom fromto="0 0 0 0 0 -.3" size=".049"/>
</default>
<default class="foot">
<geom size=".027"/>
<default class="foot1">
<geom fromto="-.07 -.01 0 .14 -.03 0"/>
</default>
<default class="foot2">
<geom fromto="-.07 .01 0 .14 .03 0"/>
</default>
</default>
<default class="arm_upper">
<geom size=".04"/>
</default>
<default class="arm_lower">
<geom size=".031"/>
</default>
<default class="hand">
<geom type="sphere" size=".04"/>
</default>
<!-- joints -->
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="joint_big">
<joint damping="5" stiffness="10"/>
<default class="hip_x">
<joint range="-30 10"/>
</default>
<default class="hip_z">
<joint range="-60 35"/>
</default>
<default class="hip_y">
<joint axis="0 1 0" range="-150 20"/>
</default>
<default class="joint_big_stiff">
<joint stiffness="20"/>
</default>
</default>
<default class="knee">
<joint pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
</default>
<default class="ankle">
<joint range="-50 50"/>
<default class="ankle_y">
<joint pos="0 0 .08" axis="0 1 0" stiffness="6"/>
</default>
<default class="ankle_x">
<joint pos="0 0 .04" stiffness="3"/>
</default>
</default>
<default class="shoulder">
<joint range="-85 60"/>
</default>
<default class="elbow">
<joint range="-100 50" stiffness="0"/>
</default>
</default>
<default class="capsule">
<geom type="capsule" size="0.1 0.05"/>
</default>
<default class="ellipsoid">
<geom type="ellipsoid" size="0.15 0.1 0.07" pos=".01 .02 .03"/>
</default>
</default>
<worldbody>
<geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>
<light name="top" pos="0 0 10" mode="trackcom"/>
<frame pos="-2 -2 2.5">
<replicate count="5" offset="0 1 0" euler="0 180 0">
<frame pos="0 0 -1.5">
<body euler="30 40 0">
<freejoint/>
<geom class="capsule"/>
</body>
</frame>
</replicate>
</frame>
<replicate count="3" offset="1.5 0 0" sep="-">
<body name="torso" pos="0 0 1.282" childclass="body">
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="root"/>
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="waist_upper" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="head" pos="0 0 .19">
<geom name="head" type="sphere" size=".09"/>
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="waist_lower" pos="-.01 0 -.26">
<geom name="waist_lower" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="joint_big_stiff"/>
<joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="joint_big"/>
<body name="pelvis" pos="0 0 -.165">
<joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="joint_big"/>
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="thigh_right" pos="0 -.1 -.04">
<joint name="hip_right" type="ball"/>
<geom name="thigh_right" fromto="0 0 0 0 .01 -.34" class="thigh"/>
<body name="shin_right" pos="0 .01 -.4">
<joint name="knee_right" class="knee"/>
<geom name="shin_right" class="shin"/>
<body name="foot_right" pos="0 0 -.39">
<joint name="ankle_y_right" class="ankle_y"/>
<joint name="ankle_x_right" class="ankle_x" axis="1 0 .5"/>
<geom name="foot1_right" class="foot1"/>
<geom name="foot2_right" class="foot2"/>
</body>
</body>
</body>
<body name="thigh_left" pos="0 .1 -.04">
<joint name="hip_x_left" axis="-1 0 0" class="hip_x"/>
<joint name="hip_z_left" axis="0 0 -1" class="hip_z"/>
<joint name="hip_y_left" class="hip_y"/>
<geom name="thigh_left" fromto="0 0 0 0 -.01 -.34" class="thigh"/>
<body name="shin_left" pos="0 -.01 -.4">
<joint name="knee_left" class="knee"/>
<geom name="shin_left" fromto="0 0 0 0 0 -.3" class="shin"/>
<body name="foot_left" pos="0 0 -.39">
<joint name="ankle_y_left" class="ankle_y"/>
<joint name="ankle_x_left" class="ankle_x" axis="-1 0 -.5"/>
<geom name="foot1_left" class="foot1"/>
<geom name="foot2_left" class="foot2"/>
</body>
</body>
</body>
</body>
</body>
<body name="upper_arm_right" pos="0 -.17 .06">
<joint name="shoulder1_right" axis="2 1 1" class="shoulder"/>
<joint name="shoulder2_right" axis="0 -1 1" class="shoulder"/>
<geom name="upper_arm_right" fromto="0 0 0 .16 -.16 -.16" class="arm_upper"/>
<body name="lower_arm_right" pos=".18 -.18 -.18">
<joint name="elbow_right" axis="0 -1 1" class="elbow"/>
<geom name="lower_arm_right" fromto=".01 .01 .01 .17 .17 .17" class="arm_lower"/>
<body name="hand_right" pos=".18 .18 .18">
<geom name="hand_right" zaxis="1 1 1" class="hand"/>
</body>
</body>
</body>
<body name="upper_arm_left" pos="0 .17 .06">
<joint name="shoulder1_left" axis="-2 1 -1" class="shoulder"/>
<joint name="shoulder2_left" axis="0 -1 -1" class="shoulder"/>
<geom name="upper_arm_left" fromto="0 0 0 .16 .16 -.16" class="arm_upper"/>
<body name="lower_arm_left" pos=".18 .18 -.18">
<joint name="elbow_left" axis="0 -1 -1" class="elbow"/>
<geom name="lower_arm_left" fromto=".01 -.01 .01 .17 -.17 .17" class="arm_lower"/>
<body name="hand_left" pos=".18 -.18 .18">
<geom name="hand_left" zaxis="1 -1 1" class="hand"/>
</body>
</body>
</body>
</body>
</replicate>
<frame pos="-1 -2 2.5">
<replicate count="5" offset="0 1 0" euler="0 180 0">
<frame pos="0 0 -1.5">
<body euler="20 40 60">
<freejoint align="false"/>
<geom class="ellipsoid"/>
</body>
</frame>
</replicate>
</frame>
</worldbody>
</mujoco>
test/engine/engine_core_smooth_test.cc
+47 -2
View File
@@ -496,8 +496,7 @@ TEST_F(CoreSmoothTest, SolveLDs) {
mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv);
mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv,
d->C_rownnz, d->C_rowadr, d->C_diag, m->dof_simplenum,
d->C_colind);
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
// expect vectors to match up to floating point precision
for (int i=0; i < nv; i++) {
@@ -508,5 +507,51 @@ TEST_F(CoreSmoothTest, SolveLDs) {
mj_deleteModel(m);
}
TEST_F(CoreSmoothTest, FactorIs) {
const std::string xml_path = GetTestDataFilePath(kInertiaPath);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
mjData* d = mj_makeData(m);
mj_forward(m, d);
int nC = m->nC, nv = m->nv;
// copy qM into LDs, qLD into qLDexpected: CSR format
vector<mjtNum> qLDsExpected(nC);
vector<mjtNum> qLDs(nC);
for (int i=0; i < nC; i++) {
int index = d->mapM2C[i];
qLDs[i] = d->qM[index]; // mj_factorIs is in-place
qLDsExpected[i] = d->qLD[index];
}
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
vector<mjtNum> qLDiagInv(nv, 0);
mj_factorIs(qLDs.data(), qLDiagInv.data(), nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
// expect outputs to match to floating point precision
EXPECT_THAT(qLDs, Pointwise(DoubleNear(1e-12), qLDsExpected));
EXPECT_THAT(qLDiagInv, Pointwise(DoubleNear(1e-12), qLDiagInvExpected));
/* uncomment for debugging
vector<mjtNum> LDdense(nv*nv);
mju_sparse2dense(LDdense.data(), qLDexpected.data(), nv, nv,
d->C_rownnz, d->C_rowadr, d->C_colind);
PrintMatrix(LDdense.data(), nv, nv, 2);
mju_sparse2dense(LDdense.data(), qLDs.data(), nv, nv,
d->C_rownnz, d->C_rowadr, d->C_colind);
PrintMatrix(LDdense.data(), nv, nv, 2);
*/
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
test/fixture.h
+7 -1
View File
@@ -118,7 +118,13 @@ inline void PrintMatrix(const mjtNum* mat, int nrow, int ncol, int p = 5) {
std::cerr << "\n";
for (int r = 0; r < nrow; r++) {
for (int c = 0; c < ncol; c++) {
std::cerr << std::fixed << std::setw(3 + p) << mat[c + r*ncol] << " ";
mjtNum val = mat[c + r*ncol];
if (val) {
std::cerr << std::fixed << std::setw(5 + p) << val << " ";
} else {
// don't print exact zeros
std::cerr << std::string(6 + p, ' ');
}
}
std::cerr << "\n";
}