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

Break out passive force functions into a new file engine_passive.c

Browse Source 
PiperOrigin-RevId: 513834952
Change-Id: I5347440f24d767a7dc81a34e48e1e17d8c10173f
This commit is contained in:
Yuval Tassa
2023-03-03 08:31:35 -08:00
committed by Copybara-Service
parent fd1070cf03
commit 222ee00a66
13 changed files with 832 additions and 723 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/CMakeLists.txt
+3
View File
@@ -46,6 +46,9 @@ target_link_libraries(
absl::str_format
)
mujoco_test(engine_passive_test)
target_link_libraries(engine_passive_test fixture gmock)
mujoco_test(engine_print_test)
target_link_libraries(engine_print_test fixture gmock)
test/engine/engine_core_smooth_test.cc
-196
View File
@@ -243,201 +243,5 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
mj_deleteModel(model);
}
// ------------------------ ellipsoid fluid model ------------------------------
using EllipsoidFluidTest = MujocoTest;
TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
static constexpr char two_bodies_xml[] = R"(
<mujoco>
<option wind="5 5 0" density="10"/>
<worldbody>
<body>
<freejoint/>
<body>
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
</body>
<body>
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
</body>
</body>
</worldbody>
</mujoco>
)";
mjModel* m2 = LoadModelFromString(two_bodies_xml);
mjData* d2 = mj_makeData(m2);
for (int i = 0; i < 6; i++) {
d2->qvel[i] = (mjtNum) i+1;
}
d2->qpos[3] = 0.5;
d2->qpos[4] = 0.5;
d2->qpos[5] = 0.5;
d2->qpos[6] = 0.5;
static constexpr char one_body_xml[] = R"(
<mujoco>
<option wind="5 5 0" density="10"/>
<worldbody>
<body pos="1 2 3">
<freejoint/>
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* m1 = LoadModelFromString(one_body_xml);
mjData* d1 = mj_makeData(m1);
for (int i = 0; i < 6; i++) {
d1->qvel[i] = (mjtNum) i+1;
}
d1->qpos[3] = 0.5;
d1->qpos[4] = 0.5;
d1->qpos[5] = 0.5;
d1->qpos[6] = 0.5;
const mjtNum tol = 1e-14; // tolerance for floating point numbers
EXPECT_EQ(m1->nv, m2->nv);
mj_forward(m2, d2);
mj_forward(m1, d1);
for (int i = 0; i < m1->nv; i++) {
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
}
mj_deleteData(d1);
mj_deleteModel(m1);
mj_deleteData(d2);
mj_deleteModel(m2);
}
TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
static constexpr char xml[] = R"(
<mujoco>
<option wind="5 5 0" density="10"/>
<default>
<geom fluidshape="ellipsoid" fluidcoef="2 3 4 5 6"/>
<default class="test_class">
<geom fluidshape="none" fluidcoef="5 4 3 2 1"/>
</default>
</default>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" class="test_class"/>
<geom type="box" size=".1 .01 0.01" pos="-0.1 0 0"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
EXPECT_THAT(GetVector(model->geom_fluid, 6),
ElementsAre(0, 0, 0, 0, 0, 0));
EXPECT_THAT(GetVector(model->geom_fluid + mjNFLUID, 6),
ElementsAre(1, 2, 3, 4, 5, 6));
mj_deleteModel(model);
}
// -------------------------- adhesion actuators -------------------------------
using AdhesionTest = MujocoTest;
TEST_F(AdhesionTest, ExpectedAdhesionForce) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -1"/>
<worldbody>
<body name="static">
<!-- small increase to size to ensure contact -->
<geom size=".02001" pos=" .01 .01 .07"/>
<geom size=".02001" pos="-.01 .01 .07"/>
<geom size=".02001" pos=" .01 -.01 .07"/>
<geom size=".02001" pos="-.01 -.01 .07"/>
</body>
<body name="free">
<freejoint/>
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<adhesion body="static" ctrlrange="0 2"/>
<adhesion body="free" ctrlrange="0 2"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
// iterate over cone type
for (mjtCone cone : {mjCONE_ELLIPTIC, mjCONE_PYRAMIDAL}) {
// set cone
model->opt.cone = cone;
// iterate over condim
for (int condim : {1, 3, 4, 6}) {
// set condim
for (int id=0; id < model->ngeom; id++) {
model->geom_condim[id] = condim;
}
// iterate over actuators
for (int id=0; id < 2; id++) {
// set ctrl > 1, expect free body to not fall
mj_resetData(model, data);
data->ctrl[id] = 1.01;
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
// moved down at most 10 microns
EXPECT_GT(data->qpos[2], -1e-5);
// set ctrl < 1, expect free body to fall below 1cm
mj_resetData(model, data);
data->ctrl[id] = 0.99;
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
// fell lower than 1cm
EXPECT_LT(data->qpos[2], -0.01);
}
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
// ------------------------------ tendons --------------------------------------
using TendonTest = MujocoTest;
// check tendon spring deadband using example model
TEST_F(TendonTest, SpringrangeDeadband) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/tendon_springlength.xml");
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
// initial state outside deadband: spring is active
mj_forward(model, data);
mjtNum expected_force = model->tendon_stiffness[0] *
(model->tendon_lengthspring[1] - data->ten_length[0]);
EXPECT_EQ(expected_force, data->qfrc_passive[0]);
// put body inside deadband: spring is inactive
data->qpos[0] = -1;
mj_forward(model, data);
EXPECT_EQ(0, data->qfrc_passive[0]);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_forward_test.cc
+69
View File
@@ -492,5 +492,74 @@ TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) {
mj_deleteData(data);
mj_deleteModel(model);
}
// -------------------------- adhesion actuators -------------------------------
using AdhesionTest = MujocoTest;
TEST_F(AdhesionTest, ExpectedAdhesionForce) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -1"/>
<worldbody>
<body name="static">
<!-- small increase to size to ensure contact -->
<geom size=".02001" pos=" .01 .01 .07"/>
<geom size=".02001" pos="-.01 .01 .07"/>
<geom size=".02001" pos=" .01 -.01 .07"/>
<geom size=".02001" pos="-.01 -.01 .07"/>
</body>
<body name="free">
<freejoint/>
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<adhesion body="static" ctrlrange="0 2"/>
<adhesion body="free" ctrlrange="0 2"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
// iterate over cone type
for (mjtCone cone : {mjCONE_ELLIPTIC, mjCONE_PYRAMIDAL}) {
// set cone
model->opt.cone = cone;
// iterate over condim
for (int condim : {1, 3, 4, 6}) {
// set condim
for (int id=0; id < model->ngeom; id++) {
model->geom_condim[id] = condim;
}
// iterate over actuators
for (int id=0; id < 2; id++) {
// set ctrl > 1, expect free body to not fall
mj_resetData(model, data);
data->ctrl[id] = 1.01;
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
// moved down at most 10 microns
EXPECT_GT(data->qpos[2], -1e-5);
// set ctrl < 1, expect free body to fall below 1cm
mj_resetData(model, data);
data->ctrl[id] = 0.99;
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
// fell lower than 1cm
EXPECT_LT(data->qpos[2], -0.01);
}
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_passive_test.cc
+166
View File
@@ -0,0 +1,166 @@
// Copyright 2021 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.
// Tests for engine/engine_core_smooth.c.
#include <cstddef>
#include <string>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::ElementsAre;
using ::testing::NotNull;
using CoreSmoothTest = MujocoTest;
static std::vector<mjtNum> GetVector(const mjtNum* array, int length) {
return std::vector<mjtNum>(array, array + length);
}
// ------------------------ ellipsoid fluid model ------------------------------
using EllipsoidFluidTest = MujocoTest;
TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
static constexpr char two_bodies_xml[] = R"(
<mujoco>
<option wind="5 5 0" density="10"/>
<worldbody>
<body>
<freejoint/>
<body>
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
</body>
<body>
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
</body>
</body>
</worldbody>
</mujoco>
)";
mjModel* m2 = LoadModelFromString(two_bodies_xml);
mjData* d2 = mj_makeData(m2);
for (int i = 0; i < 6; i++) {
d2->qvel[i] = (mjtNum) i+1;
}
d2->qpos[3] = 0.5;
d2->qpos[4] = 0.5;
d2->qpos[5] = 0.5;
d2->qpos[6] = 0.5;
static constexpr char one_body_xml[] = R"(
<mujoco>
<option wind="5 5 0" density="10"/>
<worldbody>
<body pos="1 2 3">
<freejoint/>
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* m1 = LoadModelFromString(one_body_xml);
mjData* d1 = mj_makeData(m1);
for (int i = 0; i < 6; i++) {
d1->qvel[i] = (mjtNum) i+1;
}
d1->qpos[3] = 0.5;
d1->qpos[4] = 0.5;
d1->qpos[5] = 0.5;
d1->qpos[6] = 0.5;
const mjtNum tol = 1e-14; // tolerance for floating point numbers
EXPECT_EQ(m1->nv, m2->nv);
mj_forward(m2, d2);
mj_forward(m1, d1);
for (int i = 0; i < m1->nv; i++) {
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
}
mj_deleteData(d1);
mj_deleteModel(m1);
mj_deleteData(d2);
mj_deleteModel(m2);
}
TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
static constexpr char xml[] = R"(
<mujoco>
<option wind="5 5 0" density="10"/>
<default>
<geom fluidshape="ellipsoid" fluidcoef="2 3 4 5 6"/>
<default class="test_class">
<geom fluidshape="none" fluidcoef="5 4 3 2 1"/>
</default>
</default>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" class="test_class"/>
<geom type="box" size=".1 .01 0.01" pos="-0.1 0 0"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
EXPECT_THAT(GetVector(model->geom_fluid, 6),
ElementsAre(0, 0, 0, 0, 0, 0));
EXPECT_THAT(GetVector(model->geom_fluid + mjNFLUID, 6),
ElementsAre(1, 2, 3, 4, 5, 6));
mj_deleteModel(model);
}
// ------------------------------ tendons --------------------------------------
using TendonTest = MujocoTest;
// check tendon spring deadband using example model
TEST_F(TendonTest, SpringrangeDeadband) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/tendon_springlength.xml");
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
// initial state outside deadband: spring is active
mj_forward(model, data);
mjtNum expected_force = model->tendon_stiffness[0] *
(model->tendon_lengthspring[1] - data->ten_length[0]);
EXPECT_EQ(expected_force, data->qfrc_passive[0]);
// put body inside deadband: spring is inactive
data->qpos[0] = -1;
mj_forward(model, data);
EXPECT_EQ(0, data->qfrc_passive[0]);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco