Add dampratio attribute to position and intvelocity actuators.
PiperOrigin-RevId: 639745135 Change-Id: I94ba4c346d2750c6c454a60a568f2e6f6bce19d4
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
d6712f2944
commit
2830a4071f
@@ -18,6 +18,7 @@
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
@@ -839,6 +840,83 @@ TEST_F(ActuatorTest, ActuatorGravcomp) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// Check that dampratio works as expected
|
||||
TEST_F(ActuatorTest, DampRatio) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast"/>
|
||||
|
||||
<worldbody>
|
||||
<body>
|
||||
<joint name="slide1" axis="1 0 0" type="slide"/>
|
||||
<geom size=".05"/>
|
||||
</body>
|
||||
|
||||
<body pos="0 0 -.15">
|
||||
<joint name="slide2" axis="1 0 0" type="slide"/>
|
||||
<geom size=".05"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<position name="slightly underdamped" joint="slide1" kp="10" dampratio="0.99"/>
|
||||
<position name="slightly overdamped" joint="slide2" kp="10" dampratio="1.01"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
mjModel* model = LoadModelFromString(xml);
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
data->qpos[0] = data->qpos[1] = -0.1;
|
||||
|
||||
mjtNum under_damped = data->qpos[0];
|
||||
mjtNum over_damped = data->qpos[1];
|
||||
while (data->time < 10) {
|
||||
mj_step(model, data);
|
||||
under_damped = mju_max(under_damped, data->qpos[0]);
|
||||
over_damped = mju_max(over_damped, data->qpos[1]);
|
||||
}
|
||||
|
||||
// expect slightly underdamped to slightly overshoot
|
||||
EXPECT_GT(under_damped, 0);
|
||||
EXPECT_LT(under_damped, 1e-6);
|
||||
|
||||
// expect slightly overdamped to slightly undershoot
|
||||
EXPECT_LT(over_damped, 0);
|
||||
EXPECT_GT(over_damped, -1e-6);
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
|
||||
// Check dampratio for actuators with nontrivial transmission
|
||||
TEST_F(ActuatorTest, DampRatioTendon) {
|
||||
const std::string xml_path =
|
||||
GetTestDataFilePath("engine/testdata/actuation/tendon_dampratio.xml");
|
||||
char error[1000];
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
data->ctrl[0] = 1;
|
||||
data->ctrl[1] = 4;
|
||||
|
||||
while (data->time < 1) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// expect first and second fingers to move together
|
||||
double tol = 1e-10;
|
||||
EXPECT_THAT(AsVector(data->qpos, 4),
|
||||
Pointwise(DoubleNear(tol), AsVector(data->qpos + 4, 4)));
|
||||
EXPECT_THAT(AsVector(data->qvel, 4),
|
||||
Pointwise(DoubleNear(tol), AsVector(data->qvel + 4, 4)));
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// ----------------------- filterexact actuators -------------------------------
|
||||
|
||||
using FilterExactTest = MujocoTest;
|
||||
|
||||
+44
@@ -0,0 +1,44 @@
|
||||
<mujoco>
|
||||
<visual>
|
||||
<global elevation="0"/>
|
||||
</visual>
|
||||
|
||||
<default>
|
||||
<position ctrlrange="-.2 .2"/>
|
||||
<joint axis="1 0 0" type="slide"/>
|
||||
</default>
|
||||
|
||||
<option integrator="implicitfast"/>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 2"/>
|
||||
|
||||
<body pos="0 0 .15">
|
||||
<joint name="0"/>
|
||||
<geom size=".05"/>
|
||||
</body>
|
||||
|
||||
<body>
|
||||
<joint name="1"/>
|
||||
<geom mass="1" size=".05"/>
|
||||
</body>
|
||||
|
||||
<body pos="0 0 -.15">
|
||||
<joint name="2"/>
|
||||
<geom mass="1" size=".05"/>
|
||||
</body>
|
||||
|
||||
<body pos="0 0 -.3">
|
||||
<joint name="3"/>
|
||||
<geom mass="1" size=".05"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<position name="r=1/3" joint="0" dampratio=".333" kp="10"/>
|
||||
<position name="r=1" joint="1" dampratio="1" kp="10"/>
|
||||
<position name="r=3" joint="2" dampratio="3" kp="10"/>
|
||||
<!-- transmission increased by 10: reduce kp by 10^2 -->
|
||||
<position name="r=1, g=10" joint="3" dampratio="1" kp=".1" gear="10" ctrlrange="-2 2"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
+2
-3
@@ -3,7 +3,6 @@
|
||||
<!--
|
||||
Adding a high fluid viscosity and using implicit integration for extra stabillity.
|
||||
Extra stabillity is required because the abstract "arm" model is not very realistic.
|
||||
(e.g. 4 consecutive ball joints is not a realistic kinematic design)
|
||||
-->
|
||||
<option viscosity="10" integrator="implicit">
|
||||
<flag contact="disable"/>
|
||||
@@ -13,7 +12,7 @@
|
||||
|
||||
<default>
|
||||
<default class="translation">
|
||||
<position kp="100" kv="10" ctrlrange="-.25 .25"/>
|
||||
<position kp="100" dampratio="1" ctrlrange="-.25 .25"/>
|
||||
</default>
|
||||
<default class="rotation">
|
||||
<!--
|
||||
@@ -22,7 +21,7 @@
|
||||
Increase this range to pi or bigger in order to see the instabillity.
|
||||
See here for more details https://mujoco.readthedocs.io/en/latest/XMLreference.html#actuator
|
||||
-->
|
||||
<position kp=".5" kv=".05" ctrlrange="-1.571 1.571"/>
|
||||
<position kp=".5" dampratio="1" ctrlrange="-1.571 1.571"/>
|
||||
</default>
|
||||
<!--
|
||||
Clamping the total actuator torque at the joints means that the motion produced by the Cartesian
|
||||
|
||||
@@ -0,0 +1,84 @@
|
||||
<mujoco>
|
||||
<visual>
|
||||
<global elevation="-20"/>
|
||||
</visual>
|
||||
|
||||
<default>
|
||||
<joint axis="0 1 0" armature="0.01"/>
|
||||
<geom type="capsule" size="0.02"/>
|
||||
</default>
|
||||
|
||||
<option integrator="implicitfast">
|
||||
<flag gravity="disable"/>
|
||||
</option>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 2"/>
|
||||
|
||||
<body>
|
||||
<joint name="a0"/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint name="b0"/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint name="c0"/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint name="d0"/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<body pos="0 -0.05 0">
|
||||
<joint name="a1"/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint name="b1"/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint name="c1"/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint name="d1"/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<equality>
|
||||
<joint joint1="a0" joint2="b0" polycoef="0 .5"/>
|
||||
<joint joint1="b0" joint2="c0" polycoef="0 .5"/>
|
||||
<joint joint1="c0" joint2="d0" polycoef="0 .5"/>
|
||||
|
||||
<joint joint1="a1" joint2="b1" polycoef="0 .5"/>
|
||||
<joint joint1="b1" joint2="c1" polycoef="0 .5"/>
|
||||
<joint joint1="c1" joint2="d1" polycoef="0 .5"/>
|
||||
</equality>
|
||||
|
||||
<tendon>
|
||||
<fixed name="finger0">
|
||||
<joint joint="a0" coef="1"/>
|
||||
<joint joint="b0" coef=".5"/>
|
||||
<joint joint="c0" coef=".25"/>
|
||||
<joint joint="d0" coef=".125"/>
|
||||
</fixed>
|
||||
|
||||
<fixed name="finger1">
|
||||
<joint joint="a1" coef="4"/>
|
||||
<joint joint="b1" coef="2"/>
|
||||
<joint joint="c1" coef="1"/>
|
||||
<joint joint="d1" coef="0.5"/>
|
||||
</fixed>
|
||||
</tendon>
|
||||
|
||||
<actuator>
|
||||
<position name="finger0" tendon="finger0" dampratio="1" kp="128" ctrlrange="-1 1"/>
|
||||
<!-- transmission increased by 4: reduce kp by 16 -->
|
||||
<position name="finger1" tendon="finger1" dampratio="1" kp="8" ctrlrange="-4 4"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
Reference in New Issue
Block a user