Add contact sensor.
PiperOrigin-RevId: 783011982 Change-Id: Ica56fe9d520fa1d1ee7338e09148b1a55a049912
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
e441868dad
commit
d0e4771c8c
@@ -14,6 +14,8 @@
|
||||
|
||||
// Tests for engine/engine_sensor.c.
|
||||
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
@@ -28,21 +30,39 @@
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::std::string;
|
||||
using ::std::vector;
|
||||
|
||||
using ::testing::DoubleNear;
|
||||
using ::testing::ElementsAre;
|
||||
using ::testing::ElementsAreArray;
|
||||
using ::testing::HasSubstr;
|
||||
using ::testing::IsNull;
|
||||
using ::testing::Not;
|
||||
using ::testing::NotNull;
|
||||
using ::testing::Pointwise;
|
||||
using ::testing::SizeIs;
|
||||
using ::testing::StrEq;
|
||||
using ::testing::WhenSorted;
|
||||
|
||||
const mjtNum tol = 1e-14; // nearness tolerance for floating point numbers
|
||||
|
||||
// returns as a vector the measured values from sensor with index `id`
|
||||
static std::vector<mjtNum> GetSensor(const mjModel* model,
|
||||
const mjData* data,
|
||||
int id) {
|
||||
return std::vector<mjtNum>(
|
||||
static vector<mjtNum> GetSensor(const mjModel* model,
|
||||
const mjData* data, int id) {
|
||||
return vector<mjtNum>(
|
||||
data->sensordata + model->sensor_adr[id],
|
||||
data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]);
|
||||
}
|
||||
|
||||
using ::testing::Pointwise;
|
||||
using ::testing::DoubleNear;
|
||||
using ::testing::NotNull;
|
||||
using ::testing::StrEq;
|
||||
// returns as a vector the measured values from sensor with name `name
|
||||
static vector<mjtNum> GetSensor(const mjModel* model,
|
||||
const mjData* data, const char* name) {
|
||||
int id = mj_name2id(model, mjOBJ_SENSOR, name);
|
||||
return vector<mjtNum>(
|
||||
data->sensordata + model->sensor_adr[id],
|
||||
data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]);
|
||||
}
|
||||
|
||||
using SensorTest = MujocoTest;
|
||||
|
||||
@@ -112,13 +132,13 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMat) {
|
||||
mj_forward(model, data);
|
||||
|
||||
// compare actual and expected values
|
||||
std::vector pos = GetSensor(model, data, 0);
|
||||
vector pos = GetSensor(model, data, 0);
|
||||
EXPECT_THAT(pos, Pointwise(DoubleNear(tol), {5, 5, 0}));
|
||||
|
||||
std::vector xaxis = GetSensor(model, data, 1);
|
||||
vector xaxis = GetSensor(model, data, 1);
|
||||
EXPECT_THAT(xaxis, Pointwise(DoubleNear(tol), {0, -1, 0}));
|
||||
|
||||
std::vector yaxis = GetSensor(model, data, 2);
|
||||
vector yaxis = GetSensor(model, data, 2);
|
||||
EXPECT_THAT(yaxis, Pointwise(DoubleNear(tol), {1, 0, 0}));
|
||||
|
||||
mj_deleteData(data);
|
||||
@@ -155,7 +175,7 @@ TEST_F(RelativeFrameSensorTest, ReferenceQuatMat) {
|
||||
mju_mat2Quat(converted_quat, mat);
|
||||
|
||||
// compare quaternion sensor and quat derived from orientation matrix
|
||||
std::vector quat = GetSensor(model, data, 3);
|
||||
vector quat = GetSensor(model, data, 3);
|
||||
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), converted_quat));
|
||||
|
||||
mj_deleteData(data);
|
||||
@@ -199,7 +219,7 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) {
|
||||
|
||||
// call mj_forward, save global sensors (colocated with reference frame)
|
||||
mj_forward(model, data);
|
||||
std::vector expected_values(data->sensordata, data->sensordata+nsensordata/2);
|
||||
vector expected_values(data->sensordata, data->sensordata+nsensordata/2);
|
||||
|
||||
// set qpos to arbitrary values, call mj_forward
|
||||
for (int i=0; i < 7; i++) {
|
||||
@@ -208,7 +228,7 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) {
|
||||
mj_forward(model, data);
|
||||
|
||||
// get values from relative sensors after moving the object
|
||||
std::vector actual_values(data->sensordata+nsensordata/2,
|
||||
vector actual_values(data->sensordata+nsensordata/2,
|
||||
data->sensordata+nsensordata);
|
||||
|
||||
// object and reference have moved together, we expect values to not change
|
||||
@@ -245,7 +265,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) {
|
||||
mj_forward(model, data);
|
||||
|
||||
// compare to expected values
|
||||
std::vector linvel = GetSensor(model, data, 0);
|
||||
vector linvel = GetSensor(model, data, 0);
|
||||
const mjtNum expected_linvel[3] = {-mju_sqrt(0.5), mju_sqrt(0.5), 0};
|
||||
EXPECT_THAT(linvel, Pointwise(DoubleNear(tol), expected_linvel));
|
||||
|
||||
@@ -278,7 +298,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) {
|
||||
mj_forward(model, data);
|
||||
|
||||
// obj and ref rotate together, relative angular velocities should be zero
|
||||
std::vector angvel = GetSensor(model, data, 0);
|
||||
vector angvel = GetSensor(model, data, 0);
|
||||
EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), {0, 0, 0}));
|
||||
|
||||
mj_deleteData(data);
|
||||
@@ -314,7 +334,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngOpposing) {
|
||||
mj_forward(model, data);
|
||||
|
||||
// obj and ref rotate on same axis, we can just difference the velocities
|
||||
std::vector angvel = GetSensor(model, data, 0);
|
||||
vector angvel = GetSensor(model, data, 0);
|
||||
const mjtNum expected_angvel[3] = {0, data->qvel[1]-data->qvel[0], 0};
|
||||
EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), expected_angvel));
|
||||
|
||||
@@ -358,8 +378,8 @@ TEST_F(RelativeFrameSensorTest, FrameVelGeneral) {
|
||||
mj_forward(model, data);
|
||||
|
||||
// save measured linear and angular velocities as vectors
|
||||
std::vector linvel = GetSensor(model, data, 2);
|
||||
std::vector angvel = GetSensor(model, data, 3);
|
||||
vector linvel = GetSensor(model, data, 2);
|
||||
vector angvel = GetSensor(model, data, 3);
|
||||
|
||||
// save current position, quaternion as arrays
|
||||
mjtNum pos0[3], quat0[4];
|
||||
@@ -615,20 +635,20 @@ TEST_F(SensorTest, CollisionSequential) {
|
||||
mjtNum eps = 1e-14;
|
||||
|
||||
EXPECT_THAT(GetSensor(model, data, 3),
|
||||
Pointwise(DoubleNear(eps), std::vector<mjtNum>{0, 0, 1}));
|
||||
Pointwise(DoubleNear(eps), vector<mjtNum>{0, 0, 1}));
|
||||
EXPECT_THAT(GetSensor(model, data, 4),
|
||||
Pointwise(DoubleNear(eps), std::vector<mjtNum>{0, 0, -1}));
|
||||
Pointwise(DoubleNear(eps), vector<mjtNum>{0, 0, -1}));
|
||||
EXPECT_THAT(GetSensor(model, data, 5),
|
||||
Pointwise(DoubleNear(eps), std::vector<mjtNum>{1, 0, 0}));
|
||||
Pointwise(DoubleNear(eps), vector<mjtNum>{1, 0, 0}));
|
||||
EXPECT_THAT(GetSensor(model, data, 6),
|
||||
Pointwise(DoubleNear(eps),
|
||||
std::vector<mjtNum>{0, 0, 0, 0, 0, .8}));
|
||||
vector<mjtNum>{0, 0, 0, 0, 0, .8}));
|
||||
EXPECT_THAT(GetSensor(model, data, 7),
|
||||
Pointwise(DoubleNear(eps),
|
||||
std::vector<mjtNum>{1, 0, .7, 1, 0, 0}));
|
||||
vector<mjtNum>{1, 0, .7, 1, 0, 0}));
|
||||
EXPECT_THAT(GetSensor(model, data, 8),
|
||||
Pointwise(DoubleNear(eps),
|
||||
std::vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
|
||||
vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
|
||||
|
||||
EXPECT_THAT(GetSensor(model, data, 9),
|
||||
Pointwise(DoubleNear(eps), GetSensor(model, data, 0)));
|
||||
@@ -647,6 +667,202 @@ TEST_F(SensorTest, CollisionSequential) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(SensorTest, BadContact) {
|
||||
string xml_template = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<geom name="sphere1" pos="0 0 1" size="0.2"/>
|
||||
<body name="body">
|
||||
<freejoint/>
|
||||
<geom name="sphere2" pos="1 0 1" size="0.3"/>
|
||||
<site name="site" pos="1 0 1" size="0.3"/>
|
||||
<body name="non_root">
|
||||
<geom name="sphere3" pos="1 0 1" size="0.3"/>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
<sensor>
|
||||
<contact BAD_ATTR/>
|
||||
</sensor>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
struct Case {
|
||||
string bad_attr;
|
||||
string expected_error;
|
||||
};
|
||||
|
||||
Case test_cases[] = {
|
||||
{"geom1='sphere1' geom2='sphere2' data='dist force normal'",
|
||||
"must be in order: found, force, torque, dist, pos, normal, tangent"},
|
||||
{"geom1='sphere1' geom2='sphere2' num='-3'",
|
||||
"'num' must be positive in sensor"},
|
||||
{"geom1='sphere1' geom2='sphere2' site='site'",
|
||||
"at most one of (geom1, body1, subtree1, site) can be specified"},
|
||||
{"geom2='sphere1' body2='body'",
|
||||
"at most one of (geom2, body2, subtree2) can be specified"},
|
||||
{"subtree1='non_root'",
|
||||
"must be a child of the world"}
|
||||
};
|
||||
|
||||
for (const auto& test : test_cases) {
|
||||
string xml = xml_template;
|
||||
size_t pos = xml.find("BAD_ATTR");
|
||||
ASSERT_NE(pos, string::npos);
|
||||
xml.replace(pos, 8, test.bad_attr);
|
||||
|
||||
char error[1024];
|
||||
mjModel* model = LoadModelFromString(xml.c_str(), error, sizeof(error));
|
||||
ASSERT_THAT(model, IsNull()) << "Test case: " << test.bad_attr;
|
||||
EXPECT_THAT(error, HasSubstr(test.expected_error))
|
||||
<< "Test case: " << test.bad_attr;
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(SensorTest, Contact) {
|
||||
const string xml_path =
|
||||
GetTestDataFilePath("engine/testdata/sensor/contact.xml");
|
||||
char error[1024];
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
for (mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) {
|
||||
model->opt.cone = cone;
|
||||
|
||||
mj_resetData(model, data);
|
||||
while (data->time < 2) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
vector all = GetSensor(model, data, "all");
|
||||
EXPECT_EQ(all, vector<mjtNum>{4});
|
||||
|
||||
vector world = GetSensor(model, data, "world");
|
||||
EXPECT_EQ(world, vector<mjtNum>{3});
|
||||
|
||||
vector b1 = GetSensor(model, data, "b1");
|
||||
EXPECT_EQ(b1, vector<mjtNum>{3});
|
||||
|
||||
vector g1 = GetSensor(model, data, "g1");
|
||||
EXPECT_EQ(g1, vector<mjtNum>{3});
|
||||
|
||||
vector b1g2 = GetSensor(model, data, "b1:g2");
|
||||
EXPECT_EQ(b1g2, vector<mjtNum>{1});
|
||||
|
||||
vector b1world = GetSensor(model, data, "b1:world");
|
||||
EXPECT_EQ(b1world, vector<mjtNum>{2});
|
||||
|
||||
vector site = GetSensor(model, data, "site");
|
||||
EXPECT_EQ(site, vector<mjtNum>{2});
|
||||
|
||||
vector sitewall = GetSensor(model, data, "site:wall");
|
||||
EXPECT_EQ(sitewall, vector<mjtNum>{1});
|
||||
|
||||
mjtNum tol = 1e-4;
|
||||
vector wall = GetSensor(model, data, "wall");
|
||||
EXPECT_THAT(wall, Pointwise(DoubleNear(tol), {1, 8, 0, 0, -1, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0}));
|
||||
|
||||
// normals points *away* from b2 (towards floor / b1)
|
||||
vector b2 = GetSensor(model, data, "b2");
|
||||
EXPECT_THAT(b2, Pointwise(DoubleNear(tol), {3, 0, 0, 0, 0, -1,
|
||||
4, 0, 0, 1, 0, 0}));
|
||||
|
||||
// normal points *towards* b2
|
||||
vector b2f = GetSensor(model, data, "b2_flipped");
|
||||
EXPECT_THAT(b2f, Pointwise(DoubleNear(tol), {3, 0, 0, 0, 0, 1,
|
||||
4, 0, 0, -1, 0, 0}));
|
||||
|
||||
vector b2r = GetSensor(model, data, "b2_reduced");
|
||||
EXPECT_THAT(b2r, Pointwise(DoubleNear(tol), {4, 0, 0, -1, 0, 0}));
|
||||
}
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(SensorTest, ContactSorted) {
|
||||
const string xml_path =
|
||||
GetTestDataFilePath("engine/testdata/sensor/contact_sorted.xml");
|
||||
char error[1024];
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
while (data->time < .5) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
vector unsorted = GetSensor(model, data, "unsorted");
|
||||
EXPECT_THAT(unsorted, SizeIs(4));
|
||||
EXPECT_THAT(unsorted, Not(WhenSorted(ElementsAreArray(unsorted))));
|
||||
|
||||
vector sorted = GetSensor(model, data, "sorted dist");
|
||||
EXPECT_THAT(sorted, SizeIs(4));
|
||||
EXPECT_THAT(sorted, WhenSorted(ElementsAreArray(sorted)));
|
||||
|
||||
vector sorted_force = GetSensor(model, data, "sorted force");
|
||||
EXPECT_THAT(sorted_force, SizeIs(12));
|
||||
vector<mjtNum> nnorms;
|
||||
for (size_t i = 0; i < sorted_force.size(); i += 3) {
|
||||
nnorms.push_back(-sorted_force[i]*sorted_force[i] +
|
||||
-sorted_force[i+1]*sorted_force[i+1] +
|
||||
-sorted_force[i+2]*sorted_force[i+2]);
|
||||
}
|
||||
EXPECT_THAT(nnorms, WhenSorted(ElementsAreArray(nnorms)));
|
||||
|
||||
vector smallest = GetSensor(model, data, "smallest dist");
|
||||
EXPECT_THAT(smallest, SizeIs(1));
|
||||
EXPECT_EQ(smallest[0], sorted[0]);
|
||||
|
||||
vector largest = GetSensor(model, data, "largest force");
|
||||
EXPECT_THAT(largest, SizeIs(3));
|
||||
EXPECT_THAT(largest, ElementsAre(sorted_force[0],
|
||||
sorted_force[1],
|
||||
sorted_force[2]));
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(SensorTest, ContactSubtree) {
|
||||
const string xml_path =
|
||||
GetTestDataFilePath("engine/testdata/sensor/contact_subtree.xml");
|
||||
char error[1024];
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
while (data->time < 0.2) {
|
||||
mj_step(model, data);
|
||||
|
||||
int all = GetSensor(model, data, "all")[0];
|
||||
int w_t1 = GetSensor(model, data, "w_t1")[0];
|
||||
int w_t2 = GetSensor(model, data, "w_t2")[0];
|
||||
int t1 = GetSensor(model, data, "t1")[0];
|
||||
int t2 = GetSensor(model, data, "t2")[0];
|
||||
int t1_t1 = GetSensor(model, data, "t1_t1")[0];
|
||||
int t2_t2 = GetSensor(model, data, "t2_t2")[0];
|
||||
int t1_t2 = GetSensor(model, data, "t1_t2")[0];
|
||||
int t2_t1 = GetSensor(model, data, "t2_t1")[0];
|
||||
|
||||
// compute the number of first tree contacts in two different ways
|
||||
EXPECT_EQ(t1, w_t1 + t1_t1 + t1_t2);
|
||||
|
||||
// compute the number of second tree contacts in two different ways
|
||||
EXPECT_EQ(t2, w_t2 + t2_t2 + t2_t1);
|
||||
|
||||
// compute the number of all contacts in two different ways
|
||||
EXPECT_EQ(all, w_t1 + w_t2 + t1_t1 + t2_t2 + t1_t2);
|
||||
}
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(SensorTest, CameraProjection) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
@@ -725,7 +941,7 @@ TEST_F(SensorTest, InsideSite) {
|
||||
for (int i = 0; i < 5; i++) {
|
||||
data->qpos[0] = hpos[i];
|
||||
mj_forward(model, data);
|
||||
std::vector<mjtNum> expected(5, 0.0);
|
||||
vector<mjtNum> expected(5, 0.0);
|
||||
expected[i] = 1.0;
|
||||
EXPECT_EQ(AsVector(data->sensordata, model->nsensordata), expected);
|
||||
}
|
||||
|
||||
@@ -947,5 +947,15 @@ TEST_F(SupportTest, SetKeyframe) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(SupportTest, ContactSensorDim) {
|
||||
int dataSpec = 1 << mjCONDATA_FOUND |
|
||||
1 << mjCONDATA_FORCE |
|
||||
1 << mjCONDATA_DIST |
|
||||
1 << mjCONDATA_POS |
|
||||
1 << mjCONDATA_TANGENT;
|
||||
|
||||
EXPECT_EQ(mju_condataSize(dataSpec), 1+3+1+3+3);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
+35
@@ -0,0 +1,35 @@
|
||||
<mujoco model="contact">
|
||||
<option gravity="4 0 -3"/>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 3"/>
|
||||
<geom name="floor" type="plane" size=".5 1 .01"/>
|
||||
<geom name="wall" type="plane" size=".25 1 .05" zaxis="-1 0 0" pos=".5 0 .25"/>
|
||||
<site name="site" type="box" size=".15 .15 .15" pos=".5 0 0" rgba=".5 .7 .5 .3"/>
|
||||
|
||||
<body name="b1" pos="0 0 .2">
|
||||
<freejoint/>
|
||||
<geom name="g1" size=".1" rgba=".8 .2 .1 1" mass="1"/>
|
||||
</body>
|
||||
|
||||
<body name="b2" pos="-.3 0 .2">
|
||||
<freejoint/>
|
||||
<geom name="g2" size=".1" rgba=".8 .2 .1 1" mass="1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<sensor>
|
||||
<contact name="all"/>
|
||||
<contact name="world" body1="world"/>
|
||||
<contact name="b1" body1="b1"/>
|
||||
<contact name="g1" geom1="g1"/>
|
||||
<contact name="b1:g2" body1="b1" geom2="g2"/>
|
||||
<contact name="b1:world" body1="b1" body2="world"/>
|
||||
<contact name="site" site="site"/>
|
||||
<contact name="site:wall" site="site" geom2="wall"/>
|
||||
<contact name="wall" geom1="wall" num="2" data="found force normal"/>
|
||||
<contact name="b2" body1="b2" data="force normal" num="2"/>
|
||||
<contact name="b2_flipped" body2="b2" data="force normal" num="2"/>
|
||||
<contact name="b2_reduced" body2="b2" data="force normal" reduce="maxforce"/>
|
||||
</sensor>
|
||||
</mujoco>
|
||||
+22
@@ -0,0 +1,22 @@
|
||||
<mujoco model="contact sorted">
|
||||
<worldbody>
|
||||
<light pos="0 0 3"/>
|
||||
<geom name="floor" type="plane" size=".5 1 .01"/>
|
||||
|
||||
<body name="spheres">
|
||||
<joint axis="0 -1 0"/>
|
||||
<geom size=".1" pos="-.1 0 0" rgba=".8 .2 .1 1"/>
|
||||
<geom size=".1" pos=".1 0 0" rgba=".8 .2 .1 1"/>
|
||||
<geom size=".1" pos="-.2 0 0" rgba=".8 .2 .1 1"/>
|
||||
<geom size=".1" pos=".3 0 0" rgba=".8 .2 .1 1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<sensor>
|
||||
<contact name="unsorted" body1="spheres" num="4" data="dist"/>
|
||||
<contact name="sorted dist" body1="spheres" num="4" data="dist" reduce="mindist"/>
|
||||
<contact name="sorted force" body1="spheres" num="4" data="force" reduce="maxforce"/>
|
||||
<contact name="smallest dist" body1="spheres" num="1" data="dist" reduce="mindist"/>
|
||||
<contact name="largest force" body1="spheres" num="1" data="force" reduce="maxforce"/>
|
||||
</sensor>
|
||||
</mujoco>
|
||||
+30
@@ -0,0 +1,30 @@
|
||||
<mujoco model="contact subtree">
|
||||
<asset>
|
||||
<model file="humanoid.xml"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 5"/>
|
||||
<geom name="floor" type="plane" size="2 2 .01"/>
|
||||
|
||||
<!-- two horizontal humanoids, in the air, one above the other -->
|
||||
<frame pos="-.5 0 1.2" euler="0 90 0">
|
||||
<attach model="Humanoid" body="torso" prefix="1"/>
|
||||
</frame>
|
||||
<frame pos="-.5 0 0.5" euler="0 90 -90">
|
||||
<attach model="Humanoid" body="torso" prefix="2"/>
|
||||
</frame>
|
||||
</worldbody>
|
||||
|
||||
<sensor>
|
||||
<contact name="all"/>
|
||||
<contact name="w_t1" body1="world" subtree2="1torso"/>
|
||||
<contact name="w_t2" body1="world" subtree2="2torso"/>
|
||||
<contact name="t1" subtree1="1torso"/>
|
||||
<contact name="t2" subtree1="2torso"/>
|
||||
<contact name="t1_t1" subtree1="1torso" subtree2="1torso"/>
|
||||
<contact name="t2_t2" subtree1="2torso" subtree2="2torso"/>
|
||||
<contact name="t1_t2" subtree1="1torso" subtree2="2torso"/>
|
||||
<contact name="t2_t1" subtree1="2torso" subtree2="1torso"/>
|
||||
</sensor>
|
||||
</mujoco>
|
||||
+267
@@ -0,0 +1,267 @@
|
||||
<!-- 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.
|
||||
-->
|
||||
|
||||
<mujoco model="Humanoid">
|
||||
<option timestep="0.005"/>
|
||||
|
||||
<visual>
|
||||
<map force="0.1" zfar="30"/>
|
||||
<rgba haze="0.15 0.25 0.35 1"/>
|
||||
<global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
|
||||
</visual>
|
||||
|
||||
<statistic center="0 0 0.7"/>
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
|
||||
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/>
|
||||
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
|
||||
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
|
||||
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
|
||||
</asset>
|
||||
|
||||
<default>
|
||||
<motor ctrlrange="-1 1" ctrllimited="true"/>
|
||||
<default class="body">
|
||||
|
||||
<!-- geoms -->
|
||||
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" group="1"/>
|
||||
<default class="thigh">
|
||||
<geom size=".06"/>
|
||||
</default>
|
||||
<default class="shin">
|
||||
<geom fromto="0 0 0 0 0 -.3" size=".049"/>
|
||||
</default>
|
||||
<default class="foot">
|
||||
<geom size=".027"/>
|
||||
<default class="foot1">
|
||||
<geom fromto="-.07 -.01 0 .14 -.03 0"/>
|
||||
</default>
|
||||
<default class="foot2">
|
||||
<geom fromto="-.07 .01 0 .14 .03 0"/>
|
||||
</default>
|
||||
</default>
|
||||
<default class="arm_upper">
|
||||
<geom size=".04"/>
|
||||
</default>
|
||||
<default class="arm_lower">
|
||||
<geom size=".031"/>
|
||||
</default>
|
||||
<default class="hand">
|
||||
<geom type="sphere" size=".04"/>
|
||||
</default>
|
||||
|
||||
<!-- joints -->
|
||||
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
|
||||
<default class="joint_big">
|
||||
<joint damping="5" stiffness="10"/>
|
||||
<default class="hip_x">
|
||||
<joint range="-30 10"/>
|
||||
</default>
|
||||
<default class="hip_z">
|
||||
<joint range="-60 35"/>
|
||||
</default>
|
||||
<default class="hip_y">
|
||||
<joint axis="0 1 0" range="-150 20"/>
|
||||
</default>
|
||||
<default class="joint_big_stiff">
|
||||
<joint stiffness="20"/>
|
||||
</default>
|
||||
</default>
|
||||
<default class="knee">
|
||||
<joint pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
|
||||
</default>
|
||||
<default class="ankle">
|
||||
<joint range="-50 50"/>
|
||||
<default class="ankle_y">
|
||||
<joint pos="0 0 .08" axis="0 1 0" stiffness="6"/>
|
||||
</default>
|
||||
<default class="ankle_x">
|
||||
<joint pos="0 0 .04" stiffness="3"/>
|
||||
</default>
|
||||
</default>
|
||||
<default class="shoulder">
|
||||
<joint range="-85 60"/>
|
||||
</default>
|
||||
<default class="elbow">
|
||||
<joint range="-100 50" stiffness="0"/>
|
||||
</default>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>
|
||||
<light name="spotlight" mode="targetbodycom" target="torso" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 -6 4" cutoff="30"/>
|
||||
<light name="top" pos="0 0 2" mode="trackcom"/>
|
||||
<body name="torso" pos="0 0 1.282" childclass="body">
|
||||
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
|
||||
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
|
||||
<freejoint name="root"/>
|
||||
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
|
||||
<geom name="waist_upper" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
|
||||
<body name="head" pos="0 0 .19">
|
||||
<geom name="head" type="sphere" size=".09"/>
|
||||
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
|
||||
</body>
|
||||
<body name="waist_lower" pos="-.01 0 -.26">
|
||||
<geom name="waist_lower" fromto="0 -.06 0 0 .06 0" size=".06"/>
|
||||
<joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="joint_big_stiff"/>
|
||||
<joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="joint_big"/>
|
||||
<body name="pelvis" pos="0 0 -.165">
|
||||
<joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="joint_big"/>
|
||||
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
|
||||
<body name="thigh_right" pos="0 -.1 -.04">
|
||||
<joint name="hip_x_right" axis="1 0 0" class="hip_x"/>
|
||||
<joint name="hip_z_right" axis="0 0 1" class="hip_z"/>
|
||||
<joint name="hip_y_right" class="hip_y"/>
|
||||
<geom name="thigh_right" fromto="0 0 0 0 .01 -.34" class="thigh"/>
|
||||
<body name="shin_right" pos="0 .01 -.4">
|
||||
<joint name="knee_right" class="knee"/>
|
||||
<geom name="shin_right" class="shin"/>
|
||||
<body name="foot_right" pos="0 0 -.39">
|
||||
<joint name="ankle_y_right" class="ankle_y"/>
|
||||
<joint name="ankle_x_right" class="ankle_x" axis="1 0 .5"/>
|
||||
<geom name="foot1_right" class="foot1"/>
|
||||
<geom name="foot2_right" class="foot2"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="thigh_left" pos="0 .1 -.04">
|
||||
<joint name="hip_x_left" axis="-1 0 0" class="hip_x"/>
|
||||
<joint name="hip_z_left" axis="0 0 -1" class="hip_z"/>
|
||||
<joint name="hip_y_left" class="hip_y"/>
|
||||
<geom name="thigh_left" fromto="0 0 0 0 -.01 -.34" class="thigh"/>
|
||||
<body name="shin_left" pos="0 -.01 -.4">
|
||||
<joint name="knee_left" class="knee"/>
|
||||
<geom name="shin_left" fromto="0 0 0 0 0 -.3" class="shin"/>
|
||||
<body name="foot_left" pos="0 0 -.39">
|
||||
<joint name="ankle_y_left" class="ankle_y"/>
|
||||
<joint name="ankle_x_left" class="ankle_x" axis="-1 0 -.5"/>
|
||||
<geom name="foot1_left" class="foot1"/>
|
||||
<geom name="foot2_left" class="foot2"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="upper_arm_right" pos="0 -.17 .06">
|
||||
<joint name="shoulder1_right" axis="2 1 1" class="shoulder"/>
|
||||
<joint name="shoulder2_right" axis="0 -1 1" class="shoulder"/>
|
||||
<geom name="upper_arm_right" fromto="0 0 0 .16 -.16 -.16" class="arm_upper"/>
|
||||
<body name="lower_arm_right" pos=".18 -.18 -.18">
|
||||
<joint name="elbow_right" axis="0 -1 1" class="elbow"/>
|
||||
<geom name="lower_arm_right" fromto=".01 .01 .01 .17 .17 .17" class="arm_lower"/>
|
||||
<body name="hand_right" pos=".18 .18 .18">
|
||||
<geom name="hand_right" zaxis="1 1 1" class="hand"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="upper_arm_left" pos="0 .17 .06">
|
||||
<joint name="shoulder1_left" axis="-2 1 -1" class="shoulder"/>
|
||||
<joint name="shoulder2_left" axis="0 -1 -1" class="shoulder"/>
|
||||
<geom name="upper_arm_left" fromto="0 0 0 .16 .16 -.16" class="arm_upper"/>
|
||||
<body name="lower_arm_left" pos=".18 .18 -.18">
|
||||
<joint name="elbow_left" axis="0 -1 -1" class="elbow"/>
|
||||
<geom name="lower_arm_left" fromto=".01 -.01 .01 .17 -.17 .17" class="arm_lower"/>
|
||||
<body name="hand_left" pos=".18 -.18 .18">
|
||||
<geom name="hand_left" zaxis="1 -1 1" class="hand"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<contact>
|
||||
<exclude body1="waist_lower" body2="thigh_right"/>
|
||||
<exclude body1="waist_lower" body2="thigh_left"/>
|
||||
</contact>
|
||||
|
||||
<tendon>
|
||||
<fixed name="hamstring_right" limited="true" range="-0.3 2">
|
||||
<joint joint="hip_y_right" coef=".5"/>
|
||||
<joint joint="knee_right" coef="-.5"/>
|
||||
</fixed>
|
||||
<fixed name="hamstring_left" limited="true" range="-0.3 2">
|
||||
<joint joint="hip_y_left" coef=".5"/>
|
||||
<joint joint="knee_left" coef="-.5"/>
|
||||
</fixed>
|
||||
</tendon>
|
||||
|
||||
<actuator>
|
||||
<motor name="abdomen_z" gear="40" joint="abdomen_z"/>
|
||||
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
|
||||
<motor name="abdomen_x" gear="40" joint="abdomen_x"/>
|
||||
<motor name="hip_x_right" gear="40" joint="hip_x_right"/>
|
||||
<motor name="hip_z_right" gear="40" joint="hip_z_right"/>
|
||||
<motor name="hip_y_right" gear="120" joint="hip_y_right"/>
|
||||
<motor name="knee_right" gear="80" joint="knee_right"/>
|
||||
<motor name="ankle_y_right" gear="20" joint="ankle_y_right"/>
|
||||
<motor name="ankle_x_right" gear="20" joint="ankle_x_right"/>
|
||||
<motor name="hip_x_left" gear="40" joint="hip_x_left"/>
|
||||
<motor name="hip_z_left" gear="40" joint="hip_z_left"/>
|
||||
<motor name="hip_y_left" gear="120" joint="hip_y_left"/>
|
||||
<motor name="knee_left" gear="80" joint="knee_left"/>
|
||||
<motor name="ankle_y_left" gear="20" joint="ankle_y_left"/>
|
||||
<motor name="ankle_x_left" gear="20" joint="ankle_x_left"/>
|
||||
<motor name="shoulder1_right" gear="20" joint="shoulder1_right"/>
|
||||
<motor name="shoulder2_right" gear="20" joint="shoulder2_right"/>
|
||||
<motor name="elbow_right" gear="40" joint="elbow_right"/>
|
||||
<motor name="shoulder1_left" gear="20" joint="shoulder1_left"/>
|
||||
<motor name="shoulder2_left" gear="20" joint="shoulder2_left"/>
|
||||
<motor name="elbow_left" gear="40" joint="elbow_left"/>
|
||||
</actuator>
|
||||
|
||||
<keyframe>
|
||||
<!--
|
||||
The values below are split into rows for readibility:
|
||||
torso position
|
||||
torso orientation
|
||||
spinal
|
||||
right leg
|
||||
left leg
|
||||
arms
|
||||
-->
|
||||
<key name="squat"
|
||||
qpos="0 0 0.596
|
||||
0.988015 0 0.154359 0
|
||||
0 0.4 0
|
||||
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
|
||||
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
|
||||
0 0 0 0 0 0"/>
|
||||
<key name="stand_on_left_leg"
|
||||
qpos="0 0 1.21948
|
||||
0.971588 -0.179973 0.135318 -0.0729076
|
||||
-0.0516 -0.202 0.23
|
||||
-0.24 -0.007 -0.34 -1.76 -0.466 -0.0415
|
||||
-0.08 -0.01 -0.37 -0.685 -0.35 -0.09
|
||||
0.109 -0.067 -0.7 -0.05 0.12 0.16"/>
|
||||
<key name="prone"
|
||||
qpos="0.4 0 0.0757706
|
||||
0.7325 0 0.680767 0
|
||||
0 0.0729 0
|
||||
0.0077 0.0019 -0.026 -0.351 -0.27 0
|
||||
0.0077 0.0019 -0.026 -0.351 -0.27 0
|
||||
0.56 -0.62 -1.752
|
||||
0.56 -0.62 -1.752"/>
|
||||
<key name="supine"
|
||||
qpos="-0.4 0 0.08122
|
||||
0.722788 0 -0.69107 0
|
||||
0 -0.25 0
|
||||
0.0182 0.0142 0.3 0.042 -0.44 -0.02
|
||||
0.0182 0.0142 0.3 0.042 -0.44 -0.02
|
||||
0.186 -0.73 -1.73
|
||||
0.186 -0.73 -1.73"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
Reference in New Issue
Block a user