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

Polynomial stiffness and damping https://youtu.be/aKa3ZlEF9_Y

Browse Source 
PiperOrigin-RevId: 884607673
Change-Id: If8088dbf37fed1055304778a7eb84dec52cba920
This commit is contained in:
Yuval Tassa
2026-03-16 13:24:44 -07:00
committed by Copybara-Service
parent aec1b45dce
commit efae9157a7
38 changed files with 1093 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_derivative.c
+6 -2
View File
@@ -1733,7 +1733,10 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
int nv_awake = sleep_filter ? d->nv_awake : nv;
for (int j = 0; j < nv_awake; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j;
d->qDeriv[m->D_rowadr[i] + m->D_diag[i]] -= m->dof_damping[i];
mjtNum v = d->qvel[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
int adr = m->D_rowadr[i] + m->D_diag[i];
d->qDeriv[adr] -= mjd_xPolyForce(m->dof_damping[i], poly, v, mjNPOLY, 1);
}
// flex edge damping
@@ -1771,7 +1774,8 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
if (treenum == 2 && !d->tree_awake[id1] && !d->tree_awake[id2]) continue;
}
mjtNum B = -m->tendon_damping[i];
mjtNum v = d->ten_velocity[i];
mjtNum B = -mjd_xPolyForce(m->tendon_damping[i], m->tendon_dampingpoly+mjNPOLY*i, v, mjNPOLY, 1);
if (!B) {
continue;
src/engine/engine_forward.c
+5 -2
View File
@@ -953,7 +953,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
if (!mjDISABLED(mjDSBL_EULERDAMP) && !mjDISABLED(mjDSBL_DAMPER)) {
for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : v;
if (m->dof_damping[i] > 0) {
if (m->dof_damping[i] > 0 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) {
dof_damping = 1;
break;
}
@@ -982,7 +982,10 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// qH += h*diag(B)
for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : v;
d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
mjtNum qv = d->qvel[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
mjtNum damp_deriv = mjd_xPolyForce(m->dof_damping[i], poly, qv, mjNPOLY, 1);
d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * damp_deriv;
}
// factorize in-place
src/engine/engine_inverse.c
+5 -2
View File
@@ -92,7 +92,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
dof_damping = 0;
if (!mjDISABLED(mjDSBL_EULERDAMP)) {
for (int i=0; i < nv; i++) {
if (m->dof_damping[i] > 0) {
if (m->dof_damping[i] > 0 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) {
dof_damping = 1;
break;
}
@@ -108,7 +108,10 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
// set qfrc = (M + h*diag(B)) * qacc
mj_mulM(m, d, qfrc, qacc);
for (int i=0; i < nv; i++) {
qfrc[i] += m->opt.timestep * m->dof_damping[i] * d->qacc[i];
mjtNum v = d->qvel[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
mjtNum damp_deriv = mjd_xPolyForce(m->dof_damping[i], poly, v, mjNPOLY, 1);
qfrc[i] += m->opt.timestep * damp_deriv * d->qacc[i];
}
break;
src/engine/engine_passive.c
+31 -22
View File
@@ -130,9 +130,9 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
int jnt_end = jnt_start + m->body_jntnum[i];
for (int j=jnt_start; j < jnt_end; j++) {
mjtNum stiffness = m->jnt_stiffness[j];
const mjtNum* spoly = m->jnt_stiffnesspoly + mjNPOLY*j;
// disabled : nothing to do
if (stiffness == 0) {
if (stiffness == 0 && mju_isZero(spoly, mjNPOLY)) {
continue;
}
@@ -142,9 +142,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
// apply force
d->qfrc_spring[dadr+0] = -stiffness*(d->qpos[padr+0] - m->qpos_spring[padr+0]);
d->qfrc_spring[dadr+1] = -stiffness*(d->qpos[padr+1] - m->qpos_spring[padr+1]);
d->qfrc_spring[dadr+2] = -stiffness*(d->qpos[padr+2] - m->qpos_spring[padr+2]);
{
mjtNum dif[3];
mji_sub3(dif, d->qpos+padr, m->qpos_spring+padr);
mjtNum r = mju_norm3(dif);
mjtNum k = mju_polyForce(stiffness, spoly, r, mjNPOLY, 0);
mji_addToScl3(d->qfrc_spring + dadr, dif, -k);
}
// continue with rotations
dadr += 3;
@@ -158,18 +162,21 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mji_copy4(quat, d->qpos+padr);
mju_normalize4(quat);
mji_subQuat(dif, quat, m->qpos_spring + padr);
mjtNum r = mju_norm3(dif);
mjtNum k = mju_polyForce(stiffness, spoly, r, mjNPOLY, 0);
// apply torque
d->qfrc_spring[dadr+0] = -stiffness*dif[0];
d->qfrc_spring[dadr+1] = -stiffness*dif[1];
d->qfrc_spring[dadr+2] = -stiffness*dif[2];
mji_addToScl3(d->qfrc_spring + dadr, dif, -k);
}
break;
case mjJNT_SLIDE:
case mjJNT_HINGE:
// apply force or torque
d->qfrc_spring[dadr] = -stiffness*(d->qpos[padr] - m->qpos_spring[padr]);
{
// apply force or torque
mjtNum x = d->qpos[padr] - m->qpos_spring[padr];
d->qfrc_spring[dadr] = -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0);
}
break;
}
}
@@ -182,8 +189,10 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
for (int j = 0; j < nv_awake; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j;
mjtNum damping = m->dof_damping[i];
if (damping != 0) {
d->qfrc_damper[i] = -damping*d->qvel[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
if (damping != 0 || !mju_isZero(poly, mjNPOLY)) {
mjtNum v = d->qvel[i];
d->qfrc_damper[i] = -v * mju_polyForce(damping, poly, v, mjNPOLY, 1);
}
}
}
@@ -341,7 +350,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mjtNum gradient[6][2][3];
GradSquaredLengths(gradient, xpos, vert, edges[dim-2], nedge);
// we add generalized Rayleigh damping as decribed in Section 5.2 of
// we add generalized Rayleigh damping as described in Section 5.2 of
// Kharevych et al., "Geometric, Variational Integrators for Computer
// Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf
@@ -450,10 +459,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
}
mjtNum stiffness = m->tendon_stiffness[i] * has_spring;
const mjtNum* spoly = m->tendon_stiffnesspoly + mjNPOLY*i;
mjtNum damping = m->tendon_damping[i] * has_damping;
const mjtNum* dpoly = m->tendon_dampingpoly + mjNPOLY*i;
// disabled : nothing to do
if (stiffness == 0 && damping == 0) {
if (stiffness == 0 && mju_isZero(spoly, mjNPOLY) &&
damping == 0 && mju_isZero(dpoly, mjNPOLY)) {
continue;
}
@@ -461,15 +473,12 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mjtNum length = d->ten_length[i];
mjtNum lower = m->tendon_lengthspring[2*i];
mjtNum upper = m->tendon_lengthspring[2*i+1];
mjtNum frc_spring = 0;
if (length > upper) {
frc_spring = stiffness * (upper - length);
} else if (length < lower) {
frc_spring = stiffness * (lower - length);
}
mjtNum x = (length > upper) ? length - upper : (length < lower) ? length - lower : 0;
mjtNum frc_spring = has_spring ? -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0) : 0;
// compute damper linear force along tendon
mjtNum frc_damper = -damping * d->ten_velocity[i];
// compute damper force along tendon
mjtNum v = d->ten_velocity[i];
mjtNum frc_damper = has_damping ? -v * mju_polyForce(damping, dpoly, v, mjNPOLY, 1) : 0;
// transform to joint torque, add to qfrc_{spring, damper}
if (frc_spring || frc_damper) {
src/engine/engine_sensor.c
+24 -25
View File
@@ -1637,7 +1637,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// position-dependent energy (potential)
void mj_energyPos(const mjModel* m, mjData* d) {
int padr;
mjtNum dif[3], quat[4], stiffness;
mjtNum dif[3], quat[4], stiffness, x;
// init potential energy: -sum_i body(i).mass * mju_dot(body(i).pos, gravity)
d->energy[0] = 0;
@@ -1659,7 +1659,8 @@ void mj_energyPos(const mjModel* m, mjData* d) {
int jnt_end = jnt_start + m->body_jntnum[b];
for (int j=jnt_start; j < jnt_end; j++) {
stiffness = m->jnt_stiffness[j];
if (stiffness == 0) {
const mjtNum* poly = m->jnt_stiffnesspoly + mjNPOLY*j;
if (stiffness == 0 && mju_isZero(poly, mjNPOLY)) {
continue;
}
padr = m->jnt_qposadr[j];
@@ -1667,8 +1668,8 @@ void mj_energyPos(const mjModel* m, mjData* d) {
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr);
d->energy[0] += 0.5 * stiffness * mju_dot3(dif, dif);
x = mju_norm3(dif);
d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0);
// continue with rotations
padr += 3;
mjFALLTHROUGH;
@@ -1678,14 +1679,15 @@ void mj_energyPos(const mjModel* m, mjData* d) {
mju_copy4(quat, d->qpos+padr);
mju_normalize4(quat);
mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr);
d->energy[0] += 0.5 * stiffness * mju_dot3(dif, dif);
x = mju_norm3(dif);
d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0);
break;
case mjJNT_SLIDE:
case mjJNT_HINGE:
d->energy[0] += 0.5 * stiffness *
(d->qpos[padr] - m->qpos_spring[padr]) *
(d->qpos[padr] - m->qpos_spring[padr]);
x = d->qpos[padr] - m->qpos_spring[padr];
d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0);
break;
}
}
@@ -1695,25 +1697,22 @@ void mj_energyPos(const mjModel* m, mjData* d) {
// add tendon-level springs
if (!mjDISABLED(mjDSBL_SPRING)) {
for (int i=0; i < m->ntendon; i++) {
// skip sleeping or static tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) {
continue;
}
stiffness = m->tendon_stiffness[i];
mjtNum length = d->ten_length[i];
mjtNum displacement = 0;
// compute spring displacement
mjtNum lower = m->tendon_lengthspring[2*i];
mjtNum upper = m->tendon_lengthspring[2*i+1];
if (length > upper) {
displacement = upper - length;
} else if (length < lower) {
displacement = lower - length;
// skip sleeping or static tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) {
continue;
}
d->energy[0] += 0.5*stiffness*displacement*displacement;
stiffness = m->tendon_stiffness[i];
const mjtNum* poly = m->tendon_stiffnesspoly + mjNPOLY*i;
mjtNum length = d->ten_length[i];
// compute spring displacement x
mjtNum lower = m->tendon_lengthspring[2*i];
mjtNum upper = m->tendon_lengthspring[2*i+1];
x = (length > upper) ? length - upper : (length < lower) ? length - lower : 0;
// add potential energy
d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0);
}
}
src/engine/engine_setconst.c
+3 -1
View File
@@ -211,7 +211,9 @@ static void setFixed(mjModel* m, mjData* d) {
}
// tendon spans 2 trees and has no stiffness or damping: skip
if (treenum == 2 && m->tendon_stiffness[i] == 0 && m->tendon_damping[i] == 0) {
if (treenum == 2 &&
m->tendon_stiffness[i] == 0 && mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY) &&
m->tendon_damping[i] == 0 && mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY)) {
continue;
}
src/engine/engine_util_misc.c
+47
View File
@@ -1884,6 +1884,53 @@ char* mju_strncpy(char *dst, const char *src, int n) {
}
// polynomial force coefficient: force = -x * mju_polyForce(...)
// flg_odd=0: linear + poly[0]*x + poly[1]*x^2 + ...
// flg_odd=1: linear + poly[0]*|x| + poly[1]*x^2 + ... (p is even, p*x is odd)
mjtNum mju_polyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd) {
x = flg_odd ? mju_abs(x) : x;
mjtNum res = linear;
mjtNum xpow = 1;
for (int i=0; i < n; i++) {
xpow *= x;
res += poly[i] * xpow;
}
return res;
}
// derivative of (x * mju_polyForce) w.r.t. x
mjtNum mjd_xPolyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd) {
x = flg_odd ? mju_abs(x) : x;
mjtNum res = linear;
mjtNum xpow = 1;
for (int i=0; i < n; i++) {
xpow *= x;
res += (i+2) * poly[i] * xpow;
}
return res;
}
// potential energy: integral from 0 to x of mju_polyForce(t) * t dt
mjtNum mju_polyPotential(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd) {
x = flg_odd ? mju_abs(x) : x;
mjtNum res = 0.5 * linear * (x * x);
mjtNum xpow = x;
for (int i=0; i < n; i++) {
xpow *= x;
res += poly[i] / (i+3) * (xpow * x);
}
return res;
}
// sigmoid function over 0<=x<=1 using quintic polynomial
mjtNum mju_sigmoid(mjtNum x) {
// fast return
src/engine/engine_util_misc.h
+11
View File
@@ -239,6 +239,17 @@ MJAPI mjtNum mju_Halton(int index, int base);
// call strncpy, then set dst[n-1] = 0
MJAPI char* mju_strncpy(char *dst, const char *src, int n);
// polynomial force coefficient: force = -mju_polyForce(...) * x
// flg_odd=0: linear + poly[0]*x + poly[1]*x^2 + ...
// flg_odd=1: linear + poly[0]*|x| + poly[1]*x^2 + ...
MJAPI mjtNum mju_polyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd);
// derivative of (mju_polyForce * x) w.r.t. x
MJAPI mjtNum mjd_xPolyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd);
// potential energy: integral from 0 to x of mju_polyForce * t dt
MJAPI mjtNum mju_polyPotential(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd);
// sigmoid function over 0<=x<=1 using quintic polynomial
MJAPI mjtNum mju_sigmoid(mjtNum x);
src/engine/engine_vis_visualize.c
+8 -3
View File
@@ -1055,9 +1055,12 @@ static void addSpatialTendonGeoms(const mjModel* m, mjData* d, const mjvOption*
continue;
}
int has_stiffness = m->tendon_stiffness[i] ||
!mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY);
// tendon has a deadband spring
int limitedspring =
m->tendon_stiffness[i] > 0 && // positive stiffness
has_stiffness && // positive stiffness
m->tendon_lengthspring[2*i] == 0 && // range lower-bound is 0
m->tendon_lengthspring[2*i+1] > 0; // range upper-bound is positive
@@ -1066,18 +1069,20 @@ static void addSpatialTendonGeoms(const mjModel* m, mjData* d, const mjvOption*
mjtNum lower = m->tendon_range[2*i];
mjtNum upper = m->tendon_range[2*i + 1];
int limitedconstraint =
m->tendon_stiffness[i] == 0 && // zero stiffness
!has_stiffness && // zero stiffness
m->tendon_limited[i] == 1 && // limited length range
lower == 0 && // range lower-bound is 0
ten_length < upper; // current length is smaller than upper bound
int has_damping = m->tendon_damping[i] || !mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY);
// conditions for drawing a catenary
int draw_catenary =
!mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled
mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero
m->tendon_num[i] == 2 && // only two sites on the tendon
(limitedspring != limitedconstraint) && // either spring or constraint length limits
m->tendon_damping[i] == 0 && // no damping
!has_damping && // no damping
m->tendon_frictionloss[i] == 0; // no frictionloss
// no actuator