Add constraint island discovery
PiperOrigin-RevId: 557067599 Change-Id: Ic41e1d0efef02b7a79142518afe49cf9d4e74725
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
e4dddea42a
commit
3e034e38b2
@@ -30,6 +30,9 @@ target_link_libraries(engine_derivative_test fixture gmock)
|
||||
mujoco_test(engine_forward_test)
|
||||
target_link_libraries(engine_forward_test fixture gmock)
|
||||
|
||||
mujoco_test(engine_island_test)
|
||||
target_link_libraries(engine_island_test fixture gmock)
|
||||
|
||||
mujoco_test(engine_io_test)
|
||||
target_link_libraries(
|
||||
engine_io_test
|
||||
|
||||
@@ -17,6 +17,7 @@
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
@@ -24,15 +25,12 @@
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "src/engine/engine_core_constraint.h"
|
||||
#include "src/engine/engine_support.h"
|
||||
#include "src/engine/engine_util_sparse.h"
|
||||
#include "test/fixture.h"
|
||||
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::testing::DoubleNear;
|
||||
using ::testing::ElementsAre;
|
||||
using ::testing::ElementsAreArray;
|
||||
using ::testing::Pointwise;
|
||||
using CoreConstraintTest = MujocoTest;
|
||||
|
||||
@@ -272,233 +270,5 @@ TEST_F(CoreConstraintTest, CombineSparseCount) {
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(CoreConstraintTest, EdgeToSparse4) {
|
||||
// unsorted edges, with duplication
|
||||
constexpr int ne = 6;
|
||||
constexpr int nr = 5;
|
||||
int edge[2*ne] = {
|
||||
1, 1,
|
||||
0, 0,
|
||||
0, 1,
|
||||
3, 2,
|
||||
1, 1,
|
||||
0, 0
|
||||
};
|
||||
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[ne];
|
||||
|
||||
int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
|
||||
|
||||
constexpr int expected_nnz = 4;
|
||||
|
||||
EXPECT_EQ(nnz, expected_nnz);
|
||||
|
||||
EXPECT_THAT(rownnz, ElementsAre(2, 1, 0, 1, 0));
|
||||
EXPECT_THAT(rowadr, ElementsAre(0, 2, 3, 3, 4));
|
||||
|
||||
int expected_colind[expected_nnz] = {0, 1, 1, 2};
|
||||
EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
|
||||
}
|
||||
|
||||
TEST_F(CoreConstraintTest, EdgeToSparse2) {
|
||||
// unsorted edges, with duplication
|
||||
constexpr int ne = 4;
|
||||
constexpr int nr = 5;
|
||||
int edge[2*ne] = {
|
||||
3, 4,
|
||||
1, 1,
|
||||
3, 4,
|
||||
1, 1
|
||||
};
|
||||
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[ne];
|
||||
|
||||
int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
|
||||
|
||||
constexpr int expected_nnz = 2;
|
||||
|
||||
EXPECT_EQ(nnz, expected_nnz);
|
||||
|
||||
EXPECT_THAT(rownnz, ElementsAre(0, 1, 0, 1, 0));
|
||||
EXPECT_THAT(rowadr, ElementsAre(0, 0, 1, 1, 2));
|
||||
|
||||
int expected_colind[expected_nnz] = {1, 4};
|
||||
EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
|
||||
}
|
||||
|
||||
TEST_F(CoreConstraintTest, EdgeToSparse3) {
|
||||
// unsorted edges, with duplication
|
||||
constexpr int ne = 3;
|
||||
constexpr int nr = 1;
|
||||
int edge[2*ne] = {
|
||||
0, 0,
|
||||
0, 0,
|
||||
0, 0
|
||||
};
|
||||
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[ne];
|
||||
|
||||
int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
|
||||
|
||||
constexpr int expected_nnz = 1;
|
||||
|
||||
EXPECT_EQ(nnz, expected_nnz);
|
||||
|
||||
EXPECT_THAT(rownnz, ElementsAre(1));
|
||||
EXPECT_THAT(rowadr, ElementsAre(0));
|
||||
|
||||
int expected_colind[expected_nnz] = {0};
|
||||
EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
|
||||
}
|
||||
|
||||
|
||||
TEST_F(CoreConstraintTest, FloodFillSingleton) {
|
||||
// adjacency matrix for the graph 0 1 2
|
||||
// U U
|
||||
// (3 singletons, 0 and 2 have self-edges)
|
||||
mjtNum mat[9] = {
|
||||
1, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 1
|
||||
};
|
||||
constexpr int nr = 3;
|
||||
constexpr int nnz = 2;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / scratch
|
||||
int island[nr];
|
||||
int scratch[2*nr];
|
||||
|
||||
// flood fill
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, scratch);
|
||||
|
||||
EXPECT_EQ(nisland, 2);
|
||||
EXPECT_THAT(island, ElementsAre(0, -1, 1));
|
||||
}
|
||||
|
||||
TEST_F(CoreConstraintTest, FloodFill1) {
|
||||
// adjacency matrix for the graph 0 - 1 - 2
|
||||
mjtNum mat[9] = {
|
||||
0, 1, 0,
|
||||
1, 0, 1,
|
||||
0, 1, 0
|
||||
};
|
||||
constexpr int nr = 3;
|
||||
constexpr int nnz = 4;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / stack
|
||||
int island[nr];
|
||||
int stack[nnz];
|
||||
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
|
||||
|
||||
EXPECT_EQ(nisland, 1);
|
||||
EXPECT_THAT(island, ElementsAre(0, 0, 0));
|
||||
}
|
||||
|
||||
TEST_F(CoreConstraintTest, FloodFill2) {
|
||||
// adjacency matrix for the graph 6 – 1 – 4 0 – 3 – 5 – 2
|
||||
mjtNum mat[49] = {
|
||||
0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 0, 1, 0, 1,
|
||||
0, 0, 0, 0, 0, 1, 0,
|
||||
1, 0, 0, 0, 0, 1, 0,
|
||||
0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 1, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0, 0,
|
||||
};
|
||||
constexpr int nr = 7;
|
||||
constexpr int nnz = 10;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / stack
|
||||
int island[nr];
|
||||
int stack[nnz];
|
||||
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
|
||||
|
||||
EXPECT_EQ(nisland, 2);
|
||||
EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1));
|
||||
}
|
||||
|
||||
TEST_F(CoreConstraintTest, FloodFill3a) {
|
||||
// adjacency matrix for the graph 0 2 1 – 3
|
||||
// U
|
||||
mjtNum mat[16] = {
|
||||
0, 0, 0, 0,
|
||||
0, 0, 0, 1,
|
||||
0, 0, 1, 0,
|
||||
0, 1, 0, 0,
|
||||
};
|
||||
constexpr int nr = 4;
|
||||
constexpr int nnz = 3;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / stack
|
||||
int island[nr];
|
||||
int stack[nnz];
|
||||
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
|
||||
|
||||
EXPECT_EQ(nisland, 2);
|
||||
EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0));
|
||||
}
|
||||
|
||||
TEST_F(CoreConstraintTest, FloodFill3b) {
|
||||
/*
|
||||
adjacency matrix for the graph 1 – 2 3 4 – 5
|
||||
U | \ |
|
||||
0 – 6
|
||||
*/
|
||||
mjtNum mat[49] = {
|
||||
0, 0, 0, 0, 1, 0, 1,
|
||||
0, 1, 1, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0, 0, 0, 0, 1, 1,
|
||||
0, 0, 0, 0, 1, 0, 1,
|
||||
1, 0, 0, 0, 1, 1, 0,
|
||||
};
|
||||
constexpr int nr = 7;
|
||||
constexpr int nnz = 13;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / stack
|
||||
int island[nr];
|
||||
int stack[nnz];
|
||||
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
|
||||
|
||||
EXPECT_EQ(nisland, 2);
|
||||
EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -0,0 +1,381 @@
|
||||
// Copyright 2023 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_island.c.
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "src/engine/engine_island.h"
|
||||
#include "src/engine/engine_util_sparse.h"
|
||||
#include "test/fixture.h"
|
||||
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::testing::ElementsAre;
|
||||
using ::testing::ElementsAreArray;
|
||||
using IslandTest = MujocoTest;
|
||||
|
||||
std::vector<int> AsVector(const int* array, int n) {
|
||||
return std::vector<int>(array, array + n);
|
||||
}
|
||||
|
||||
TEST_F(IslandTest, EdgeToSparse2) {
|
||||
// unsorted edges, with duplication
|
||||
constexpr int ne = 4;
|
||||
constexpr int nr = 5;
|
||||
int edge[2*ne] = {
|
||||
3, 4,
|
||||
1, 1,
|
||||
3, 4,
|
||||
1, 1
|
||||
};
|
||||
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[ne];
|
||||
|
||||
int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
|
||||
|
||||
constexpr int expected_nnz = 2;
|
||||
|
||||
EXPECT_EQ(nnz, expected_nnz);
|
||||
|
||||
EXPECT_THAT(rownnz, ElementsAre(0, 1, 0, 1, 0));
|
||||
EXPECT_THAT(rowadr, ElementsAre(0, 0, 1, 1, 2));
|
||||
|
||||
int expected_colind[expected_nnz] = {1, 4};
|
||||
EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
|
||||
}
|
||||
|
||||
TEST_F(IslandTest, EdgeToSparse3) {
|
||||
// unsorted edges, with duplication
|
||||
constexpr int ne = 3;
|
||||
constexpr int nr = 1;
|
||||
int edge[2*ne] = {
|
||||
0, 0,
|
||||
0, 0,
|
||||
0, 0
|
||||
};
|
||||
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[ne];
|
||||
|
||||
int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
|
||||
|
||||
constexpr int expected_nnz = 1;
|
||||
|
||||
EXPECT_EQ(nnz, expected_nnz);
|
||||
|
||||
EXPECT_THAT(rownnz, ElementsAre(1));
|
||||
EXPECT_THAT(rowadr, ElementsAre(0));
|
||||
|
||||
int expected_colind[expected_nnz] = {0};
|
||||
EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
|
||||
}
|
||||
|
||||
|
||||
TEST_F(IslandTest, FloodFillSingleton) {
|
||||
// adjacency matrix for the graph 0 1 2
|
||||
// U U
|
||||
// (3 singletons, 0 and 2 have self-edges)
|
||||
mjtNum mat[9] = {
|
||||
1, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 1
|
||||
};
|
||||
constexpr int nr = 3;
|
||||
constexpr int nnz = 2;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / scratch
|
||||
int island[nr];
|
||||
int scratch[2*nr];
|
||||
|
||||
// flood fill
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, scratch);
|
||||
|
||||
EXPECT_EQ(nisland, 2);
|
||||
EXPECT_THAT(island, ElementsAre(0, -1, 1));
|
||||
}
|
||||
|
||||
|
||||
TEST_F(IslandTest, FloodFill1) {
|
||||
// adjacency matrix for the graph 0 - 1 - 2
|
||||
mjtNum mat[9] = {
|
||||
0, 1, 0,
|
||||
1, 0, 1,
|
||||
0, 1, 0
|
||||
};
|
||||
constexpr int nr = 3;
|
||||
constexpr int nnz = 4;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / stack
|
||||
int island[nr];
|
||||
int stack[nnz];
|
||||
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
|
||||
|
||||
EXPECT_EQ(nisland, 1);
|
||||
EXPECT_THAT(island, ElementsAre(0, 0, 0));
|
||||
}
|
||||
|
||||
|
||||
TEST_F(IslandTest, FloodFill2) {
|
||||
// adjacency matrix for the graph 6 – 1 – 4 0 – 3 – 5 – 2
|
||||
mjtNum mat[49] = {
|
||||
0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 0, 1, 0, 1,
|
||||
0, 0, 0, 0, 0, 1, 0,
|
||||
1, 0, 0, 0, 0, 1, 0,
|
||||
0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 1, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0, 0,
|
||||
};
|
||||
constexpr int nr = 7;
|
||||
constexpr int nnz = 10;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / stack
|
||||
int island[nr];
|
||||
int stack[nnz];
|
||||
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
|
||||
|
||||
EXPECT_EQ(nisland, 2);
|
||||
EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1));
|
||||
}
|
||||
|
||||
|
||||
TEST_F(IslandTest, FloodFill3a) {
|
||||
// adjacency matrix for the graph 0 2 1 – 3
|
||||
// U
|
||||
mjtNum mat[16] = {
|
||||
0, 0, 0, 0,
|
||||
0, 0, 0, 1,
|
||||
0, 0, 1, 0,
|
||||
0, 1, 0, 0,
|
||||
};
|
||||
constexpr int nr = 4;
|
||||
constexpr int nnz = 3;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / stack
|
||||
int island[nr];
|
||||
int stack[nnz];
|
||||
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
|
||||
|
||||
EXPECT_EQ(nisland, 2);
|
||||
EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0));
|
||||
}
|
||||
|
||||
|
||||
TEST_F(IslandTest, FloodFill3b) {
|
||||
/*
|
||||
adjacency matrix for the graph 1 – 2 3 4 – 5
|
||||
U | \ |
|
||||
0 – 6
|
||||
*/
|
||||
mjtNum mat[49] = {
|
||||
0, 0, 0, 0, 1, 0, 1,
|
||||
0, 1, 1, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0, 0, 0, 0, 1, 1,
|
||||
0, 0, 0, 0, 1, 0, 1,
|
||||
1, 0, 0, 0, 1, 1, 0,
|
||||
};
|
||||
constexpr int nr = 7;
|
||||
constexpr int nnz = 13;
|
||||
int rownnz[nr];
|
||||
int rowadr[nr];
|
||||
int colind[nnz];
|
||||
mjtNum res[nnz]; // unused
|
||||
mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
|
||||
|
||||
// outputs / stack
|
||||
int island[nr];
|
||||
int stack[nnz];
|
||||
|
||||
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
|
||||
|
||||
EXPECT_EQ(nisland, 2);
|
||||
EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0));
|
||||
}
|
||||
|
||||
static const char* const kAbacusPath =
|
||||
"engine/testdata/island/abacus.xml";
|
||||
|
||||
TEST_F(IslandTest, Abacus) {
|
||||
const std::string xml_path = GetTestDataFilePath(kAbacusPath);
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||
|
||||
// disable gravity
|
||||
model->opt.disableflags |= mjDSBL_GRAVITY;
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mj_forward(model, data);
|
||||
|
||||
// no islands at qpos0
|
||||
EXPECT_EQ(data->nisland, 0);
|
||||
|
||||
// push bead 0 to the left and bead 2 to the right until there are 3 contacts
|
||||
data->qfrc_applied[0] = -1;
|
||||
data->qfrc_applied[2] = 1;
|
||||
while (data->ncon != 3) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// sizes
|
||||
int nv = model->nv;
|
||||
int nefc = data->nefc;
|
||||
int nisland = data->nisland;
|
||||
|
||||
// 4 dofs, 12 constraints, 2 islands
|
||||
EXPECT_EQ(nv, 4);
|
||||
EXPECT_EQ(nefc, 12); // 3 pyramidal contacts
|
||||
EXPECT_EQ(nisland, 2);
|
||||
|
||||
// the islands begin at dofs 0 and 2
|
||||
EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 2));
|
||||
|
||||
// dof 0 in island 0
|
||||
// dof 1 in no island
|
||||
// dofs 2,3 in island 1
|
||||
EXPECT_THAT(AsVector(data->dof_island, nv), ElementsAre(0, -1, 1, 1));
|
||||
|
||||
// dof 0 is last dof of island 0
|
||||
// dof 1 in no island
|
||||
// next dof after 2 is 3
|
||||
// dof 3 is last dof of island 1
|
||||
EXPECT_THAT(AsVector(data->dof_islandnext, nv), ElementsAre(-1, -1, 3, -1));
|
||||
|
||||
// island 0 starts at constraint 0
|
||||
// island 1 starts at constraint 4
|
||||
EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 4));
|
||||
|
||||
// first contact (4 constraints) is in island 0
|
||||
// second contact (8 constraints) is in island 1
|
||||
EXPECT_THAT(AsVector(data->efc_island, nefc),
|
||||
ElementsAre(0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1));
|
||||
|
||||
// linked list for island 0
|
||||
// linked list for island 1
|
||||
EXPECT_THAT(AsVector(data->efc_islandnext, nefc),
|
||||
ElementsAre(1, 2, 3, -1, 5, 6, 7, 8, 9, 10, 11, -1));
|
||||
|
||||
// reset, push 0 to the left, 3 to the right, 1,2 to the middle
|
||||
mj_resetData(model, data);
|
||||
data->qfrc_applied[0] = -1;
|
||||
data->qfrc_applied[1] = 1;
|
||||
data->qfrc_applied[2] = -1;
|
||||
data->qfrc_applied[3] = 1;
|
||||
|
||||
// simulate until there are 3 contacts
|
||||
while (data->ncon != 3) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// local variables
|
||||
nefc = data->nefc;
|
||||
nisland = data->nisland;
|
||||
|
||||
EXPECT_EQ(nisland, 3);
|
||||
EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 1, 3));
|
||||
EXPECT_THAT(AsVector(data->dof_island, nv), ElementsAre(0, 1, 1, 2));
|
||||
EXPECT_THAT(AsVector(data->dof_islandnext, nv), ElementsAre(-1, 2, -1, -1));
|
||||
EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 4, 8));
|
||||
EXPECT_THAT(AsVector(data->efc_island, nefc),
|
||||
ElementsAre(0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2));
|
||||
EXPECT_THAT(AsVector(data->efc_islandnext, nefc),
|
||||
ElementsAre(1, 2, 3, -1, 5, 6, 7, -1, 9, 10, 11, -1));
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
static const char* const kTendonWrapPath =
|
||||
"engine/testdata/island/tendon_wrap.xml";
|
||||
|
||||
TEST_F(IslandTest, DenseSparse) {
|
||||
const std::string xml_path = GetTestDataFilePath(kTendonWrapPath);
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||
mjData* data1 = mj_makeData(model);
|
||||
mjData* data2 = mj_makeData(model);
|
||||
|
||||
// dense
|
||||
model->opt.jacobian = mjJAC_DENSE;
|
||||
while (!data1->nefc) {
|
||||
mj_step(model, data1);
|
||||
}
|
||||
|
||||
// sparse
|
||||
model->opt.jacobian = mjJAC_SPARSE;
|
||||
while (!data2->nefc) {
|
||||
mj_step(model, data2);
|
||||
}
|
||||
|
||||
// sizes
|
||||
int nv = model->nv;
|
||||
int nefc = data1->nefc;
|
||||
int nisland = data1->nisland;
|
||||
|
||||
// expect sparse and dense to be identical
|
||||
EXPECT_EQ(data1->nefc, data2->nefc);
|
||||
EXPECT_EQ(data1->nisland, data2->nisland);
|
||||
EXPECT_EQ(data1->nefc, data2->nefc);
|
||||
EXPECT_EQ(AsVector(data1->island_dofadr, nisland),
|
||||
AsVector(data2->island_dofadr, nisland));
|
||||
EXPECT_EQ(AsVector(data1->dof_island, nv),
|
||||
AsVector(data2->dof_island, nv));
|
||||
EXPECT_EQ(AsVector(data1->dof_islandnext, nv),
|
||||
AsVector(data2->dof_islandnext, nv));
|
||||
EXPECT_EQ(AsVector(data1->island_efcadr, nisland),
|
||||
AsVector(data2->island_efcadr, nisland));
|
||||
EXPECT_EQ(AsVector(data1->efc_island, nefc),
|
||||
AsVector(data2->efc_island, nefc));
|
||||
EXPECT_EQ(AsVector(data1->efc_islandnext, nefc),
|
||||
AsVector(data2->efc_islandnext, nefc));
|
||||
|
||||
mj_deleteData(data2);
|
||||
mj_deleteData(data1);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
+35
@@ -0,0 +1,35 @@
|
||||
<mujoco>
|
||||
<option gravity="1 0 0">
|
||||
<flag island="enable"/>
|
||||
</option>
|
||||
|
||||
<default>
|
||||
<joint type="slide" axis="1 0 0" damping="10"/>
|
||||
<geom size=".1"/>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<body>
|
||||
<geom type="box" size=".05 .1 .1"/>
|
||||
</body>
|
||||
<body pos=".25 0 0">
|
||||
<joint/>
|
||||
<geom/>
|
||||
</body>
|
||||
<body pos=".5 0 0">
|
||||
<joint/>
|
||||
<geom/>
|
||||
</body>
|
||||
<body pos=".75 0 0">
|
||||
<joint/>
|
||||
<geom/>
|
||||
</body>
|
||||
<body pos="1 0 0">
|
||||
<joint/>
|
||||
<geom/>
|
||||
</body>
|
||||
<body pos="1.25 0 0">
|
||||
<geom type="box" size=".05 .1 .1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
+56
@@ -0,0 +1,56 @@
|
||||
<mujoco>
|
||||
<option>
|
||||
<flag island="enable"/>
|
||||
</option>
|
||||
|
||||
<default>
|
||||
<joint type="slide" axis="1 0 0" damping="3" frictionloss=".1"/>
|
||||
<geom type="cylinder" size=".07 .1" zaxis="0 1 0"/>
|
||||
<site size=".015" rgba="1 0 0 1"/>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<site name="anchor" pos="0 0 1"/>
|
||||
|
||||
<body pos="0 0 .75">
|
||||
<joint stiffness="10"/>
|
||||
<geom name="0"/>
|
||||
</body>
|
||||
<site name="side0" pos=".15 0 .75" rgba="0 0 1 1"/>
|
||||
|
||||
<site name="01" pos="0 0 .625"/>
|
||||
|
||||
<body pos="0 0 .5">
|
||||
<joint stiffness="30"/>
|
||||
<geom name="1"/>
|
||||
</body>
|
||||
<site name="side1" pos="-.15 0 .5" rgba="0 0 1 1"/>
|
||||
|
||||
<site name="12" pos="0 0 .375"/>
|
||||
|
||||
<body pos="0 0 .25">
|
||||
<joint stiffness="100"/>
|
||||
<geom name="2"/>
|
||||
</body>
|
||||
<site name="side2" pos=".15 0 .25" rgba="0 0 1 1"/>
|
||||
|
||||
<body>
|
||||
<freejoint/>
|
||||
<site name="box" pos="0 0 .1"/>
|
||||
<geom type="box" size=".1 .1 .1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<tendon>
|
||||
<spatial limited="true" range="0 1.05" rgba="0 1 0 1" width=".005">
|
||||
<site site="anchor"/>
|
||||
<geom geom="0" sidesite="side0"/>
|
||||
<site site="01"/>
|
||||
<geom geom="1" sidesite="side1"/>
|
||||
<site site="12"/>
|
||||
<geom geom="2" sidesite="side2"/>
|
||||
<site site="box"/>
|
||||
</spatial>
|
||||
</tendon>
|
||||
</mujoco>
|
||||
|
||||
Reference in New Issue
Block a user