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
src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc
+2 -2
View File
@@ -2079,7 +2079,7 @@ class ModelWriter {
joint->solimp_friction + mjNIMP));
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcStiffness, joint->stiffness);
MjcPhysicsTokens->mjcStiffness, joint->stiffness[0]);
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcActuatorfrcrangeMin,
@@ -2116,7 +2116,7 @@ class ModelWriter {
MjcPhysicsTokens->mjcArmature, joint->armature);
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcDamping, joint->damping);
MjcPhysicsTokens->mjcDamping, joint->damping[0]);
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcFrictionloss,
src/user/user_composite.cc
+1 -1
View File
@@ -419,7 +419,7 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix,
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
jnt->type = (first && strcmp(initial.c_str(), "free") == 0) ? mjJNT_FREE : mjJNT_BALL;
jnt->damping = jnt->type == mjJNT_FREE ? 0 : jnt->damping;
if (jnt->type == mjJNT_FREE) mjuu_zerovec(jnt->damping, 1+mjNPOLY);
jnt->armature = jnt->type == mjJNT_FREE ? 0 : jnt->armature;
jnt->frictionloss = jnt->type == mjJNT_FREE ? 0 : jnt->frictionloss;
mjs_setName(jnt->element, this_joint);
src/user/user_model.cc
+19 -10
View File
@@ -2395,8 +2395,8 @@ void mjCModel::AutoSpringDamper(mjModel* m) {
mjtNum damping = 2 * inertia / std::max(mjMINVAL, timeconst);
// save stiffness and damping in the private mjsJoints
joints_[n]->stiffness = stiffness;
joints_[n]->damping = damping;
joints_[n]->stiffness[0] = stiffness;
joints_[n]->damping[0] = damping;
// assign
m->jnt_stiffness[n] = stiffness;
@@ -2805,7 +2805,8 @@ void mjCModel::CopyTree(mjModel* m) {
m->jnt_bodyid[jid] = pj->body->id;
mjuu_copyvec(m->jnt_pos+3*jid, pj->pos, 3);
mjuu_copyvec(m->jnt_axis+3*jid, pj->axis, 3);
m->jnt_stiffness[jid] = (mjtNum)pj->stiffness;
m->jnt_stiffness[jid] = (mjtNum)pj->stiffness[0];
mjuu_copyvec(m->jnt_stiffnesspoly + mjNPOLY * jid, pj->stiffness + 1, mjNPOLY);
mjuu_copyvec(m->jnt_range+2*jid, pj->range, 2);
mjuu_copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2);
mjuu_copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF);
@@ -2862,7 +2863,9 @@ void mjCModel::CopyTree(mjModel* m) {
mjuu_copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP);
m->dof_frictionloss[dofadr] = (mjtNum)pj->frictionloss;
m->dof_armature[dofadr] = (mjtNum)pj->armature;
m->dof_damping[dofadr] = (mjtNum)pj->damping;
m->dof_damping[dofadr] = (mjtNum)pj->damping[0];
mjuu_copyvec(m->dof_dampingpoly + mjNPOLY * dofadr, pj->damping + 1,
mjNPOLY);
// set dof_parentid, update body.lastdof
m->dof_parentid[dofadr] = pb->lastdof;
@@ -3805,8 +3808,10 @@ void mjCModel::CopyObjects(mjModel* m) {
m->tendon_actfrcrange[2*i] = (mjtNum)pte->actfrcrange[0];
m->tendon_actfrcrange[2*i+1] = (mjtNum)pte->actfrcrange[1];
m->tendon_margin[i] = (mjtNum)pte->margin;
m->tendon_stiffness[i] = (mjtNum)pte->stiffness;
m->tendon_damping[i] = (mjtNum)pte->damping;
m->tendon_stiffness[i] = (mjtNum)pte->stiffness[0];
mjuu_copyvec(m->tendon_stiffnesspoly + mjNPOLY * i, pte->stiffness + 1, mjNPOLY);
m->tendon_damping[i] = (mjtNum)pte->damping[0];
mjuu_copyvec(m->tendon_dampingpoly + mjNPOLY * i, pte->damping + 1, mjNPOLY);
m->tendon_armature[i] = (mjtNum)pte->armature;
m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss;
m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0];
@@ -5542,7 +5547,8 @@ bool mjCModel::CopyBack(const mjModel* m) {
// joint data
mjuu_copyvec(pj->pos, m->jnt_pos+3*i, 3);
mjuu_copyvec(pj->axis, m->jnt_axis+3*i, 3);
pj->stiffness = (double)m->jnt_stiffness[i];
pj->stiffness[0] = (double)m->jnt_stiffness[i];
mjuu_copyvec(pj->stiffness + 1, m->jnt_stiffnesspoly + mjNPOLY * i, mjNPOLY);
mjuu_copyvec(pj->range, m->jnt_range+2*i, 2);
mjuu_copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF);
mjuu_copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP);
@@ -5557,7 +5563,8 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjuu_copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF);
mjuu_copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP);
pj->armature = (double)m->dof_armature[j];
pj->damping = (double)m->dof_damping[j];
pj->damping[0] = (double)m->dof_damping[j];
mjuu_copyvec(pj->damping + 1, m->dof_dampingpoly + mjNPOLY * j, mjNPOLY);
pj->frictionloss = (double)m->dof_frictionloss[j];
}
@@ -5680,8 +5687,10 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjuu_copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4);
tendons_[i]->width = (double)m->tendon_width[i];
tendons_[i]->margin = (double)m->tendon_margin[i];
tendons_[i]->stiffness = (double)m->tendon_stiffness[i];
tendons_[i]->damping = (double)m->tendon_damping[i];
tendons_[i]->stiffness[0] = (double)m->tendon_stiffness[i];
mjuu_copyvec(tendons_[i]->stiffness + 1, m->tendon_stiffnesspoly + mjNPOLY * i, mjNPOLY);
tendons_[i]->damping[0] = (double)m->tendon_damping[i];
mjuu_copyvec(tendons_[i]->damping + 1, m->tendon_dampingpoly + mjNPOLY * i, mjNPOLY);
tendons_[i]->armature = (double)m->tendon_armature[i];
tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i];
src/xml/xml_native_reader.cc
+13 -6
View File
@@ -1815,14 +1815,18 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* joint) {
ReadAttr(elem, "pos", 3, joint->pos, text);
ReadAttr(elem, "axis", 3, joint->axis, text);
ReadAttr(elem, "springdamper", 2, joint->springdamper, text);
ReadAttr(elem, "stiffness", 1, &joint->stiffness, text);
ReadAttr(elem, "stiffness", 1+mjNPOLY, joint->stiffness, text, false, false);
ReadAttr(elem, "range", 2, joint->range, text);
ReadAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, text);
ReadAttr(elem, "margin", 1, &joint->margin, text);
ReadAttr(elem, "ref", 1, &joint->ref, text);
ReadAttr(elem, "springref", 1, &joint->springref, text);
ReadAttr(elem, "armature", 1, &joint->armature, text);
ReadAttr(elem, "damping", 1, &joint->damping, text);
ReadAttr(elem, "damping", 1+mjNPOLY, joint->damping, text, false, false);
ReadAttr(elem, "frictionloss", 1, &joint->frictionloss, text);
if (MapValue(elem, "actuatorgravcomp", &n, bool_map, 2)) {
joint->actgravcomp = (n == 1);
@@ -2267,8 +2271,11 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) {
ReadAttr(elem, "range", 2, tendon->range, text);
ReadAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, text);
ReadAttr(elem, "margin", 1, &tendon->margin, text);
ReadAttr(elem, "stiffness", 1, &tendon->stiffness, text);
ReadAttr(elem, "damping", 1, &tendon->damping, text);
ReadAttr(elem, "stiffness", 1+mjNPOLY, tendon->stiffness, text, false, false);
ReadAttr(elem, "damping", 1+mjNPOLY, tendon->damping, text, false, false);
ReadAttr(elem, "armature", 1, &tendon->armature, text);
ReadAttr(elem, "frictionloss", 1, &tendon->frictionloss, text);
// read springlength, either one or two values; if one, copy to second value
@@ -2679,11 +2686,11 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsFrame* frame, c
"solreffriction", mjNREF, djoint.solref_friction, text, false, false);
ReadAttr(ejnt,
"solimpfriction", mjNIMP, djoint.solimp_friction, text, false, false);
ReadAttr(ejnt, "stiffness", 1, &djoint.stiffness, text);
ReadAttr(ejnt, "stiffness", 1, djoint.stiffness, text);
ReadAttr(ejnt, "range", 2, djoint.range, text);
ReadAttr(ejnt, "margin", 1, &djoint.margin, text);
ReadAttr(ejnt, "armature", 1, &djoint.armature, text);
ReadAttr(ejnt, "damping", 1, &djoint.damping, text);
ReadAttr(ejnt, "damping", 1, djoint.damping, text);
ReadAttr(ejnt, "frictionloss", 1, &djoint.frictionloss, text);
// advance
src/xml/xml_native_writer.cc
+24 -4
View File
@@ -395,7 +395,12 @@ void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def,
true);
WriteAttr(elem, "solimpfriction", mjNIMP, joint->solimp_friction, def->Joint().solimp_friction,
true);
WriteAttr(elem, "stiffness", 1, &joint->stiffness, &def->Joint().stiffness);
{
int nstiff = 1+mjNPOLY;
while (nstiff > 1 && joint->stiffness[nstiff-1] == 0
&& def->Joint().stiffness[nstiff-1] == 0) nstiff--;
WriteAttr(elem, "stiffness", nstiff, joint->stiffness, def->Joint().stiffness);
}
if (joint->type != mjJNT_FREE) {
WriteAttrKey(elem, "limited", TFAuto_map, 3, joint->limited, def->Joint().limited);
}
@@ -408,7 +413,12 @@ void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def,
WriteAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, def->Joint().actfrcrange);
WriteAttr(elem, "margin", 1, &joint->margin, &def->Joint().margin);
WriteAttr(elem, "armature", 1, &joint->armature, &def->Joint().armature);
WriteAttr(elem, "damping", 1, &joint->damping, &def->Joint().damping);
{
int ndamp = 1+mjNPOLY;
while (ndamp > 1 && joint->damping[ndamp-1] == 0
&& def->Joint().damping[ndamp-1] == 0) ndamp--;
WriteAttr(elem, "damping", ndamp, joint->damping, def->Joint().damping);
}
WriteAttr(elem, "frictionloss", 1, &joint->frictionloss, &def->Joint().frictionloss);
// userdata
@@ -753,8 +763,18 @@ void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def
WriteAttr(elem, "range", 2, tendon->range, def->Tendon().range);
WriteAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, def->Tendon().actfrcrange);
WriteAttr(elem, "margin", 1, &tendon->margin, &def->Tendon().margin);
WriteAttr(elem, "stiffness", 1, &tendon->stiffness, &def->Tendon().stiffness);
WriteAttr(elem, "damping", 1, &tendon->damping, &def->Tendon().damping);
{
int nstiff = 1+mjNPOLY;
while (nstiff > 1 && tendon->stiffness[nstiff-1] == 0
&& def->Tendon().stiffness[nstiff-1] == 0) nstiff--;
WriteAttr(elem, "stiffness", nstiff, tendon->stiffness, def->Tendon().stiffness);
}
{
int ndamp = 1+mjNPOLY;
while (ndamp > 1 && tendon->damping[ndamp-1] == 0
&& def->Tendon().damping[ndamp-1] == 0) ndamp--;
WriteAttr(elem, "damping", ndamp, tendon->damping, def->Tendon().damping);
}
WriteAttr(elem, "armature", 1, &tendon->armature, &def->Tendon().armature);
WriteAttr(elem, "frictionloss", 1, &tendon->frictionloss, &def->Tendon().frictionloss);
if (tendon->springlength[0] != tendon->springlength[1] ||
src/xml/xml_urdf.cc
+3 -2
View File
@@ -486,12 +486,13 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
// dynamics element
if ((elem = FindSubElem(joint_elem, "dynamics"))) {
ReadAttr(elem, "damping", 1, &pjoint->damping, text);
ReadAttr(elem, "damping", 1, pjoint->damping, text);
ReadAttr(elem, "friction", 1, &pjoint->frictionloss, text);
// copy parameters to all elements of planar joint
if (pjoint1) {
pjoint1->damping = pjoint2->damping = pjoint->damping;
mjuu_copyvec(pjoint1->damping, pjoint->damping, 1+mjNPOLY);
mjuu_copyvec(pjoint2->damping, pjoint->damping, 1+mjNPOLY);
pjoint1->frictionloss = pjoint2->frictionloss = pjoint->frictionloss;
}
}