Skip to content

Commit c931565

Browse files
yuvaltassacopybara-github
yuvaltassa
authored and
copybara-github
committed
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
1 parent 4e206c2 commit c931565

File tree

7 files changed

+334
-0
lines changed

7 files changed

+334
-0
lines changed

‎src/engine/engine_core_smooth.c

+122
Original file line numberDiff line numberDiff line change
@@ -861,6 +861,128 @@ void mj_tendon(const mjModel* m, mjData* d) {
861861

862862

863863

864+
// compute time derivative of dense tendon Jacobian for one tendon
865+
void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) {
866+
int nv = m->nv;
867+
868+
// allocate stack arrays
869+
mjtNum *jac1, *jac2, *jacdif, *tmp;
870+
mj_markStack(d);
871+
jac1 = mjSTACKALLOC(d, 3*nv, mjtNum);
872+
jac2 = mjSTACKALLOC(d, 3*nv, mjtNum);
873+
jacdif = mjSTACKALLOC(d, 3*nv, mjtNum);
874+
tmp = mjSTACKALLOC(d, nv, mjtNum);
875+
876+
// return if tendon id is invalid
877+
if (id < 0 || id >= m->ntendon) {
878+
return;
879+
}
880+
881+
// clear output
882+
mju_zero(Jdot, nv);
883+
884+
// fixed tendon has zero Jdot: return
885+
int adr = m->tendon_adr[id];
886+
if (m->wrap_type[adr] == mjWRAP_JOINT) {
887+
return;
888+
}
889+
890+
// process spatial tendon
891+
mjtNum divisor = 1;
892+
int wraptype, j = 0;
893+
int num = m->tendon_num[id];
894+
while (j < num-1) {
895+
// get 1st and 2nd object
896+
int type0 = m->wrap_type[adr+j+0];
897+
int type1 = m->wrap_type[adr+j+1];
898+
int id0 = m->wrap_objid[adr+j+0];
899+
int id1 = m->wrap_objid[adr+j+1];
900+
901+
// pulley
902+
if (type0 == mjWRAP_PULLEY || type1 == mjWRAP_PULLEY) {
903+
// get divisor, insert obj=-2
904+
if (type0 == mjWRAP_PULLEY) {
905+
divisor = m->wrap_prm[adr+j];
906+
}
907+
908+
// move to next
909+
j++;
910+
continue;
911+
}
912+
913+
// init sequence; assume it starts with site
914+
mjtNum wpnt[6];
915+
mju_copy3(wpnt, d->site_xpos+3*id0);
916+
mjtNum vel[6];
917+
mj_objectVelocity(m, d, mjOBJ_SITE, id0, vel, /*flg_local=*/0);
918+
mjtNum wvel[6] = {vel[3], vel[4], vel[5], 0, 0, 0};
919+
int wbody[2];
920+
wbody[0] = m->site_bodyid[id0];
921+
922+
// second object is geom: process site-geom-site
923+
if (type1 == mjWRAP_SPHERE || type1 == mjWRAP_CYLINDER) {
924+
// TODO(tassa) support geom wrapping (requires derivatives of mju_wrap)
925+
mjERROR("geom wrapping not supported");
926+
} else {
927+
wraptype = mjWRAP_NONE;
928+
}
929+
930+
// complete sequence
931+
wbody[1] = m->site_bodyid[id1];
932+
mju_copy3(wpnt+3, d->site_xpos+3*id1);
933+
mj_objectVelocity(m, d, mjOBJ_SITE, id1, vel, /*flg_local=*/0);
934+
mju_copy3(wvel+3, vel+3);
935+
936+
// accumulate moments if consecutive points are in different bodies
937+
if (wbody[0] != wbody[1]) {
938+
// dpnt = 3D position difference, normalize
939+
mjtNum dpnt[3];
940+
mju_sub3(dpnt, wpnt+3, wpnt);
941+
mjtNum norm = mju_norm3(dpnt);
942+
mju_scl3(dpnt, dpnt, 1/norm);
943+
944+
// dvel = d / dt (dpnt)
945+
mjtNum dvel[3];
946+
mju_sub3(dvel, wvel+3, wvel);
947+
mjtNum dot = mju_dot3(dpnt, dvel);
948+
mju_addToScl3(dvel, dpnt, -dot);
949+
mju_scl3(dvel, dvel, 1/norm);
950+
951+
// TODO(tassa ) write sparse branch, requires mj_jacDotSparse
952+
// if (mj_isSparse(m)) { ... }
953+
954+
// get endpoint JacobianDots, subtract
955+
mj_jacDot(m, d, jac1, 0, wpnt, wbody[0]);
956+
mj_jacDot(m, d, jac2, 0, wpnt+3, wbody[1]);
957+
mju_sub(jacdif, jac2, jac1, 3*nv);
958+
959+
// chain rule, first term: Jdot += d/dt(jac2 - jac1) * dpnt
960+
mju_mulMatTVec(tmp, jacdif, dpnt, 3, nv);
961+
962+
// add to existing
963+
mju_addToScl(Jdot, tmp, 1/divisor, nv);
964+
965+
// get endpoint Jacobians, subtract
966+
mj_jac(m, d, jac1, 0, wpnt, wbody[0]);
967+
mj_jac(m, d, jac2, 0, wpnt+3, wbody[1]);
968+
mju_sub(jacdif, jac2, jac1, 3*nv);
969+
970+
// chain rule, second term: Jdot += (jac2 - jac1) * d/dt(dpnt)
971+
mju_mulMatTVec(tmp, jacdif, dvel, 3, nv);
972+
973+
// add to existing
974+
mju_addToScl(Jdot, tmp, 1/divisor, nv);
975+
}
976+
977+
// advance
978+
j += (wraptype != mjWRAP_NONE ? 2 : 1);
979+
}
980+
981+
mj_freeStack(d);
982+
}
983+
984+
985+
864986
// compute actuator/transmission lengths and moments
865987
void mj_transmission(const mjModel* m, mjData* d) {
866988
int nv = m->nv, nu = m->nu;

‎src/engine/engine_core_smooth.h

+3
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,9 @@ MJAPI void mj_flex(const mjModel* m, mjData* d);
3939
// compute tendon lengths, velocities and moment arms
4040
MJAPI void mj_tendon(const mjModel* m, mjData* d);
4141

42+
// compute time derivative of dense tendon Jacobian for one tendon
43+
MJAPI void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot);
44+
4245
// compute actuator transmission lengths and moments
4346
MJAPI void mj_transmission(const mjModel* m, mjData* d);
4447

‎test/engine/engine_core_smooth_test.cc

+54
Original file line numberDiff line numberDiff line change
@@ -159,6 +159,60 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) {
159159
mj_deleteModel(model);
160160
}
161161

162+
static const char* const kTen_J0 = "engine/testdata/core_smooth/ten_J0.xml";
163+
static const char* const kTen_J1 = "engine/testdata/core_smooth/ten_J1.xml";
164+
static const char* const kTen_J2 = "engine/testdata/core_smooth/ten_J2.xml";
165+
static const char* const kTen_J3 = "engine/testdata/core_smooth/ten_J3.xml";
166+
167+
TEST_F(CoreSmoothTest, TendonJdot) {
168+
for (const char* local_path : {kTen_J0, kTen_J1, kTen_J2, kTen_J3}) {
169+
const std::string xml_path = GetTestDataFilePath(local_path);
170+
char error[1024];
171+
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
172+
int nv = m->nv;
173+
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
174+
EXPECT_EQ(m->ntendon, 1);
175+
mjData* d = mj_makeData(m);
176+
177+
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
178+
m->opt.jacobian = sparsity;
179+
180+
if (m->nkey) {
181+
mj_resetDataKeyframe(m, d, 0);
182+
} else {
183+
mj_resetData(m, d);
184+
while (d->time < 1) {
185+
mj_step(m, d);
186+
}
187+
}
188+
189+
mj_forward(m, d);
190+
191+
// get current J and Jdot for the tendon
192+
vector<mjtNum> ten_J(d->ten_J, d->ten_J + nv);
193+
vector<mjtNum> ten_Jdot(nv, 0);
194+
mj_tendonDot(m, d, 0, ten_Jdot.data());
195+
196+
// compute finite-differenced Jdot
197+
mjtNum h = 1e-7;
198+
mj_integratePos(m, d->qpos, d->qvel, h);
199+
mj_kinematics(m, d);
200+
mj_comPos(m, d);
201+
mj_tendon(m, d);
202+
vector<mjtNum> ten_Jh(d->ten_J, d->ten_J + nv);
203+
mju_subFrom(ten_Jh.data(), ten_J.data(), nv);
204+
mju_scl(ten_Jh.data(), ten_Jh.data(), 1.0 / h, nv);
205+
206+
// expect analytic and FD derivatives to be similar to eps precision
207+
mjtNum eps = 1e-6;
208+
EXPECT_THAT(ten_Jdot, Pointwise(DoubleNear(eps), ten_Jh));
209+
}
210+
211+
mj_deleteData(d);
212+
mj_deleteModel(m);
213+
}
214+
}
215+
162216
// --------------------------- connect constraint ------------------------------
163217

164218
// test that bodies hanging on connects lead to expected force sensor readings

‎test/engine/testdata/core_smooth/ten_J0.xml

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
<mujoco>
2+
<option integrator="RK4">
3+
<flag energy="enable"/>
4+
</option>
5+
6+
<worldbody>
7+
<site name="tendon" type="box" size=".03 .03 .03" pos="-.5 0 -.5"/>
8+
<body name="link1" pos="-1 0 0">
9+
<joint name="link1" axis="0 -1 0" stiffness="100" springref="90"/>
10+
<geom type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
11+
<geom type="sphere" size=".08" pos="0 0 -1"/>
12+
<site name="link1" type="box" size=".03 .03 .03" pos="0 0 -.5"/>
13+
</body>
14+
</worldbody>
15+
16+
<tendon>
17+
<spatial width=".01" rgba=".2 .2 1 1">
18+
<site site="link1"/>
19+
<site site="tendon"/>
20+
</spatial>
21+
</tendon>
22+
23+
<keyframe>
24+
<key qpos="1" qvel="1"/>
25+
</keyframe>
26+
</mujoco>

‎test/engine/testdata/core_smooth/ten_J1.xml

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
<mujoco>
2+
<option integrator="RK4">
3+
<flag contact="disable" gravity="disable" energy="enable"/>
4+
</option>
5+
6+
<worldbody>
7+
<site name="tendon" type="box" size=".03 .03 .03" pos="-.5 0 0"/>
8+
<body name="link1" pos="-1 0 0">
9+
<joint name="link1" axis="0 -1 0" damping="0" stiffness="50"/>
10+
<geom name="link1" type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
11+
<body name="link2" pos="0 0 -1">
12+
<joint name="link2" axis="0 -1 0" damping="0" stiffness="1" springref="-180"/>
13+
<geom name="link2" type="capsule" size=".03" fromto="0 0 0 .6 0 0"/>
14+
<site name="link2" type="box" size=".03 .03 .03" pos=".5 0 0"/>
15+
</body>
16+
</body>
17+
</worldbody>
18+
19+
<tendon>
20+
<spatial width=".01" rgba=".2 .2 1 1">
21+
<site site="link2"/>
22+
<site site="tendon"/>
23+
</spatial>
24+
</tendon>
25+
26+
<keyframe>
27+
<key qpos="-1 0" qvel="1 1"/>
28+
</keyframe>
29+
</mujoco>

‎test/engine/testdata/core_smooth/ten_J2.xml

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
<mujoco>
2+
<option integrator="RK4">
3+
<flag contact="disable" gravity="disable" energy="enable"/>
4+
</option>
5+
6+
<worldbody>
7+
<body name="thigh" pos="-1 0 0">
8+
<site name="0" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
9+
<joint axis="1 0 0" stiffness="50"/>
10+
<joint axis="0 1 0" stiffness="50"/>
11+
<joint axis="0 0 1" stiffness="50"/>
12+
<geom type="capsule" size=".04" fromto="0 0 0 0 0 -1"/>
13+
<geom type="capsule" size=".02" fromto="0 0 -1 .05 -.08 -1.1"/>
14+
<site name="1" type="box" size=".03 .03 .03" pos=".05 -.08 -1.1"/>
15+
<body name="calf" pos="0 0 -1">
16+
<joint axis="0 -1 0" damping="0" stiffness="5" springref="90"/>
17+
<geom type="capsule" size=".03" fromto="0 0 0 -.6 0 0"/>
18+
<body name="foot" pos="-.6 0 0">
19+
<joint type="ball" stiffness="20" armature=".1"/>
20+
<site name="2" type="box" size=".03 .03 .03" pos=".03 .1 .15"/>
21+
<geom name="foot" type="box" size=".03 .1 .15" pos="0 0 -.05"/>
22+
</body>
23+
</body>
24+
</body>
25+
</worldbody>
26+
27+
<tendon>
28+
<spatial name="ten" width=".01" rgba=".2 .2 1 1">
29+
<site site="0"/>
30+
<site site="1"/>
31+
<site site="2"/>
32+
</spatial>
33+
</tendon>
34+
</mujoco>

‎test/engine/testdata/core_smooth/ten_J3.xml

+66
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
<mujoco>
2+
<option integrator="RK4">
3+
<flag contact="disable" energy="enable"/>
4+
</option>
5+
6+
<default>
7+
<joint axis="0 1 0" range="0 60" limited="true"/>
8+
<geom type="capsule" size=".01" rgba=".9 .7 .1 1"/>
9+
<site rgba=".9 .9 .9 1"/>
10+
<default class="visual_cylinder">
11+
<geom type="cylinder" fromto="0 .015 0 0 -.015 0" size=".02" rgba=".3 .9 .3 .4"/>
12+
</default>
13+
</default>
14+
15+
<worldbody>
16+
<light pos="0 0 1"/>
17+
<body name="weight" pos="-.03 0 .175">
18+
<joint axis="0 0 1" pos="0 0 .025" type="slide"/>
19+
<geom type="cylinder" size=".03 .025" density="5000" rgba=".2 .2 .5 1"/>
20+
<site name="s1" pos="0 0 .025"/>
21+
</body>
22+
23+
<site name="s2" pos="-.03 0 .33"/>
24+
25+
<body pos="0 0 .3">
26+
<joint/>
27+
<geom fromto="0 0 0 .1 0 0"/>
28+
<site name="s3" pos=".02 0 .03"/>
29+
30+
<body pos=".1 0 0">
31+
<joint/>
32+
<geom fromto="0 0 0 .1 0 0"/>
33+
<site name="s4" pos=".03 0 .01"/>
34+
<site name="s5" pos=".05 0 .02"/>
35+
<site name="side2" pos="0 0 .03"/>
36+
37+
<body pos=".1 0 0">
38+
<joint/>
39+
<geom fromto="0 0 0 .1 0 0" size=".017"/>
40+
<site name="s6" pos=".03 0 .01"/>
41+
<site name="side3" pos="0 0 .02"/>
42+
</body>
43+
</body>
44+
</body>
45+
</worldbody>
46+
47+
<tendon>
48+
<spatial range="0 .33" limited="true" width=".002" rgba=".95 .3 .3 1">
49+
<site site="s1"/>
50+
<site site="s2"/>
51+
<site site="s3"/>
52+
53+
<pulley divisor="2"/>
54+
<site site="s3"/>
55+
<site site="side2"/>
56+
<site site="s4"/>
57+
58+
<pulley divisor="2"/>
59+
<site site="s3"/>
60+
<site site="side2"/>
61+
<site site="s5"/>
62+
<site site="side3"/>
63+
<site site="s6"/>
64+
</spatial>
65+
</tendon>
66+
</mujoco>

0 commit comments

Comments
 (0)