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

Add Signed Distance Field to collision geometries.

Browse Source 
PiperOrigin-RevId: 557507088
Change-Id: I358a642407aee1ba8dfc9d405eb9a4f609435fe9
This commit is contained in:
Alessio Quaglino
2023-08-16 09:14:29 -07:00
committed by Copybara-Service
parent 6245edae28
commit fdb041580c
57 changed files with 3229 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/CMakeLists.txt
+6 -1
View File
@@ -41,7 +41,12 @@ target_link_libraries(
absl::str_format
)
mujoco_test(engine_plugin_test)
mujoco_test(
engine_plugin_test
PROPERTIES
ENVIRONMENT
"MUJOCO_PLUGIN_DIR=$<TARGET_FILE_DIR:elasticity>"
)
target_link_libraries(
engine_plugin_test
fixture
test/engine/engine_collision_sdf_test.cc
+84
View File
@@ -0,0 +1,84 @@
// Copyright 2022 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 sdf collisions.
#include <cstdio>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_collision_sdf.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::NotNull;
using SdfTest = MujocoTest;
static constexpr int kpoints = 6;
static constexpr int kgeoms = 5;
static constexpr char kSdfModel[] = R"(
<mujoco>
<worldbody>
<geom type="plane" size="5 5 .1" pos="0 0 -1"/>
<body pos="-.1 .2 2" euler="0 0 45">
<geom type="sphere" size="1"/>
<geom type="capsule" size=".1" fromto="-2 -2 1 2 2 1"/>
<geom type="cylinder" size="1" fromto="0 0 0 2 0 0"/>
<geom type="box" size="1 1 1"/>
</body>
</worldbody>
</mujoco>
)";
TEST_F(SdfTest, SdfPrimitive) {
mjModel* model = LoadModelFromString(kSdfModel);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
ASSERT_THAT(data, NotNull());
ASSERT_THAT(model->ngeom, kgeoms);
mjSDF sdf;
const mjpPlugin* null_plugin = NULL;
mjtNum gradient[3], dist[kgeoms][kpoints] = {
{0, 0, 0, 0, 1, 1}, // plane
{-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere
{-.1, .9, .9, mju_sqrt(2)-.1, .9, mju_sqrt(2)-.1}, // capsule
{-1, 0, 0, mju_sqrt(2)-1, 0, mju_sqrt(2)-1}, // cylinder
{-1, 0, 0, 0, 0, 0}, // box
};
mjtNum points[kpoints][3] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0},
{1, 1, 0}, {0, 1, 1}, {1, 1, 1}};
for (int i = 0; i < kgeoms; i++) {
sdf.plugin = &null_plugin;
sdf.id = &i;
sdf.type = mjSDFTYPE_SINGLE;
sdf.geomtype = (mjtGeom*)(model->geom_type+i);
for (int j = 0; j < kpoints; j++) {
ASSERT_THAT(mjc_distance(model, data, &sdf, points[j]), dist[i][j]);
mjc_gradient(model, data, &sdf, gradient, points[j]);
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_plugin_test.cc
+12 -8
View File
@@ -32,9 +32,9 @@ namespace {
using ::testing::HasSubstr;
using ::testing::NotNull;
constexpr int kNumTruePlugins = 8;
constexpr int kNumFakePlugins = 30;
constexpr int kNumTestPlugins = 3;
const int kNumTruePlugins = mjp_pluginCount();
class BaseTestPlugin {
public:
@@ -319,10 +319,10 @@ int RegisterPassivePlugin() {
return mjp_registerPlugin(&plugin);
}
class PluginTest : public MujocoTest {
class EnginePluginTest : public PluginTest {
public:
// register all plugins
PluginTest() : MujocoTest() {
EnginePluginTest() : PluginTest() {
RegisterSensorPlugin();
for (int i = 1; i <= kNumFakePlugins; ++i) {
@@ -388,7 +388,11 @@ constexpr char xml[] = R"(
</mujoco>
)";
TEST_F(PluginTest, MultiplePluginTableBlocks) {
TEST_F(PluginTest, FirstPartyPlugins) {
EXPECT_THAT(mjp_pluginCount(), kNumTruePlugins);
}
TEST_F(EnginePluginTest, MultiplePluginTableBlocks) {
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
const mjpPlugin* last_plugin = nullptr;
@@ -412,14 +416,14 @@ TEST_F(PluginTest, MultiplePluginTableBlocks) {
EXPECT_LT(table_count, kNumFakePlugins);
}
TEST_F(PluginTest, RegisterIdenticalPlugin) {
TEST_F(EnginePluginTest, RegisterIdenticalPlugin) {
EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins);
EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1);
EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2);
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
}
TEST_F(PluginTest, SaveXml) {
TEST_F(EnginePluginTest, SaveXml) {
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
@@ -471,7 +475,7 @@ TEST_F(PluginTest, SaveXml) {
mj_deleteModel(m2);
}
TEST_F(PluginTest, SensorPlugin) {
TEST_F(EnginePluginTest, SensorPlugin) {
int expected_init_count = TestSensor::InitCount();
int expected_destroy_count = TestSensor::DestroyCount();
EXPECT_EQ(expected_init_count, expected_destroy_count);
@@ -529,7 +533,7 @@ TEST_F(PluginTest, SensorPlugin) {
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
}
TEST_F(PluginTest, ActuatorPlugin) {
TEST_F(EnginePluginTest, ActuatorPlugin) {
int expected_init_count = TestActuator::InitCount();
int expected_destroy_count = TestActuator::DestroyCount();
EXPECT_EQ(expected_init_count, expected_destroy_count);