mujoco

// 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) {
  char error[1024];
  mjModel* model = LoadModelFromString(kSdfModel, error, sizeof(error));
  ASSERT_THAT(model, NotNull()) << error;
  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
    {-mju_sqrt(3), 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++) {
      EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j], 1e-9);
      mjc_gradient(model, data, &sdf, gradient, points[j]);
    }
  }

  mj_deleteData(data);
  mj_deleteModel(model);
}

static constexpr char kFlexSdfModel[] = R"(
<mujoco>
  <extension>
    <plugin plugin="mujoco.sdf.torus">
      <instance name="torus">
        <config key="radius1" value="0.35"/>
        <config key="radius2" value="0.15"/>
      </instance>
    </plugin>
  </extension>

  <asset>
    <mesh name="torus">
      <plugin instance="torus"/>
    </mesh>
  </asset>

  <worldbody>
    <body pos="0 0 -0.3">
      <geom type="sdf" mesh="torus">
        <plugin instance="torus"/>
      </geom>
    </body>
    <body name="flex">
      <flexcomp name="test" type="grid" count="3 3 1" spacing=".2 .2 .2" dim="2">
        <elasticity young="1e4"/>
        <contact selfcollide="none" internal="false"/>
      </flexcomp>
    </body>
  </worldbody>
</mujoco>
)";

TEST_F(SdfTest, FlexSdfCollision) {
  char error[1024];
  mjModel* model = LoadModelFromString(kFlexSdfModel, error, sizeof(error));
  ASSERT_THAT(model, NotNull()) << error;
  mjData* data = mj_makeData(model);
  ASSERT_THAT(data, NotNull());

  // check there is at least one flex
  ASSERT_GT(model->nflex, 0);

  // simulate for a few steps to let flex fall onto torus
  for (int i = 0; i < 100; i++) {
    mj_step(model, data);
  }

  // should have contacts between flex and SDF
  EXPECT_GT(data->ncon, 0) << "Expected flex-SDF contacts";

  // check that contacts involve the flex
  bool has_flex_contact = false;
  for (int i = 0; i < data->ncon; i++) {
    if (data->contact[i].flex[0] >= 0 || data->contact[i].flex[1] >= 0) {
      has_flex_contact = true;
      break;
    }
  }
  EXPECT_TRUE(has_flex_contact) << "Expected at least one flex contact";

  mj_deleteData(data);
  mj_deleteModel(model);
}

}  // namespace
}  // namespace mujoco