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

Derivatives of ellipsoid fluid model.

Browse Source 
PiperOrigin-RevId: 459751881
Change-Id: I52db87d82e20e10698b13065df2e983b9841bd7a
This commit is contained in:
Yuval Tassa
2022-07-08 07:19:10 -07:00
committed by Copybara-Service
parent 7034851d0a
commit ea956dfe34
10 changed files with 784 additions and 309 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_derivative_test.cc
+44 -35
View File
@@ -89,6 +89,8 @@ static const char* const kEnergyConservingPendulumPath =
"engine/testdata/derivative/energy_conserving_pendulum.xml";
static const char* const kTumblingThinObjectPath =
"engine/testdata/derivative/tumbling_thin_object.xml";
static const char* const kTumblingThinObjectEllipsoidPath =
"engine/testdata/derivative/tumbling_thin_object_ellipsoid.xml";
static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
static const char* const kDamperActuatorsPath =
@@ -150,42 +152,47 @@ TEST_F(DerivativeTest, SmoothDvel) {
// compare analytic and fin-diff d_qfrc_passive/d_qvel
TEST_F(DerivativeTest, PassiveDvel) {
const std::string xml_path = GetTestDataFilePath(kTumblingThinObjectPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
int nv = model->nv;
mjData* data = mj_makeData(model);
// allocate d_qfrc_passive/d_qvel Jacobians
mjtNum* DfDv_analytic = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv);
mjtNum* DfDv_FD = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv);
for (const char* local_path : {kTumblingThinObjectPath,
kTumblingThinObjectEllipsoidPath}) {
// load model
const std::string xml_path = GetTestDataFilePath(local_path);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
int nv = model->nv;
mjData* data = mj_makeData(model);
// allocate Jacobians
mjtNum* DfDv_analytic = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv);
mjtNum* DfDv_FD = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv);
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
// set sparsity
model->opt.jacobian = sparsity;
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
// set sparsity
model->opt.jacobian = sparsity;
// take 100 steps so we have some velocities, then call forward
mj_resetData(model, data);
for (int i=0; i < 100; i++) {
mj_step(model, data);
// take 100 steps so we have some velocities, then call forward
mj_resetData(model, data);
for (int i=0; i < 100; i++) {
mj_step(model, data);
}
mj_forward(model, data);
// clear DfDv, get analytic derivatives
mju_zero(DfDv_analytic, nv*nv);
mjd_passive_vel(model, data, DfDv_analytic);
// clear DfDv, get finite-difference derivatives
mju_zero(DfDv_FD, nv*nv);
mjtNum eps = 1e-6;
mjd_passive_velFD(model, data, eps, DfDv_FD);
// expect FD and analytic derivatives to be similar to tol precision
mjtNum tol = 1e-4;
CompareMatrices(DfDv_analytic, DfDv_FD, nv, nv, tol);
}
mj_forward(model, data);
// clear DfDv, get analytic derivatives
mju_zero(DfDv_analytic, nv*nv);
mjd_passive_vel(model, data, DfDv_analytic);
// clear DfDv, get finite-difference derivatives
mju_zero(DfDv_FD, nv*nv);
mjtNum eps = 1e-6;
mjd_passive_velFD(model, data, eps, DfDv_FD);
// expect FD and analytic derivatives to be similar to eps precision
CompareMatrices(DfDv_analytic, DfDv_FD, nv, nv, eps);
mju_free(DfDv_FD);
mju_free(DfDv_analytic);
mj_deleteData(data);
mj_deleteModel(model);
}
mju_free(DfDv_FD);
mju_free(DfDv_analytic);
mj_deleteData(data);
mj_deleteModel(model);
}
// ----------------------- derivatives of mj_step() ----------------------------
@@ -336,8 +343,9 @@ TEST_F(DerivativeTest, LinearSystem) {
LinearSystem(model, data, A, B);
PrintMatrix(A, 2*nv, 2*nv);
PrintMatrix(B, 2*nv, nu);
// uncomment for debugging:
// PrintMatrix(A, 2*nv, 2*nv);
// PrintMatrix(B, 2*nv, nu);
// forward differenced A and B
mjtNum eps = 1e-6;
@@ -346,8 +354,9 @@ TEST_F(DerivativeTest, LinearSystem) {
mjd_transitionFD(model, data, eps, /*centered=*/0, AFD, BFD);
PrintMatrix(AFD, 2*nv, 2*nv);
PrintMatrix(BFD, 2*nv, nu);
// uncomment for debugging:
// PrintMatrix(AFD, 2*nv, 2*nv);
// PrintMatrix(BFD, 2*nv, nu);
// expect FD and analytic derivatives to be similar to eps precision
CompareMatrices(A, AFD, 2*nv, 2*nv, eps);
test/engine/testdata/derivative/tumbling_thin_object_ellipsoid.xml
Vendored
+13
View File
@@ -0,0 +1,13 @@
<mujoco>
<option density="1.225" viscosity="1.8e-5" wind="0 0 1" integrator="implicit"/>
<worldbody>
<light pos="0 0 1"/>
<geom type="plane" size="1 1 .01" pos="0 0 -1"/>
<body>
<freejoint/>
<geom type="box" size=".025 .01 0.0001" pos=".025 0 0" euler="20 0 0" mass="1e-4" fluidshape="ellipsoid"/>
<geom type="box" size=".025 .01 0.0001" pos="-.025 0 0" euler="-19 0 0" mass="1e-4" fluidshape="ellipsoid"/>
</body>
</worldbody>
</mujoco>