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

Improvements related to models where joint-actuator relationship is not one-to-one:

Browse Source 
- Add `joint-actuatorforcerange` for clamping total actuator force at joints. Add `sensor-jointactuatorfrc` for sensing total actuator forces on a single joint.
See [documentation](https://mujoco.readthedocs.io/en/latest//modeling.html#actuator-force-clamping) for justification and use cases.
- Add simple car model to `model/`.
- Move actuation-related test models into `engine/testdata/actuation/`.

PiperOrigin-RevId: 549355941
Change-Id: I27f6c1f80426d73a2811ef5ae74684a228b2fbd1
This commit is contained in:
Yuval Tassa
2023-07-19 10:28:24 -07:00
committed by Copybara-Service
parent 7603b07a20
commit 51aa375af0
29 changed files with 453 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_core_smooth_test.cc
+6 -6
View File
@@ -252,7 +252,7 @@ TEST_F(CoreSmoothTest, WeldRatioMultipleConstraints) {
// Test Cartesian position control using site transmission with refsite
TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
constexpr char kRefsitePath[] = "engine/testdata/refsite.xml";
constexpr char kRefsitePath[] = "engine/testdata/actuation/refsite.xml";
const std::string xml_path = GetTestDataFilePath(kRefsitePath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
ASSERT_THAT(model, NotNull());
@@ -265,7 +265,7 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
mju_copy3(data->ctrl+3, targetrot);
// step for 5 seconds
while (data->time < 5) {
while (data->time < 10) {
mj_step(model, data);
}
@@ -273,14 +273,14 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
int refsite_id = mj_name2id(model, mjOBJ_SITE, "reference");
int site_id = mj_name2id(model, mjOBJ_SITE, "end_effector");
// check that position matches target to within 1e-5 length units
double tol_pos = 1e-5;
// check that position matches target to within 1e-3 length units
double tol_pos = 1e-3;
mjtNum relpos[3];
mju_sub3(relpos, data->site_xpos+3*site_id, data->site_xpos+3*refsite_id);
EXPECT_THAT(relpos, Pointwise(DoubleNear(tol_pos), targetpos));
// check that orientation matches target to within 1e-3 radians
double tol_rot = 1e-3;
// check that orientation matches target to within 0.06 radians
double tol_rot = 0.06;
mjtNum site_xquat[4], refsite_xquat[4], relrot[3];
mju_mat2Quat(refsite_xquat, data->site_xmat+9*refsite_id);
mju_mat2Quat(site_xquat, data->site_xmat+9*site_id);
test/engine/engine_derivative_test.cc
+1 -1
View File
@@ -97,7 +97,7 @@ static const char* const kTumblingThinObjectEllipsoidPath =
static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
static const char* const kDamperActuatorsPath =
"engine/testdata/damper.xml";
"engine/testdata/actuation/damper.xml";
static const char* const kDampedPendulumPath =
"engine/testdata/derivative/damped_pendulum.xml";
static const char* const kLinearPath =
test/engine/engine_forward_test.cc
+50 -18
View File
@@ -38,6 +38,8 @@ static const char* const kEnergyConservingPendulumPath =
"engine/testdata/derivative/energy_conserving_pendulum.xml";
static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
static const char* const kJointForceClamp =
"engine/testdata/actuation/joint_force_clamp.xml";
using ::testing::Pointwise;
using ::testing::DoubleNear;
@@ -79,7 +81,7 @@ TEST_P(ParametrizedForwardTest, ActLimited) {
data->ctrl[0] = 1.0;
// integrating up from 0, we will hit the clamp after 99 steps
for (int i=0; i<200; i++) {
for (int i=0; i < 200; i++) {
mj_step(model, data);
// always greater than lower bound
EXPECT_GT(data->act[0], -1);
@@ -90,7 +92,7 @@ TEST_P(ParametrizedForwardTest, ActLimited) {
data->ctrl[0] = -1.0;
// integrating down from 1, we will hit the clamp after 199 steps
for (int i=0; i<300; i++) {
for (int i=0; i < 300; i++) {
mj_step(model, data);
// always smaller than upper bound
EXPECT_LT(data->act[0], model->actuator_actrange[1]);
@@ -139,13 +141,13 @@ TEST_F(ForwardTest, DamperDampens) {
// move the joint
data->ctrl[0] = 100.0;
data->ctrl[1] = 0.0;
for (int i=0; i<100; i++)
for (int i=0; i < 100; i++)
mj_step(model, data);
// stop the joint with damping
data->ctrl[0] = 0.0;
data->ctrl[1] = 100.0;
for (int i=0; i<1000; i++)
for (int i=0; i < 1000; i++)
mj_step(model, data);
EXPECT_LE(data->qvel[0], std::numeric_limits<double>::epsilon());
@@ -178,7 +180,7 @@ TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) {
mjData* data = mj_makeData(model);
// step 10 times with Euler, save copy of qpos as vector
for (int i=0; i<10; i++) {
for (int i=0; i < 10; i++) {
mj_step(model, data);
}
std::vector<mjtNum> qposEuler = AsVector(data->qpos, model->nq);
@@ -186,7 +188,7 @@ TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) {
// reset, step 10 times with implicit
mj_resetData(model, data);
model->opt.integrator = mjINT_IMPLICIT;
for (int i=0; i<10; i++) {
for (int i=0; i < 10; i++) {
mj_step(model, data);
}
@@ -208,7 +210,7 @@ TEST_F(ImplicitIntegratorTest, JointActuatorEqivalent) {
mjData* data = mj_makeData(model);
// take 1000 steps with Euler
for (int i=0; i<1000; i++) {
for (int i=0; i < 1000; i++) {
mj_step(model, data);
}
// expect corresponding joint values to be significantly different
@@ -218,7 +220,7 @@ TEST_F(ImplicitIntegratorTest, JointActuatorEqivalent) {
// reset, take 1000 steps with implicit
mj_resetData(model, data);
model->opt.integrator = mjINT_IMPLICIT;
for (int i=0; i<10; i++) {
for (int i=0; i < 10; i++) {
mj_step(model, data);
}
@@ -241,7 +243,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) {
// take nstep steps with Euler, measure energy (potential + kinetic)
model->opt.integrator = mjINT_EULER;
for (int i=0; i<nstep; i++) {
for (int i=0; i < nstep; i++) {
mj_step(model, data);
}
mjtNum energyEuler = data->energy[0] + data->energy[1];
@@ -249,7 +251,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) {
// take nstep steps with implicit, measure energy
model->opt.integrator = mjINT_IMPLICIT;
mj_resetData(model, data);
for (int i=0; i<nstep; i++) {
for (int i=0; i < nstep; i++) {
mj_step(model, data);
}
mjtNum energyImplicit = data->energy[0] + data->energy[1];
@@ -257,7 +259,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) {
// take nstep steps with 4th order Runge-Kutta, measure energy
model->opt.integrator = mjINT_RK4;
mj_resetData(model, data);
for (int i=0; i<nstep; i++) {
for (int i=0; i < nstep; i++) {
mj_step(model, data);
}
mjtNum energyRK4 = data->energy[0] + data->energy[1];
@@ -326,7 +328,8 @@ TEST_F(ForwardTest, ControlClamping) {
// for the unclamped actuator, huge raises warning
data->ctrl[0] = 10*mjMAXVAL;
mj_forward(model, data);
EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 0"));
EXPECT_THAT(warning,
HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 0"));
// for the clamped actuator, huge does not raise warning
mj_resetData(model, data);
@@ -339,7 +342,8 @@ TEST_F(ForwardTest, ControlClamping) {
mj_resetData(model, data);
data->ctrl[1] = std::numeric_limits<double>::quiet_NaN();
mj_forward(model, data);
EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 1"));
EXPECT_THAT(warning,
HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 1"));
mj_deleteData(data);
mj_deleteModel(model);
@@ -412,11 +416,11 @@ TEST_F(ForwardTest, gravcomp) {
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
while(data->time < 1) { mj_step(model, data); }
while (data->time < 1) { mj_step(model, data); }
mjtNum dist = 0.5*mju_norm3(model->opt.gravity)*(data->time*data->time);
// expect that body 1 moves down allowing some slack from our estimated distance moved
// expect that body 1 moved down, allowing some slack from our estimate
EXPECT_NEAR(data->qpos[0], -dist, 0.011);
// expect that body 2 does not move
@@ -493,11 +497,11 @@ TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) {
mj_deleteModel(model);
}
// -------------------------- adhesion actuators -------------------------------
// ------------------------------ actuators -----------------------------------
using AdhesionTest = MujocoTest;
using ActuatorTest = MujocoTest;
TEST_F(AdhesionTest, ExpectedAdhesionForce) {
TEST_F(ActuatorTest, ExpectedAdhesionForce) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -1"/>
@@ -561,5 +565,33 @@ TEST_F(AdhesionTest, ExpectedAdhesionForce) {
mj_deleteModel(model);
}
// Actuator force clamping at joints
TEST_F(ActuatorTest, ActuatorForceClamping) {
const std::string xml_path = GetTestDataFilePath(kJointForceClamp);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
data->ctrl[0] = 10;
mj_forward(model, data);
// expect clamping as specified in the model
EXPECT_EQ(data->actuator_force[0], 1);
EXPECT_EQ(data->qfrc_actuator[0], 0.4);
// simulate for 2 seconds to gain velocity
while (data->time < 2) {
mj_step(model, data);
}
// activate damper, expect force to be clamped at lower bound
data->ctrl[1] = 1;
mj_forward(model, data);
EXPECT_EQ(data->qfrc_actuator[0], -0.4);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_vfs_test.cc
+1 -1
View File
@@ -26,7 +26,7 @@ using ::testing::NotNull;
using EngineVfsTest = MujocoTest;
TEST_F(EngineVfsTest, AddFileVFS) {
constexpr char path[] = "engine/testdata/";
constexpr char path[] = "engine/testdata/actuation/";
const std::string dir = GetTestDataFilePath(path);
std::string file1 = "activation.xml";
std::string file2 = "damper.xml";
test/engine/testdata/activation.xml → test/engine/testdata/actuation/activation.xml
Vendored
View File
test/engine/testdata/damper.xml → test/engine/testdata/actuation/damper.xml
Vendored
+4 -1
View File
@@ -1,5 +1,8 @@
<mujoco>
<compiler autolimits="true" />
<compiler autolimits="true"/>
<option integrator="implicitfast"/>
<worldbody>
<geom type="plane" size="1 1 .01"/>
<light pos="0 0 2"/>
test/engine/testdata/actuation/joint_force_clamp.xml
Vendored
+26
View File
@@ -0,0 +1,26 @@
<mujoco>
<compiler autolimits="true"/>
<option integrator="implicitfast"/>
<worldbody>
<geom type="plane" size="1 1 .01"/>
<light pos="0 0 2"/>
<body pos="0 0 .3">
<joint name="hinge" damping=".01" actuatorforcerange="-.4 .4"/>
<geom type="capsule" size=".01" fromto="0 0 0 .2 0 0"/>
<geom size=".03" pos=".2 0 0"/>
</body>
</worldbody>
<actuator>
<motor name="motor" joint="hinge" ctrlrange="-1 1"/>
<damper name="damper" joint="hinge" kv="10" ctrlrange="0 1"/>
</actuator>
<sensor>
<actuatorfrc name="motor" actuator="motor"/>
<actuatorfrc name="damper" actuator="damper"/>
<jointactuatorfrc name="hinge" joint="hinge"/>
</sensor>
</mujoco>
test/engine/testdata/refsite.xml → test/engine/testdata/actuation/refsite.xml
Vendored
+15 -5
View File
@@ -24,7 +24,12 @@
-->
<general gainprm=".5" biasprm="0 -.5 -.05" biastype="affine" ctrlrange="-1.571 1.571"/>
</default>
<joint damping="1e-4"/>
<!--
Clamping the total actuator torque at the joints means that the motion produced by the Cartesian
commands is achievable by individual joint actuators with the specified torque limits.
See https://mujoco.readthedocs.io/en/latest//modeling.html#actuator-force-clamping
-->
<joint stiffness="1e-1" actuatorforcerange="-1 1"/>
<site type="box" size=".012 .012 .012" rgba=".7 .7 .8 1"/>
</default>
@@ -33,16 +38,21 @@
<geom type="box" size=".25 .25 .01" pos="0 0 -.01"/>
<site name="reference" pos="0 0 .25"/>
<body name="arm" pos="-.25 .25 0">
<joint type="ball"/>
<joint axis="1 0 0"/>
<joint axis="0 1 0"/>
<geom type="box" size=".01" fromto="0 0 0 0 0 .25"/>
<body pos="0 0 .25">
<joint type="ball"/>
<joint axis="0 1 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".01" fromto="0 0 0 .25 0 0"/>
<body pos=".25 0 0">
<joint type="ball"/>
<joint axis="1 0 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".01" fromto="0 0 0 0 -.2 0"/>
<body pos="0 -.2 0">
<joint type="ball"/>
<joint axis="1 0 0"/>
<joint axis="0 0 1"/>
<joint axis="0 1 0"/>
<geom type="box" size=".01" fromto="0 0 0 0 -.05 0"/>
<site name="end_effector" pos="0 -.05 0"/>
</body>
test/engine/testdata/sites.xml → test/engine/testdata/actuation/sites.xml
Vendored
View File