Skip to content

Commit 0998957

Browse files
yuvaltassacopybara-github
authored andcommitted
Raise NotImplementedError for contact sensors requesting linvel or angvel.
PiperOrigin-RevId: 996600360 Change-Id: I27d8c5457dc9006456ef3b4400e014b82805b876
1 parent de9b6f2 commit 0998957

18 files changed

Lines changed: 290 additions & 18 deletions

File tree

‎doc/XMLreference.rst‎

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9105,7 +9105,7 @@ Extraction
91059105

91069106
.. _sensor-contact-data:
91079107

9108-
:at:`data`: :at-val:`[found, force, torque, dist, pos, normal, tangent], "found"`
9108+
:at:`data`: :at-val:`[found, force, torque, dist, pos, normal, tangent, linvel, angvel], "found"`
91099109
Specification of which data field(s) to report from the selected contacts.
91109110

91119111
- :at-val:`found` **real(1)**: This field serves two purposes. First, it indicates whether a contact was found in
@@ -9119,6 +9119,11 @@ Extraction
91199119
- :at-val:`normal`: **real(3)**: The contact normal direction, in the global frame.
91209120
- :at-val:`tangent`: **real(3)**: The first tangent direction, in the global frame.
91219121
In order to complete the full 3x3 contact frame, use tangent2 = cross(normal, tangent).
9122+
- :at-val:`linvel`: **real(3)**: The relative linear velocity of the second contacting body with respect to the first
9123+
at the contact position, expressed in the contact frame. Positive normal velocity (first component) corresponds to
9124+
separation. Cannot be combined with ``reduce="netforce"``.
9125+
- :at-val:`angvel`: **real(3)**: The relative angular velocity of the second contacting body with respect to the
9126+
first, expressed in the contact frame. Cannot be combined with ``reduce="netforce"``.
91229127

91239128
Importantly, the :at:`data` attribute can contain **multiple sequential data types**, as long as the relative
91249129
order---as listed above---is maintained. For example, :at:`data` = :at-val:`"found force dist"` will return 5 numbers
@@ -9152,8 +9157,9 @@ Extraction
91529157
Computation stage
91539158
The computation stage (``sensor_needstage``) of a contact sensor depends on the requested :at:`data` fields and
91549159
:ref:`reduce<sensor-contact-reduce>` criterion. If :at:`data` includes ``force`` or ``torque``, or if
9155-
:at:`reduce` is ``maxforce`` or ``netforce``, the sensor is evaluated at ``mjSTAGE_ACC``; otherwise it is
9156-
evaluated at ``mjSTAGE_POS``.
9160+
:at:`reduce` is ``maxforce`` or ``netforce``, the sensor is evaluated at ``mjSTAGE_ACC``; otherwise, if
9161+
:at:`data` includes ``linvel`` or ``angvel``, it is evaluated at ``mjSTAGE_VEL``; otherwise it is evaluated at
9162+
``mjSTAGE_POS``.
91579163

91589164
.. _sensor-contact-reduce:
91599165

‎doc/changelog.rst‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,9 @@ Engine
3434
degrees of freedom. Other flexes keep all their contacts. Since version 3.6.0 the contacts of every flex were reduced,
3535
and in the midphase for all geoms of a body together, so a cloth dropped on a plane or on many static geoms in the
3636
world body was held by at most 50 contacts and fell through.
37+
- The :ref:`contact<sensor-contact>` sensor now supports ``linvel`` and ``angvel`` :ref:`data<sensor-contact-data>`
38+
fields for reporting relative contact velocities, and its computation stage (:ref:`mjtStage`) is now inferred from the
39+
requested :ref:`data<sensor-contact-data>` and :ref:`reduce<sensor-contact-reduce>` attributes.
3740
- Fixed floating-point cancellation in capsule-capsule collision detection for parallel and near-parallel axes. Thanks
3841
to :github:user:`ayonga` for reporting the issue in :doc:`MJWarp <mjwarp/index>` issue
3942
`1758 <https://github.com/google-deepmind/mujoco_warp/issues/1758>`__.

‎doc/includes/references.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2747,6 +2747,8 @@ typedef enum mjtConDataField { // data fields returned by contact sensors
27472747
mjCONDATA_POS, // contact position
27482748
mjCONDATA_NORMAL, // contact frame normal
27492749
mjCONDATA_TANGENT, // contact frame first tangent
2750+
mjCONDATA_LINVEL, // contact linear velocity
2751+
mjCONDATA_ANGVEL, // contact angular velocity
27502752

27512753
mjNCONDATA // number of contact sensor data fields
27522754
} mjtConDataField;

‎include/mujoco/mjtype.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -432,6 +432,8 @@ typedef enum mjtConDataField { // data fields returned by contact sensors
432432
mjCONDATA_POS, // contact position
433433
mjCONDATA_NORMAL, // contact frame normal
434434
mjCONDATA_TANGENT, // contact frame first tangent
435+
mjCONDATA_LINVEL, // contact linear velocity
436+
mjCONDATA_ANGVEL, // contact angular velocity
435437

436438
mjNCONDATA // number of contact sensor data fields
437439
} mjtConDataField;

‎mjx/mujoco/mjx/_src/sensor.py‎

Lines changed: 30 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -94,9 +94,9 @@ def _reduce(reduction, mask):
9494
# number of data elements per slot
9595
def nslotdata(dataspec):
9696
size = 0
97-
# found, force, torque, dist, pos, normal, tangent
97+
# found, force, torque, dist, pos, normal, tangent, linvel, angvel
9898
# TODO(taylorhowell): get sizes from mjCONDATA_SIZE
99-
for i, size_i in enumerate([1, 3, 3, 1, 3, 3, 3]):
99+
for i, size_i in enumerate([1, 3, 3, 1, 3, 3, 3, 3, 3]):
100100
if dataspec & (1 << i):
101101
size += size_i
102102
return size
@@ -214,6 +214,29 @@ def nslotdata(dataspec):
214214
if dataspec & (1 << 6): # tangent
215215
slot.append(flip[:, 2, None] * d._impl.contact.frame[cid, 1]) # pyrefly: ignore[missing-attribute]
216216

217+
if dataspec & ((1 << 7) | (1 << 8)): # linvel or angvel
218+
con_pos = d._impl.contact.pos[cid] # pyrefly: ignore[missing-attribute]
219+
con_frame = d._impl.contact.frame[cid] # pyrefly: ignore[missing-attribute]
220+
con_geom = d._impl.contact.geom[cid] # pyrefly: ignore[missing-attribute]
221+
bid0 = jp.array(m.geom_bodyid)[con_geom[:, 0]]
222+
bid1 = jp.array(m.geom_bodyid)[con_geom[:, 1]]
223+
cvel0 = d.cvel[bid0]
224+
cvel1 = d.cvel[bid1]
225+
226+
if dataspec & (1 << 7): # linvel
227+
root0 = jp.array(m.body_rootid)[bid0]
228+
root1 = jp.array(m.body_rootid)[bid1]
229+
v0 = cvel0[:, 3:] - jp.cross(con_pos - d.subtree_com[root0], cvel0[:, :3])
230+
v1 = cvel1[:, 3:] - jp.cross(con_pos - d.subtree_com[root1], cvel1[:, :3])
231+
linvel = jax.vmap(lambda f, v: f @ v)(con_frame, v1 - v0)
232+
slot.append(flip * linvel)
233+
234+
if dataspec & (1 << 8): # angvel
235+
angvel = jax.vmap(lambda f, w: f @ w)(
236+
con_frame, cvel1[:, :3] - cvel0[:, :3]
237+
)
238+
slot.append(flip * angvel)
239+
217240
found = jp.tile(jp.arange(num), nsensor) < jp.repeat(nfound, num)
218241
sensors.append((found[:, None] * jp.hstack(slot)).reshape(-1))
219242
adrs.append(
@@ -586,6 +609,11 @@ def _cvel_offset(otype, oid):
586609
elif sensor_type == SensorType.SUBTREEANGMOM:
587610
sensor = d._impl.subtree_angmom[objid] # pyrefly: ignore[missing-attribute]
588611
adr = (adr[:, None] + np.arange(3)[None]).reshape(-1)
612+
elif sensor_type == SensorType.CONTACT:
613+
s, a = _sensor_contact(m, d, idx)
614+
sensors.extend(s)
615+
adrs.extend(a)
616+
continue
589617
else:
590618
# TODO(taylorhowell): raise error after adding sensor check to io.py
591619
continue # unsupported sensor type

‎mjx/mujoco/mjx/_src/sensor_test.py‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -196,6 +196,52 @@ def test_sensor_contact(self, geom):
196196

197197
_assert_eq(dx.sensordata, d.sensordata, 'sensordata')
198198

199+
def test_sensor_contact_velocity(self):
200+
"""Tests contact sensor linvel and angvel with nonzero velocities."""
201+
contact_sensors = ''
202+
for data in ['linvel', 'angvel', 'found linvel angvel', 'dist linvel']:
203+
for match in [
204+
'',
205+
'geom1="plane"',
206+
'geom1="sphere1"',
207+
'geom1="sphere1" geom2="sphere2"',
208+
'geom1="sphere2" geom2="sphere1"',
209+
]:
210+
for reduce in ['mindist', 'maxforce']:
211+
contact_sensors += (
212+
f'<contact {match} num="2" data="{data}" reduce="{reduce}"/>\n'
213+
)
214+
215+
m = mujoco.MjModel.from_xml_string(f"""
216+
<mujoco>
217+
<worldbody>
218+
<geom name="plane" type="plane" size="10 10 .001"/>
219+
<body pos="0 0 .09">
220+
<freejoint/>
221+
<geom name="sphere1" type="sphere" size=".1"/>
222+
</body>
223+
<body pos=".02 0 .27">
224+
<freejoint/>
225+
<geom name="sphere2" type="sphere" size=".1"/>
226+
</body>
227+
</worldbody>
228+
<sensor>
229+
{contact_sensors}
230+
</sensor>
231+
</mujoco>
232+
""")
233+
d = mujoco.MjData(m)
234+
d.qvel = [.3, -.2, .1, .5, -.4, .7, -.1, .4, -.3, -.6, .2, .8]
235+
236+
mx = mjx.put_model(m)
237+
dx = mjx.put_data(m, d)
238+
239+
mujoco.mj_forward(m, d)
240+
dx = jax.jit(mjx.forward)(mx, dx)
241+
242+
self.assertTrue(np.any(d.sensordata != 0))
243+
_assert_eq(dx.sensordata, d.sensordata, 'sensordata')
244+
199245
def test_unsupported_sensor(self):
200246
"""Tests unsupported sensor raises error."""
201247
m = mujoco.MjModel.from_xml_string("""

‎mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -360,6 +360,11 @@ def put_model(mjm: mujoco.MjModel, batch_sizes: dict[str, int] | None = None) ->
360360
if (mjm.sensor_plugin != -1).any():
361361
raise NotImplementedError("Sensor plugins not supported.")
362362

363+
# TODO(team): contact sensor linvel and angvel (mjSTAGE_VEL)
364+
contact_dataspec = mjm.sensor_intprm[mjm.sensor_type == mujoco.mjtSensor.mjSENS_CONTACT, 0]
365+
if (contact_dataspec & ((1 << 7) | (1 << 8))).any():
366+
raise NotImplementedError("Contact sensor linvel and angvel not supported.")
367+
363368
if mjm.nflex > 0:
364369
for fi in range(mjm.nflex):
365370
if abs(mjm.flex_interp[fi]) == 2:

‎python/mujoco/introspect/enums.py‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -448,7 +448,9 @@
448448
('mjCONDATA_POS', 4),
449449
('mjCONDATA_NORMAL', 5),
450450
('mjCONDATA_TANGENT', 6),
451-
('mjNCONDATA', 7),
451+
('mjCONDATA_LINVEL', 7),
452+
('mjCONDATA_ANGVEL', 8),
453+
('mjNCONDATA', 9),
452454
]),
453455
)),
454456
('mjtRayDataField',

‎src/engine/engine_sensor.c‎

Lines changed: 66 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include "engine/engine_callback.h"
2525
#include "engine/engine_collision_convex.h"
2626
#include "engine/engine_collision_sdf.h"
27+
#include "engine/engine_core_constraint.h"
2728
#include "engine/engine_core_smooth.h"
2829
#include "engine/engine_core_util.h"
2930
#include "engine/engine_crossplatform.h"
@@ -392,8 +393,8 @@ static int matchContact(const mjModel* m, const mjData* d, int conid,
392393

393394
// fill in output data for contact sensor for all fields
394395
// if flg_flip > 0, normal/tangent rotate 180 about frame[2]
395-
// force/torque flip-z s.t. force is equal-and-opposite in new contact frame
396-
static void copySensorData(const mjModel* m, const mjData* d,
396+
// force/torque/linvel/angvel flip-z s.t. values are equal-and-opposite in new contact frame
397+
static void copySensorData(const mjModel* m, mjData* d,
397398
mjtNum* data[mjNCONDATA], int id, int flg_flip, int nfound) {
398399
// found flag
399400
if (data[mjCONDATA_FOUND]) *data[mjCONDATA_FOUND] = nfound;
@@ -433,6 +434,65 @@ static void copySensorData(const mjModel* m, const mjData* d,
433434
mju_copy3(data[mjCONDATA_TANGENT], d->contact[id].frame+3);
434435
if (flg_flip) mju_scl3(data[mjCONDATA_TANGENT], data[mjCONDATA_TANGENT], -1);
435436
}
437+
438+
// contact linear and angular velocity
439+
if (data[mjCONDATA_LINVEL] || data[mjCONDATA_ANGVEL]) {
440+
const mjContact* con = d->contact + id;
441+
mjtNum linvel[3] = {0}, angvel[3] = {0};
442+
443+
// single body on each side: compute from cvel
444+
if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) &&
445+
(con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) {
446+
int bid[2];
447+
mjtNum vel[2][6];
448+
for (int s=0; s < 2; s++) {
449+
bid[s] = (con->geom[s] >= 0) ?
450+
m->geom_bodyid[con->geom[s]] :
451+
m->flex_vertbodyid[m->flex_vertadr[con->flex[s]] + con->vert[s]];
452+
mju_transformSpatial(vel[s], d->cvel + 6*bid[s], 0, con->pos,
453+
d->subtree_com + 3*m->body_rootid[bid[s]], NULL);
454+
}
455+
mju_sub3(angvel, vel[1], vel[0]);
456+
mju_sub3(linvel, vel[1]+3, vel[0]+3);
457+
}
458+
459+
// general flex case: compute via contact Jacobian
460+
else if (m->nv) {
461+
int nv = m->nv, issparse = mj_isSparse(m);
462+
int need_ang = (data[mjCONDATA_ANGVEL] != NULL);
463+
mj_markStack(d);
464+
mjtNum* jacdifp = mjSTACKALLOC(d, 3*nv, mjtNum);
465+
mjtNum* jacdifr = need_ang ? mjSTACKALLOC(d, 3*nv, mjtNum) : NULL;
466+
int* chain = issparse ? mjSTACKALLOC(d, nv, int) : NULL;
467+
int NV = mj_contactJacobian(m, d, con, need_ang ? 6 : 3, jacdifp, jacdifr,
468+
NULL, NULL, NULL, NULL, chain);
469+
if (NV) {
470+
if (issparse) {
471+
for (int j=0; j < NV; j++) {
472+
mjtNum qv = d->qvel[chain[j]];
473+
for (int k=0; k < 3; k++) {
474+
linvel[k] += jacdifp[k*NV+j] * qv;
475+
if (need_ang) angvel[k] += jacdifr[k*NV+j] * qv;
476+
}
477+
}
478+
} else {
479+
mju_mulMatVec(linvel, jacdifp, d->qvel, 3, nv);
480+
if (need_ang) mju_mulMatVec(angvel, jacdifr, d->qvel, 3, nv);
481+
}
482+
}
483+
mj_freeStack(d);
484+
}
485+
486+
// rotate to contact frame and apply flip
487+
if (data[mjCONDATA_LINVEL]) {
488+
mju_mulMatVec3(data[mjCONDATA_LINVEL], con->frame, linvel);
489+
if (flg_flip) data[mjCONDATA_LINVEL][2] *= -1;
490+
}
491+
if (data[mjCONDATA_ANGVEL]) {
492+
mju_mulMatVec3(data[mjCONDATA_ANGVEL], con->frame, angvel);
493+
if (flg_flip) data[mjCONDATA_ANGVEL][2] *= -1;
494+
}
495+
}
436496
}
437497

438498

@@ -1116,6 +1176,10 @@ static void mj_computeSensorVel(const mjModel* m, mjData* d, int i, mjtNum* sens
11161176
mju_copy3(sensordata, d->subtree_angmom+3*objid);
11171177
break;
11181178

1179+
case mjSENS_CONTACT: // contact
1180+
compute_contact_sensor(m, d, i, sensordata);
1181+
break;
1182+
11191183
case mjSENS_TACTILE: // tactile
11201184
{
11211185
mj_markStack(d);

‎src/engine/engine_support.c‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,9 @@ const int mjCONDATA_SIZE[mjNCONDATA] = {
119119
1, // mjCONDATA_DIST
120120
3, // mjCONDATA_POS
121121
3, // mjCONDATA_NORMAL
122-
3 // mjCONDATA_TANGENT
122+
3, // mjCONDATA_TANGENT
123+
3, // mjCONDATA_LINVEL
124+
3 // mjCONDATA_ANGVEL
123125
};
124126

125127

0 commit comments

Comments
 (0)
⌂ 换网址