chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Clean up tendon wrapping code.

Browse Source 
PiperOrigin-RevId: 675185000
Change-Id: I3d93d7ce65b617c4869eb002dfe9440efa9abf66
This commit is contained in:
Yuval Tassa
2024-09-16 09:45:40 -07:00
committed by Copybara-Service
parent ebcffaf121
commit 48fee9487e
3 changed files with 85 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+1 -1
View File
@@ -755,7 +755,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
wlen = mju_wrap(wpnt+3, d->site_xpos+3*id0, d->site_xpos+3*id1,
d->geom_xpos+3*idw, d->geom_xmat+9*idw, m->geom_size+3*idw, tpw,
d->geom_xpos+3*idw, d->geom_xmat+9*idw, m->geom_size[3*idw], tpw,
(sideid >= 0 ? d->site_xpos+3*sideid : 0));
} else {
tpw = mjWRAP_NONE;
src/engine/engine_util_misc.c
+83 -75
View File
@@ -52,89 +52,86 @@ static mjtByte is_intersect(const mjtNum* p1, const mjtNum* p2,
// curve length along circle
static mjtNum length_circle(const mjtNum* p0, const mjtNum* p1, int ind, mjtNum rad) {
mjtNum angle, cross;
static mjtNum length_circle(const mjtNum* p0, const mjtNum* p1, int ind, mjtNum radius) {
mjtNum p0n[2] = {p0[0], p0[1]};
mjtNum p1n[2] = {p1[0], p1[1]};
// compute angle between 0 and pi
mju_normalize(p0n, 2);
mju_normalize(p1n, 2);
angle = mju_acos(mju_dot(p0n, p1n, 2));
mjtNum angle = mju_acos(mju_dot(p0n, p1n, 2));
// flip if necessary
cross = p0[1]*p1[0]-p0[0]*p1[1];
mjtNum cross = p0[1]*p1[0]-p0[0]*p1[1];
if ((cross > 0 && ind) || (cross < 0 && !ind)) {
angle = 2*mjPI - angle;
}
return rad*angle;
return radius*angle;
}
// 2D circle wrap
// input: pair of 2D points in d[4], optional 2D side point in sd[2], radius
// output: pair of 2D points in pnt[4], length of circular wrap or -1
static mjtNum wrap_circle(mjtNum* pnt, const mjtNum* d, const mjtNum* sd, mjtNum rad) {
mjtNum sqlen0 = d[0]*d[0]+d[1]*d[1];
mjtNum sqlen1 = d[2]*d[2]+d[3]*d[3];
mjtNum sqrad = rad*rad;
mjtNum dif[2] = {d[2]-d[0], d[3]-d[1]};
mjtNum a, tmp[2], dd, sqrt0, sqrt1;
mjtNum sol[2][2][2], good[2], wlen;
int sgn;
// input: pair of 2D endpoints in end[4], optional 2D side point in side[2], radius
// output: return length of circular wrap or -1
// pair of 2D points in pnt[4]
static mjtNum wrap_circle(mjtNum pnt[4], const mjtNum end[4], const mjtNum* side, mjtNum radius) {
mjtNum sqlen0 = end[0]*end[0] + end[1]*end[1];
mjtNum sqlen1 = end[2]*end[2] + end[3]*end[3];
mjtNum sqrad = radius*radius;
// either point inside circle or circle too small: no wrap
if (sqlen0 < sqrad || sqlen1 < sqrad || rad < mjMINVAL) {
if (sqlen0 < sqrad || sqlen1 < sqrad || radius < mjMINVAL) {
return -1;
}
// points too close: no wrap
dd = dif[0]*dif[0] + dif[1]*dif[1];
mjtNum dif[2] = {end[2]-end[0], end[3]-end[1]};
mjtNum dd = dif[0]*dif[0] + dif[1]*dif[1];
if (dd < mjMINVAL) {
return -1;
}
// find nearest point on line segment to origin: a*dif + d0
a = -(dif[0]*d[0]+dif[1]*d[1])/dd;
mjtNum a = -(dif[0]*end[0]+dif[1]*end[1])/dd;
if (a < 0) {
a = 0;
} else if (a > 1) {
a = 1;
}
tmp[0] = a*dif[0] + d[0];
tmp[1] = a*dif[1] + d[1];
// check for intersection and side
if (tmp[0]*tmp[0]+tmp[1]*tmp[1] > sqrad && (!sd || mju_dot(sd, tmp, 2) >= 0)) {
mjtNum tmp[2] = {a*dif[0] + end[0], a*dif[1] + end[1]};
if (tmp[0]*tmp[0]+tmp[1]*tmp[1] > sqrad && (!side || mju_dot(side, tmp, 2) >= 0)) {
return -1;
}
// construct the two solutions, compute goodness
mjtNum sol[2][2][2], good[2];
for (int i=0; i < 2; i++) {
sqrt0 = mju_sqrt(sqlen0 - sqrad);
sqrt1 = mju_sqrt(sqlen1 - sqrad);
mjtNum sqrt0 = mju_sqrt(sqlen0 - sqrad);
mjtNum sqrt1 = mju_sqrt(sqlen1 - sqrad);
sgn = (i == 0 ? 1 : -1);
int sgn = (i == 0 ? 1 : -1);
sol[i][0][0] = (d[0]*sqrad + sgn*rad*d[1]*sqrt0)/sqlen0;
sol[i][0][1] = (d[1]*sqrad - sgn*rad*d[0]*sqrt0)/sqlen0;
sol[i][1][0] = (d[2]*sqrad - sgn*rad*d[3]*sqrt1)/sqlen1;
sol[i][1][1] = (d[3]*sqrad + sgn*rad*d[2]*sqrt1)/sqlen1;
sol[i][0][0] = (end[0]*sqrad + sgn*radius*end[1]*sqrt0)/sqlen0;
sol[i][0][1] = (end[1]*sqrad - sgn*radius*end[0]*sqrt0)/sqlen0;
sol[i][1][0] = (end[2]*sqrad - sgn*radius*end[3]*sqrt1)/sqlen1;
sol[i][1][1] = (end[3]*sqrad + sgn*radius*end[2]*sqrt1)/sqlen1;
// goodness: close to sd, or shorter path
if (sd) {
if (side) {
mju_add(tmp, sol[i][0], sol[i][1], 2);
mju_normalize(tmp, 2);
good[i] = mju_dot(tmp, sd, 2);
good[i] = mju_dot(tmp, side, 2);
} else {
mju_sub(tmp, sol[i][0], sol[i][1], 2);
good[i] = -mju_dot(tmp, tmp, 2);
}
// penalize for intersection
if (is_intersect(d, sol[i][0], d+2, sol[i][1])) {
if (is_intersect(end, sol[i][0], end+2, sol[i][1])) {
good[i] = -10000;
}
}
@@ -147,63 +144,62 @@ static mjtNum wrap_circle(mjtNum* pnt, const mjtNum* d, const mjtNum* sd, mjtNum
pnt[3] = sol[i][1][1];
// check for intersection
if (is_intersect(d, pnt, d+2, pnt+2)) {
if (is_intersect(end, pnt, end+2, pnt+2)) {
return -1;
}
// compute curve length
wlen = length_circle(sol[i][0], sol[i][1], i, rad);
return wlen;
// return curve length
return length_circle(sol[i][0], sol[i][1], i, radius);
}
// 2D inside wrap
// input: pair of 2D points in d[4], radius
// input: pair of 2D endpoints in end[4], radius
// output: pair of 2D points in pnt[4]; return 0 if wrap, -1 if no wrap
static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) {
static mjtNum wrap_inside(mjtNum pnt[4], const mjtNum end[4], mjtNum radius) {
// algorithm parameters
const int maxiter = 20;
const mjtNum zinit = 1 - 1e-7;
const mjtNum tolerance = 1e-6;
// constants
mjtNum len0 = mju_norm(d, 2);
mjtNum len1 = mju_norm(d+2, 2);
mjtNum dif[2] = {d[2]-d[0], d[3]-d[1]};
mjtNum len0 = mju_norm(end, 2);
mjtNum len1 = mju_norm(end+2, 2);
mjtNum dif[2] = {end[2]-end[0], end[3]-end[1]};
mjtNum dd = dif[0]*dif[0] + dif[1]*dif[1];
// either point inside circle or circle too small: no wrap
if (len0 <= rad || len1 <= rad || rad < mjMINVAL || len0 < mjMINVAL || len1 < mjMINVAL) {
if (len0 <= radius || len1 <= radius || radius < mjMINVAL || len0 < mjMINVAL || len1 < mjMINVAL) {
return -1;
}
// segment-circle intersection: no wrap
if (dd > mjMINVAL) {
// find nearest point on line segment to origin: d0 + a*dif
mjtNum a = -(dif[0]*d[0]+dif[1]*d[1])/dd;
mjtNum a = -(dif[0]*end[0] + dif[1]*end[1]) / dd;
// in segment
if (a > 0 && a < 1) {
mjtNum tmp[2];
mju_addScl(tmp, d, dif, a, 2);
if (mju_norm(tmp, 2) <= rad) {
mju_addScl(tmp, end, dif, a, 2);
if (mju_norm(tmp, 2) <= radius) {
return -1;
}
}
}
// prepare default in case of numerical failure: average
pnt[0] = 0.5*(d[0] + d[2]);
pnt[1] = 0.5*(d[1] + d[3]);
pnt[0] = 0.5*(end[0] + end[2]);
pnt[1] = 0.5*(end[1] + end[3]);
mju_normalize(pnt, 2);
mju_scl(pnt, pnt, rad, 2);
mju_scl(pnt, pnt, radius, 2);
pnt[2] = pnt[0];
pnt[3] = pnt[1];
// compute function parameters: asin(A*z) + asin(B*z) - 2*asin(z) + G = 0
mjtNum A = rad/len0;
mjtNum B = rad/len1;
mjtNum A = radius/len0;
mjtNum B = radius/len1;
mjtNum cosG = (len0*len0 + len1*len1 - dd) / (2*len0*len1);
if (cosG < -1+mjMINVAL) {
return -1;
@@ -260,16 +256,16 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) {
// finalize: rotation by ang from vec = a or b, depending on cross(a,b) sign
mjtNum vec[2];
mjtNum ang;
if (d[0]*d[3] - d[1]*d[2] > 0) {
mju_copy(vec, d, 2);
if (end[0]*end[3] - end[1]*end[2] > 0) {
mju_copy(vec, end, 2);
ang = mju_asin(z) - mju_asin(A*z);
} else {
mju_copy(vec, d+2, 2);
mju_copy(vec, end+2, 2);
ang = mju_asin(z) - mju_asin(B*z);
}
mju_normalize(vec, 2);
pnt[0] = rad*(mju_cos(ang)*vec[0] - mju_sin(ang)*vec[1]);
pnt[1] = rad*(mju_sin(ang)*vec[0] + mju_cos(ang)*vec[1]);
pnt[0] = radius*(mju_cos(ang)*vec[0] - mju_sin(ang)*vec[1]);
pnt[1] = radius*(mju_sin(ang)*vec[0] + mju_cos(ang)*vec[1]);
pnt[2] = pnt[0];
pnt[3] = pnt[1];
@@ -279,23 +275,27 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) {
// wrap tendons around spheres and cylinders
mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
const mjtNum* xpos, const mjtNum* xmat, const mjtNum* size,
int type, const mjtNum* side) {
mjtNum tmp[3], normal[3], axis[2][3], p[2][3], s[3], d[4], sd[2], pnt[4];
mjtNum res[6], wlen, height;
mjtNum L0, L1;
// input: x0, x1: pair of 3D endpoints
// xpos, xmat, radius: position, orientation and radius of geom
// type: wrap type (mjtWrap)
// side: 3D position of sidesite
// output: return wrap length, -1 if no wrap
// wpnt: pair of 3D wrap points
mjtNum mju_wrap(mjtNum wpnt[6], const mjtNum x0[3], const mjtNum x1[3],
const mjtNum xpos[3], const mjtNum xmat[9], mjtNum radius,
int type, const mjtNum side[3]) {
// check object type; SHOULD NOT OCCUR
if (type != mjWRAP_SPHERE && type != mjWRAP_CYLINDER) {
mjERROR("unknown wrapping object type %d", type);
}
// map sites to wrap object's local frame
mjtNum tmp[3];
mju_sub3(tmp, x0, xpos);
mju_mulMatTVec(p[0], xmat, tmp, 3, 3);
mjtNum p[2][3];
mju_mulMatTVec3(p[0], xmat, tmp);
mju_sub3(tmp, x1, xpos);
mju_mulMatTVec(p[1], xmat, tmp, 3, 3);
mju_mulMatTVec3(p[1], xmat, tmp);
// too close to origin: return
if (mju_norm3(p[0]) < mjMINVAL || mju_norm3(p[1]) < mjMINVAL) {
@@ -303,12 +303,14 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
}
// construct 2D frame for circle wrap
mjtNum axis[2][3];
if (type == mjWRAP_SPHERE) {
// 1st axis = p0
mju_copy3(axis[0], p[0]);
mju_normalize3(axis[0]);
// normal to p0-0-p1 plane = cross(p0, p1)
mjtNum normal[3];
mju_cross(normal, p[0], p[1]);
mjtNum nrm = mju_normalize3(normal);
@@ -350,10 +352,13 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
}
// project points in 2D frame: p => d
mjtNum s[3], d[4], sd[2];
d[0] = mju_dot3(p[0], axis[0]);
d[1] = mju_dot3(p[0], axis[1]);
d[2] = mju_dot3(p[1], axis[0]);
d[3] = mju_dot3(p[1], axis[1]);
// handle sidesite
if (side) {
// side point: apply same projection as x0, x1
mju_sub3(tmp, side, xpos);
@@ -363,25 +368,28 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
sd[0] = mju_dot3(s, axis[0]);
sd[1] = mju_dot3(s, axis[1]);
mju_normalize(sd, 2);
mju_scl(sd, sd, size[0], 2);
mju_scl(sd, sd, radius, 2);
}
// apply inside wrap
if (side && mju_norm3(s) < size[0]) {
wlen = wrap_inside(pnt, d, size[0]);
mjtNum wlen;
mjtNum pnt[4];
if (side && mju_norm3(s) < radius) {
wlen = wrap_inside(pnt, d, radius);
}
// apply circle wrap
else {
wlen = wrap_circle(pnt, d, (side ? sd : 0), size[0]);
wlen = wrap_circle(pnt, d, (side ? sd : NULL), radius);
}
// no wrap
// no wrap: return
if (wlen < 0) {
return -1;
}
// reconstruct 3D points in local frame: res
mjtNum res[6];
for (int i=0; i < 2; i++) {
// res = axis0*d0 + axis1*d1
mju_scl3(res+3*i, axis[0], pnt[2*i]);
@@ -392,19 +400,19 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
// cylinder: correct along z
if (type == mjWRAP_CYLINDER) {
// set vertical coordinates
L0 = mju_sqrt((p[0][0]-res[0])*(p[0][0]-res[0]) + (p[0][1]-res[1])*(p[0][1]-res[1]));
L1 = mju_sqrt((p[1][0]-res[3])*(p[1][0]-res[3]) + (p[1][1]-res[4])*(p[1][1]-res[4]));
res[2] = p[0][2] + (p[1][2]-p[0][2])*L0/(L0+wlen+L1);
res[5] = p[0][2] + (p[1][2]-p[0][2])*(L0+wlen)/(L0+wlen+L1);
mjtNum L0 = mju_sqrt((p[0][0]-res[0])*(p[0][0]-res[0]) + (p[0][1]-res[1])*(p[0][1]-res[1]));
mjtNum L1 = mju_sqrt((p[1][0]-res[3])*(p[1][0]-res[3]) + (p[1][1]-res[4])*(p[1][1]-res[4]));
res[2] = p[0][2] + (p[1][2] - p[0][2])*L0 / (L0+wlen+L1);
res[5] = p[0][2] + (p[1][2] - p[0][2])*(L0+wlen) / (L0+wlen+L1);
// correct wlen for height
height = mju_abs(res[5] - res[2]);
mjtNum height = mju_abs(res[5] - res[2]);
wlen = mju_sqrt(wlen*wlen + height*height);
}
// map back to global frame: wpnt
mju_mulMatVec(wpnt, xmat, res, 3, 3);
mju_mulMatVec(wpnt+3, xmat, res+3, 3, 3);
mju_mulMatVec3(wpnt, xmat, res);
mju_mulMatVec3(wpnt+3, xmat, res+3);
mju_addTo3(wpnt, xpos);
mju_addTo3(wpnt+3, xpos);
src/engine/engine_util_misc.h
+1 -1
View File
@@ -30,7 +30,7 @@ extern "C" {
// wrap tendons around spheres and cylinders
mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
const mjtNum* xpos, const mjtNum* xmat, const mjtNum* size,
const mjtNum* xpos, const mjtNum* xmat, mjtNum radius,
int type, const mjtNum* side);
// normalized muscle length-gain curve