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

Allow actuators to add damping and armature to joint and tendon transmissions.

Browse Source 
PiperOrigin-RevId: 886899849
Change-Id: I02200ee0d4f7c96096d8188b95f823b566562a30
This commit is contained in:
Yuval Tassa
2026-03-20 11:40:35 -07:00
committed by Copybara-Service
parent f7bd6d15f7
commit 510d75f4cf
25 changed files with 1164 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+3 -3
View File
@@ -1712,7 +1712,7 @@ void mj_tendonArmature(const mjModel* m, mjData* d) {
continue;
}
mjtNum armature = m->tendon_armature[k];
mjtNum armature = m->tendon_armature[k] + mj_actuatorArmature(m, mjOBJ_TENDON, k);
if (!armature) {
continue;
}
@@ -1803,7 +1803,7 @@ void mj_crb(const mjModel* m, mjData* d) {
// init M(i,i) with armature inertia
int Madr_ij = adr + rownnz[i] - 1;
M[Madr_ij] = dof_armature[i];
M[Madr_ij] = dof_armature[i] + mj_actuatorArmature(m, mjOBJ_JOINT, m->dof_jntid[i]);
// precompute buf = crb_body_i * cdof_i
mjtNum buf[6];
@@ -2643,7 +2643,7 @@ void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) {
continue;
}
mjtNum armature = m->tendon_armature[i];
mjtNum armature = m->tendon_armature[i] + mj_actuatorArmature(m, mjOBJ_TENDON, i);
// no armature: skip
if (!armature) {
src/engine/engine_core_util.c
+106 -3
View File
@@ -693,9 +693,9 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) {
// save the inertia matrix of b-th body
mjtNum inertia[9] = {0};
inertia[0] = m->body_inertia[3*b]; // inertia(1,1)
inertia[4] = m->body_inertia[3*b+1]; // inertia(2,2)
inertia[8] = m->body_inertia[3*b+2]; // inertia(3,3)
inertia[0] = m->body_inertia[3*b+0]; // inertia(1,1)
inertia[4] = m->body_inertia[3*b+1]; // inertia(2,2)
inertia[8] = m->body_inertia[3*b+2]; // inertia(3,3)
// term1 = body angular momentum about self COM in world frame
mjtNum tmp1[9], tmp2[9];
@@ -942,6 +942,109 @@ int tendonLimit(const mjModel* m, const mjtNum* ten_length, int i) {
}
// return actuator damping contribution to joint or tendon
mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjNPOLY]) {
if (type != mjOBJ_TENDON && type != mjOBJ_JOINT) {
mjERROR("only joint and tendon objects can inherit damping from actuators");
return 0;
}
// get actuator id
int actuatorid = type == mjOBJ_JOINT ? m->jnt_actuatorid[id] : m->tendon_actuatorid[id];
if (actuatorid == -1) {
return 0;
}
mjtNum damping = 0;
// single actuator contributes damping
if (actuatorid >= 0) {
mjtNum gear2 = m->actuator_gear[6*actuatorid] * m->actuator_gear[6*actuatorid];
damping = m->actuator_damping[actuatorid] * gear2;
for (int k = 0; k < mjNPOLY; k++) {
poly[k] += m->actuator_dampingpoly[mjNPOLY*actuatorid+k] * gear2;
}
}
// actuatorid < -1: scan all actuators for contributions
else {
for (int k = 0; k < m->nu; k++) {
// skip actuators that don't actuate the given joint/tendon
if (m->actuator_trnid[2*k] != id) {
continue;
}
if (type == mjOBJ_JOINT &&
m->actuator_trntype[k] != mjTRN_JOINT &&
m->actuator_trntype[k] != mjTRN_JOINTINPARENT) {
continue;
}
if (type == mjOBJ_TENDON && m->actuator_trntype[k] != mjTRN_TENDON) {
continue;
}
// accumulate damping contribution
mjtNum gear2 = m->actuator_gear[6*k] * m->actuator_gear[6*k];
damping += m->actuator_damping[k] * gear2;
for (int j = 0; j < mjNPOLY; j++) {
poly[j] += m->actuator_dampingpoly[mjNPOLY*k+j] * gear2;
}
}
}
return damping;
}
// return actuator armature contribution to joint or tendon
mjtNum mj_actuatorArmature(const mjModel* m, mjtObj type, int id) {
if (type != mjOBJ_TENDON && type != mjOBJ_JOINT) {
mjERROR("only joint and tendon objects can inherit armature from actuators");
return 0;
}
// get actuator id
int actuatorid = type == mjOBJ_JOINT ? m->jnt_actuatorid[id] : m->tendon_actuatorid[id];
// no actuator contribution
if (actuatorid == -1) {
return 0;
}
mjtNum armature = 0;
// single actuator contributes armature
if (actuatorid >= 0) {
mjtNum gear2 = m->actuator_gear[6*actuatorid] * m->actuator_gear[6*actuatorid];
armature = m->actuator_armature[actuatorid] * gear2;
}
// actuatorid < -1: scan all actuators for contributions
else {
for (int k = 0; k < m->nu; k++) {
// skip actuators that don't actuate the given joint/tendon
if (m->actuator_trnid[2*k] != id) {
continue;
}
if (type == mjOBJ_JOINT &&
m->actuator_trntype[k] != mjTRN_JOINT &&
m->actuator_trntype[k] != mjTRN_JOINTINPARENT) {
continue;
}
if (type == mjOBJ_TENDON && m->actuator_trntype[k] != mjTRN_TENDON) {
continue;
}
// accumulate armature contribution
mjtNum gear2 = m->actuator_gear[6*k] * m->actuator_gear[6*k];
armature += m->actuator_armature[k] * gear2;
}
}
return armature;
}
// count warnings, print only the first time
void mj_warning(mjData* d, int warning, int info) {
// check type
src/engine/engine_core_util.h
+6
View File
@@ -129,6 +129,12 @@ MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum res
// count the number of length limit violations for tendon i (0, 1 or 2)
int tendonLimit(const mjModel* m, const mjtNum* ten_length, int i);
// return actuator damping contribution to joint or tendon
MJAPI mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjNPOLY]);
// return actuator armature contribution to joint or tendon
MJAPI mjtNum mj_actuatorArmature(const mjModel* m, mjtObj type, int id);
// high-level warning function: count warnings in mjData, print only the first time
MJAPI void mj_warning(mjData* d, int warning, int info);
src/engine/engine_derivative.c
+8 -3
View File
@@ -1734,9 +1734,11 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
for (int j = 0; j < nv_awake; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j;
mjtNum v = d->qvel[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
mjtNum poly[mjNPOLY];
mju_copy(poly, m->dof_dampingpoly + mjNPOLY*i, mjNPOLY);
mjtNum damping = m->dof_damping[i] + mj_actuatorDamping(m, mjOBJ_JOINT, m->dof_jntid[i], poly);
int adr = m->D_rowadr[i] + m->D_diag[i];
d->qDeriv[adr] -= mjd_xPolyForce(m->dof_damping[i], poly, v, mjNPOLY, 1);
d->qDeriv[adr] -= mjd_xPolyForce(damping, poly, v, mjNPOLY, 1);
}
// flex edge damping
@@ -1775,7 +1777,10 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
}
mjtNum v = d->ten_velocity[i];
mjtNum B = -mjd_xPolyForce(m->tendon_damping[i], m->tendon_dampingpoly+mjNPOLY*i, v, mjNPOLY, 1);
mjtNum poly[mjNPOLY];
mju_copy(poly, m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY);
mjtNum damping = m->tendon_damping[i] + mj_actuatorDamping(m, mjOBJ_TENDON, i, poly);
mjtNum B = -mjd_xPolyForce(damping, poly, v, mjNPOLY, 1);
if (!B) {
continue;
src/engine/engine_forward.c
+9 -4
View File
@@ -26,8 +26,8 @@
#include "engine/engine_collision_driver.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_derivative.h"
#include "engine/engine_core_util.h"
#include "engine/engine_derivative.h"
#include "engine/engine_inverse.h"
#include "engine/engine_island.h"
#include "engine/engine_macro.h"
@@ -953,7 +953,9 @@ 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 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) {
if (m->dof_damping[i] > 0 ||
!mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY) ||
m->jnt_actuatorid[m->dof_jntid[i]] != -1) {
dof_damping = 1;
break;
}
@@ -983,8 +985,11 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : v;
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);
mjtNum poly[mjNPOLY];
mju_copy(poly, m->dof_dampingpoly + mjNPOLY*i, mjNPOLY);
mjtNum damping = m->dof_damping[i]
+ mj_actuatorDamping(m, mjOBJ_JOINT, m->dof_jntid[i], poly);
mjtNum damp_deriv = mjd_xPolyForce(damping, poly, qv, mjNPOLY, 1);
d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * damp_deriv;
}
src/engine/engine_inverse.c
+9 -3
View File
@@ -23,6 +23,7 @@
#include "engine/engine_collision_driver.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_core_util.h"
#include "engine/engine_derivative.h"
#include "engine/engine_memory.h"
#include "engine/engine_macro.h"
@@ -92,7 +93,9 @@ 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 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) {
if (m->dof_damping[i] > 0 ||
!mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY) ||
m->jnt_actuatorid[m->dof_jntid[i]] != -1) {
dof_damping = 1;
break;
}
@@ -109,8 +112,11 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mj_mulM(m, d, qfrc, qacc);
for (int i=0; i < nv; 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);
mjtNum poly[mjNPOLY];
mju_copy(poly, m->dof_dampingpoly + mjNPOLY*i, mjNPOLY);
mjtNum damping = m->dof_damping[i]
+ mj_actuatorDamping(m, mjOBJ_JOINT, m->dof_jntid[i], poly);
mjtNum damp_deriv = mjd_xPolyForce(damping, poly, v, mjNPOLY, 1);
qfrc[i] += m->opt.timestep * damp_deriv * d->qacc[i];
}
break;
src/engine/engine_passive.c
+39 -27
View File
@@ -117,13 +117,13 @@ void mj_flexInterpState(const mjModel* m, mjData* d, int f,
// spring and damper forces
static void mj_springdamper(const mjModel* m, mjData* d) {
int nv = m->nv, ntendon = m->ntendon;
int has_spring = !mjDISABLED(mjDSBL_SPRING);
int has_damping = !mjDISABLED(mjDSBL_DAMPER);
int enbl_spring = !mjDISABLED(mjDSBL_SPRING);
int enbl_damper = !mjDISABLED(mjDSBL_DAMPER);
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
// joint-level springs
if (has_spring) {
if (enbl_spring) {
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
int jnt_start = m->body_jntadr[i];
@@ -184,12 +184,14 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
}
// dof-level dampers
if (has_damping) {
if (enbl_damper) {
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;
mjtNum damping = m->dof_damping[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
mjtNum poly[mjNPOLY];
mju_copy(poly, m->dof_dampingpoly + mjNPOLY*i, mjNPOLY);
mjtNum damping = m->dof_damping[i]
+ mj_actuatorDamping(m, mjOBJ_JOINT, m->dof_jntid[i], poly);
if (damping != 0 || !mju_isZero(poly, mjNPOLY)) {
mjtNum v = d->qvel[i];
d->qfrc_damper[i] = -v * mju_polyForce(damping, poly, v, mjNPOLY, 1);
@@ -252,14 +254,15 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
for (int x = 0; x < 3; x++) {
for (int j = 0; j < 4; j++) {
// thin plate bending force
if (has_spring) spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x];
if (enbl_spring) spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x];
// thin plate damping force
// TODO: do not assume DOFs are in the world frame
if (has_damping) damper[3*i+x] += b[17*e+4*i+j] * vel[j][x];
if (enbl_damper) damper[3*i+x] += b[17*e+4*i+j] * vel[j][x];
}
// curved reference contribution
if (has_spring) spring[3*i+x] += b[17*e+16] * frc[i][x];
if (enbl_spring) spring[3*i+x] += b[17*e+16] * frc[i][x];
}
}
@@ -269,8 +272,8 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
int body_dofnum = m->body_dofnum[bid];
int body_dofadr = m->body_dofadr[bid];
for (int x = 0; x < body_dofnum; x++) {
if (has_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x];
if (has_damping) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f];
if (enbl_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x];
if (enbl_damper) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f];
}
}
}
@@ -299,10 +302,10 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
}
// compute force in the stretch frame
if (has_spring) mju_mulMatVec(frc, k, displ, 3*nodenum, 3*nodenum);
if (enbl_spring) mju_mulMatVec(frc, k, displ, 3*nodenum, 3*nodenum);
// compute damping force in stretch frame
if (has_damping) mju_mulMatVec(dmp, k, vel, 3*nodenum, 3*nodenum);
if (enbl_damper) mju_mulMatVec(dmp, k, vel, 3*nodenum, 3*nodenum);
// rotate forces to global frame and add to qfrc
mju_negQuat(quat, quat);
@@ -312,11 +315,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mji_rotVecQuat(qdmp, dmp+3*i, quat);
mju_scl3(qdmp, qdmp, m->flex_damping[f]);
if (m->flex_centered[f]) {
if (has_spring) mji_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc);
if (has_damping) mji_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp);
if (enbl_spring) mji_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc);
if (enbl_damper) mji_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp);
} else {
if (has_spring) mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring);
if (has_damping) mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper);
if (enbl_spring) mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring);
if (enbl_damper) mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper);
}
}
@@ -420,8 +423,8 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// flexedge-level spring-dampers
for (int f=0; f < m->nflex; f++) {
mjtNum stiffness = m->flex_edgestiffness[f] * has_spring;
mjtNum damping = m->flex_edgedamping[f] * has_damping;
mjtNum stiffness = enbl_spring ? m->flex_edgestiffness[f] : 0;
mjtNum damping = enbl_damper ? m->flex_edgedamping[f] : 0;
// disabled or rigid: nothing to do
if (m->flex_rigid[f] || (stiffness == 0 && damping == 0)) {
@@ -458,13 +461,22 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
continue;
}
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;
mjtNum stiffness = 0;
const mjtNum* spoly = NULL;
if (enbl_spring) {
stiffness = m->tendon_stiffness[i];
spoly = m->tendon_stiffnesspoly + mjNPOLY*i;
}
// disabled : nothing to do
if (stiffness == 0 && mju_isZero(spoly, mjNPOLY) &&
mjtNum damping = 0;
mjtNum dpoly[mjNPOLY] = {0};
if (enbl_damper) {
mju_copy(dpoly, m->tendon_dampingpoly + mjNPOLY*i, mjNPOLY);
damping = m->tendon_damping[i] + mj_actuatorDamping(m, mjOBJ_TENDON, i, dpoly);
}
// both zero: nothing to do
if (stiffness == 0 && (!enbl_spring || mju_isZero(spoly, mjNPOLY)) &&
damping == 0 && mju_isZero(dpoly, mjNPOLY)) {
continue;
}
@@ -474,11 +486,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mjtNum lower = m->tendon_lengthspring[2*i];
mjtNum upper = m->tendon_lengthspring[2*i+1];
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;
mjtNum frc_spring = enbl_spring ? -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0) : 0;
// 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;
mjtNum frc_damper = enbl_damper ? -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_setconst.c
+46 -2
View File
@@ -54,7 +54,8 @@ static void mj_setM0(mjModel* m, mjData* d) {
mju_mulInertVec(buf, crb+10*m->dof_bodyid[i], d->cdof+6*i);
// dof_M0(i) = armature inertia + cdof_i * (crb_body_i * cdof_i)
m->dof_M0[i] = m->dof_armature[i] + mju_dot(d->cdof+6*i, buf, 6);
mjtNum armature = m->dof_armature[i] + mj_actuatorArmature(m, mjOBJ_JOINT, m->dof_jntid[i]);
m->dof_M0[i] = armature + mju_dot(d->cdof+6*i, buf, 6);
}
}
@@ -102,6 +103,48 @@ static void setFixed(mjModel* m, mjData* d) {
}
m->ngravcomp = ngravcomp;
// set jnt_actuatorid and tendon_actuatorid
mju_fillInt(m->jnt_actuatorid, -1, m->njnt);
mju_fillInt(m->tendon_actuatorid, -1, m->ntendon);
for (int i=0; i < m->nu; i++) {
// skip actuator with no damping and no armature
if (m->actuator_damping[i] == 0 &&
mju_isZero(m->actuator_dampingpoly+mjNPOLY*i, mjNPOLY) &&
m->actuator_armature[i] == 0) {
continue;
}
// joint or jointinparent transmission
if (m->actuator_trntype[i] == mjTRN_JOINT ||
m->actuator_trntype[i] == mjTRN_JOINTINPARENT) {
int jntid = m->actuator_trnid[2*i];
// first actuator: set id to i
if (m->jnt_actuatorid[jntid] == -1) {
m->jnt_actuatorid[jntid] = i;
}
// multiple actuators acting on single transmission: use -2 sentinel
else {
m->jnt_actuatorid[jntid] = -2;
}
}
// tendon transmission
else if (m->actuator_trntype[i] == mjTRN_TENDON) {
int tenid = m->actuator_trnid[2*i];
// first actuator: set id to i
if (m->tendon_actuatorid[tenid] == -1) {
m->tendon_actuatorid[tenid] = i;
}
// multiple actuators acting on single transmission: use -2 sentinel
else {
m->tendon_actuatorid[tenid] = -2;
}
}
}
// ----- tree related (body_treeid and dof_treeid already computed)
@@ -213,7 +256,8 @@ 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 && mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY) &&
m->tendon_damping[i] == 0 && mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY)) {
m->tendon_damping[i] == 0 && mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY) &&
m->tendon_actuatorid[i] == -1) {
continue;
}