|
24 | 24 | #include "engine/engine_callback.h" |
25 | 25 | #include "engine/engine_collision_convex.h" |
26 | 26 | #include "engine/engine_collision_sdf.h" |
| 27 | +#include "engine/engine_core_constraint.h" |
27 | 28 | #include "engine/engine_core_smooth.h" |
28 | 29 | #include "engine/engine_core_util.h" |
29 | 30 | #include "engine/engine_crossplatform.h" |
@@ -392,8 +393,8 @@ static int matchContact(const mjModel* m, const mjData* d, int conid, |
392 | 393 |
|
393 | 394 | // fill in output data for contact sensor for all fields |
394 | 395 | // 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, |
397 | 398 | mjtNum* data[mjNCONDATA], int id, int flg_flip, int nfound) { |
398 | 399 | // found flag |
399 | 400 | if (data[mjCONDATA_FOUND]) *data[mjCONDATA_FOUND] = nfound; |
@@ -433,6 +434,65 @@ static void copySensorData(const mjModel* m, const mjData* d, |
433 | 434 | mju_copy3(data[mjCONDATA_TANGENT], d->contact[id].frame+3); |
434 | 435 | if (flg_flip) mju_scl3(data[mjCONDATA_TANGENT], data[mjCONDATA_TANGENT], -1); |
435 | 436 | } |
| 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 | + } |
436 | 496 | } |
437 | 497 |
|
438 | 498 |
|
@@ -1116,6 +1176,10 @@ static void mj_computeSensorVel(const mjModel* m, mjData* d, int i, mjtNum* sens |
1116 | 1176 | mju_copy3(sensordata, d->subtree_angmom+3*objid); |
1117 | 1177 | break; |
1118 | 1178 |
|
| 1179 | + case mjSENS_CONTACT: // contact |
| 1180 | + compute_contact_sensor(m, d, i, sensordata); |
| 1181 | + break; |
| 1182 | + |
1119 | 1183 | case mjSENS_TACTILE: // tactile |
1120 | 1184 | { |
1121 | 1185 | mj_markStack(d); |
|
0 commit comments