Add internal functionmj_tendonDot: time derivative of tendon Jacobian for one tendon.
Notes: - Currently always uses dense math, even for sparse models. This should be easy to change in the future. - Does not support geom wrapping. This is possible but harder, requires derivatives of mju_wrap. PiperOrigin-RevId: 740378741 Change-Id: Id39ef2c4bfbb7ee11ec33c97d7d83140441cdab2
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
4e206c29c1
commit
c931565fdc
@@ -159,6 +159,60 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
static const char* const kTen_J0 = "engine/testdata/core_smooth/ten_J0.xml";
|
||||
static const char* const kTen_J1 = "engine/testdata/core_smooth/ten_J1.xml";
|
||||
static const char* const kTen_J2 = "engine/testdata/core_smooth/ten_J2.xml";
|
||||
static const char* const kTen_J3 = "engine/testdata/core_smooth/ten_J3.xml";
|
||||
|
||||
TEST_F(CoreSmoothTest, TendonJdot) {
|
||||
for (const char* local_path : {kTen_J0, kTen_J1, kTen_J2, kTen_J3}) {
|
||||
const std::string xml_path = GetTestDataFilePath(local_path);
|
||||
char error[1024];
|
||||
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||
int nv = m->nv;
|
||||
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
|
||||
EXPECT_EQ(m->ntendon, 1);
|
||||
mjData* d = mj_makeData(m);
|
||||
|
||||
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
|
||||
m->opt.jacobian = sparsity;
|
||||
|
||||
if (m->nkey) {
|
||||
mj_resetDataKeyframe(m, d, 0);
|
||||
} else {
|
||||
mj_resetData(m, d);
|
||||
while (d->time < 1) {
|
||||
mj_step(m, d);
|
||||
}
|
||||
}
|
||||
|
||||
mj_forward(m, d);
|
||||
|
||||
// get current J and Jdot for the tendon
|
||||
vector<mjtNum> ten_J(d->ten_J, d->ten_J + nv);
|
||||
vector<mjtNum> ten_Jdot(nv, 0);
|
||||
mj_tendonDot(m, d, 0, ten_Jdot.data());
|
||||
|
||||
// compute finite-differenced Jdot
|
||||
mjtNum h = 1e-7;
|
||||
mj_integratePos(m, d->qpos, d->qvel, h);
|
||||
mj_kinematics(m, d);
|
||||
mj_comPos(m, d);
|
||||
mj_tendon(m, d);
|
||||
vector<mjtNum> ten_Jh(d->ten_J, d->ten_J + nv);
|
||||
mju_subFrom(ten_Jh.data(), ten_J.data(), nv);
|
||||
mju_scl(ten_Jh.data(), ten_Jh.data(), 1.0 / h, nv);
|
||||
|
||||
// expect analytic and FD derivatives to be similar to eps precision
|
||||
mjtNum eps = 1e-6;
|
||||
EXPECT_THAT(ten_Jdot, Pointwise(DoubleNear(eps), ten_Jh));
|
||||
}
|
||||
|
||||
mj_deleteData(d);
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
}
|
||||
|
||||
// --------------------------- connect constraint ------------------------------
|
||||
|
||||
// test that bodies hanging on connects lead to expected force sensor readings
|
||||
|
||||
+26
@@ -0,0 +1,26 @@
|
||||
<mujoco>
|
||||
<option integrator="RK4">
|
||||
<flag energy="enable"/>
|
||||
</option>
|
||||
|
||||
<worldbody>
|
||||
<site name="tendon" type="box" size=".03 .03 .03" pos="-.5 0 -.5"/>
|
||||
<body name="link1" pos="-1 0 0">
|
||||
<joint name="link1" axis="0 -1 0" stiffness="100" springref="90"/>
|
||||
<geom type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
|
||||
<geom type="sphere" size=".08" pos="0 0 -1"/>
|
||||
<site name="link1" type="box" size=".03 .03 .03" pos="0 0 -.5"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<tendon>
|
||||
<spatial width=".01" rgba=".2 .2 1 1">
|
||||
<site site="link1"/>
|
||||
<site site="tendon"/>
|
||||
</spatial>
|
||||
</tendon>
|
||||
|
||||
<keyframe>
|
||||
<key qpos="1" qvel="1"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
+29
@@ -0,0 +1,29 @@
|
||||
<mujoco>
|
||||
<option integrator="RK4">
|
||||
<flag contact="disable" gravity="disable" energy="enable"/>
|
||||
</option>
|
||||
|
||||
<worldbody>
|
||||
<site name="tendon" type="box" size=".03 .03 .03" pos="-.5 0 0"/>
|
||||
<body name="link1" pos="-1 0 0">
|
||||
<joint name="link1" axis="0 -1 0" damping="0" stiffness="50"/>
|
||||
<geom name="link1" type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
|
||||
<body name="link2" pos="0 0 -1">
|
||||
<joint name="link2" axis="0 -1 0" damping="0" stiffness="1" springref="-180"/>
|
||||
<geom name="link2" type="capsule" size=".03" fromto="0 0 0 .6 0 0"/>
|
||||
<site name="link2" type="box" size=".03 .03 .03" pos=".5 0 0"/>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<tendon>
|
||||
<spatial width=".01" rgba=".2 .2 1 1">
|
||||
<site site="link2"/>
|
||||
<site site="tendon"/>
|
||||
</spatial>
|
||||
</tendon>
|
||||
|
||||
<keyframe>
|
||||
<key qpos="-1 0" qvel="1 1"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
<mujoco>
|
||||
<option integrator="RK4">
|
||||
<flag contact="disable" gravity="disable" energy="enable"/>
|
||||
</option>
|
||||
|
||||
<worldbody>
|
||||
<body name="thigh" pos="-1 0 0">
|
||||
<site name="0" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
|
||||
<joint axis="1 0 0" stiffness="50"/>
|
||||
<joint axis="0 1 0" stiffness="50"/>
|
||||
<joint axis="0 0 1" stiffness="50"/>
|
||||
<geom type="capsule" size=".04" fromto="0 0 0 0 0 -1"/>
|
||||
<geom type="capsule" size=".02" fromto="0 0 -1 .05 -.08 -1.1"/>
|
||||
<site name="1" type="box" size=".03 .03 .03" pos=".05 -.08 -1.1"/>
|
||||
<body name="calf" pos="0 0 -1">
|
||||
<joint axis="0 -1 0" damping="0" stiffness="5" springref="90"/>
|
||||
<geom type="capsule" size=".03" fromto="0 0 0 -.6 0 0"/>
|
||||
<body name="foot" pos="-.6 0 0">
|
||||
<joint type="ball" stiffness="20" armature=".1"/>
|
||||
<site name="2" type="box" size=".03 .03 .03" pos=".03 .1 .15"/>
|
||||
<geom name="foot" type="box" size=".03 .1 .15" pos="0 0 -.05"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<tendon>
|
||||
<spatial name="ten" width=".01" rgba=".2 .2 1 1">
|
||||
<site site="0"/>
|
||||
<site site="1"/>
|
||||
<site site="2"/>
|
||||
</spatial>
|
||||
</tendon>
|
||||
</mujoco>
|
||||
+66
@@ -0,0 +1,66 @@
|
||||
<mujoco>
|
||||
<option integrator="RK4">
|
||||
<flag contact="disable" energy="enable"/>
|
||||
</option>
|
||||
|
||||
<default>
|
||||
<joint axis="0 1 0" range="0 60" limited="true"/>
|
||||
<geom type="capsule" size=".01" rgba=".9 .7 .1 1"/>
|
||||
<site rgba=".9 .9 .9 1"/>
|
||||
<default class="visual_cylinder">
|
||||
<geom type="cylinder" fromto="0 .015 0 0 -.015 0" size=".02" rgba=".3 .9 .3 .4"/>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 1"/>
|
||||
<body name="weight" pos="-.03 0 .175">
|
||||
<joint axis="0 0 1" pos="0 0 .025" type="slide"/>
|
||||
<geom type="cylinder" size=".03 .025" density="5000" rgba=".2 .2 .5 1"/>
|
||||
<site name="s1" pos="0 0 .025"/>
|
||||
</body>
|
||||
|
||||
<site name="s2" pos="-.03 0 .33"/>
|
||||
|
||||
<body pos="0 0 .3">
|
||||
<joint/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
<site name="s3" pos=".02 0 .03"/>
|
||||
|
||||
<body pos=".1 0 0">
|
||||
<joint/>
|
||||
<geom fromto="0 0 0 .1 0 0"/>
|
||||
<site name="s4" pos=".03 0 .01"/>
|
||||
<site name="s5" pos=".05 0 .02"/>
|
||||
<site name="side2" pos="0 0 .03"/>
|
||||
|
||||
<body pos=".1 0 0">
|
||||
<joint/>
|
||||
<geom fromto="0 0 0 .1 0 0" size=".017"/>
|
||||
<site name="s6" pos=".03 0 .01"/>
|
||||
<site name="side3" pos="0 0 .02"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<tendon>
|
||||
<spatial range="0 .33" limited="true" width=".002" rgba=".95 .3 .3 1">
|
||||
<site site="s1"/>
|
||||
<site site="s2"/>
|
||||
<site site="s3"/>
|
||||
|
||||
<pulley divisor="2"/>
|
||||
<site site="s3"/>
|
||||
<site site="side2"/>
|
||||
<site site="s4"/>
|
||||
|
||||
<pulley divisor="2"/>
|
||||
<site site="s3"/>
|
||||
<site site="side2"/>
|
||||
<site site="s5"/>
|
||||
<site site="side3"/>
|
||||
<site site="s6"/>
|
||||
</spatial>
|
||||
</tendon>
|
||||
</mujoco>
|
||||
Reference in New Issue
Block a user