diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index f280bf03..a05a3947 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1338,7 +1338,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { //---------------------------------- fluid models -------------------------------------------------- - +// fluid forces based on inertia-box approximation void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) { mjtNum lvel[6], wind[6], lwind[6], lfrc[6], bfrc[6], box[3], diam, *inertia; inertia = m->body_inertia + 3*i; @@ -1399,38 +1399,7 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) { -// all semi-axes of a geom -static void geomSemiaxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]) { - mjtNum* size = m->geom_size + 3*geom_id; - switch (m->geom_type[geom_id]) { - case mjGEOM_SPHERE: - semiaxes[0] = size[0]; - semiaxes[1] = size[0]; - semiaxes[2] = size[0]; - break; - - case mjGEOM_CAPSULE: - semiaxes[0] = size[0]; - semiaxes[1] = size[0]; - semiaxes[2] = size[1] + size[0]; - break; - - case mjGEOM_CYLINDER: - semiaxes[0] = size[0]; - semiaxes[1] = size[0]; - semiaxes[2] = size[1]; - break; - - default: - semiaxes[0] = size[0]; - semiaxes[1] = size[1]; - semiaxes[2] = size[2]; - } -} - - - -// fluid interaction forces based on ellipsoid approximation +// fluid forces based on ellipsoid approximation void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) { mjtNum lvel[6], wind[6], lwind[6], lfrc[6], bfrc[6]; mjtNum geom_interaction_coef, magnus_lift_coef, kutta_lift_coef; @@ -1440,7 +1409,7 @@ void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) { for (int j=0; jbody_geomnum[bodyid]; j++) { const int geomid = m->body_geomadr[bodyid] + j; - geomSemiaxes(m, geomid, semiaxes); + mju_geomSemiAxes(m, geomid, semiaxes); readFluidGeomInteraction( m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef, @@ -1623,7 +1592,7 @@ void mj_viscousForces( A_proj*blunt_drag_coef + slender_drag_coef*(A_max - A_proj)); const mjtNum drag_ang_coef = // linear plus quadratic fluid_viscosity * lin_visc_torq_coef + - fluid_density * mju_norm3(mom_visc) * ang_drag_coef; + fluid_density * mju_norm3(mom_visc); local_force[0] -= drag_ang_coef * ang_vel[0]; local_force[1] -= drag_ang_coef * ang_vel[1]; diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index edc34d83..e45ac0fc 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -80,9 +80,10 @@ MJAPI void mj_subtreeVel(const mjModel* m, mjData* d); //------------------------- fluid model ------------------------------------------------------------ - +// fluid forces based on inertia-box approximation void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i); +// fluid forces based on ellipsoid approximation void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid); // compute forces due to added mass (potential flow) diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 65948af3..d573e9e8 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -19,6 +19,7 @@ #include #include +#include "engine/engine_core_smooth.h" #include "engine/engine_forward.h" #include "engine/engine_core_constraint.h" #include "engine/engine_io.h" @@ -34,6 +35,36 @@ //------------------------- derivatives of spatial algebra ----------------------------------------- + + +// derivatives of cross product +static void mjd_cross(const mjtNum a[3], const mjtNum b[3], + mjtNum Da[restrict 9], mjtNum Db[restrict 9]) { + // derivative w.r.t a + if (Da) { + mju_zero(Da, 9); + Da[1] = b[2]; + Da[2] = -b[1]; + Da[3] = -b[2]; + Da[5] = b[0]; + Da[6] = b[1]; + Da[7] = -b[0]; + } + + // derivative w.r.t b + if (Db) { + mju_zero(Db, 9); + Db[1] = -a[2]; + Db[2] = a[1]; + Db[3] = a[2]; + Db[5] = -a[0]; + Db[6] = -a[1]; + Db[7] = a[0]; + } +} + + + // derivative of mju_crossMotion w.r.t velocity static void mjd_crossMotion_vel(mjtNum D[36], const mjtNum v[6]) { @@ -478,6 +509,8 @@ static void mjd_rne_vel(const mjModel* m, mjData* d, mjtNum* DfDv) { +//--------------------- utility functions for (d force / d vel) Jacobians -------------------------- + // construct sparse Jacobian structure of body; return nnz static int bodyJacSparse(const mjModel* m, int body, int* ind) { // skip fixed bodies @@ -562,242 +595,7 @@ static void addJTBJSparse(mjtNum* DfDv, const mjtNum* J, const mjtNum* B, -// add (d qfrc_passive / d qvel) to DfDv -void mjd_passive_vel(const mjModel* m, mjData* d, mjtNum* DfDv) { - mjMARKSTACK; - int nv = m->nv; - - // disabled: nothing to add - if (mjDISABLED(mjDSBL_PASSIVE)) { - return; - } - - // dof damping - for (int i=0; idof_damping[i]; - } - - // tendon damping - for (int i=0; intendon; i++) { - if (m->tendon_damping[i]>0) { - mjtNum B = -m->tendon_damping[i]; - - // add sparse or dense - if (mj_isSparse(m)) { - addJTBJSparse(DfDv, d->ten_J, &B, 1, nv, i, - d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind); - } else { - addJTBJ(DfDv, d->ten_J+i*nv, &B, 1, nv); - } - } - } - - // body viscosity, lift and drag - if (m->opt.viscosity>0 || m->opt.density>0) { - int rownnz[6], rowadr[6]; - mjtNum* J = mj_stackAlloc(d, 6*nv); - mjtNum* tmp = mj_stackAlloc(d, 3*nv); - int* colind = (int*) mj_stackAlloc(d, 6*nv); - - for (int i=1; inbody; i++) { - if (m->body_mass[i]>mjMINVAL) { - mjtNum lvel[6], wind[6], lwind[6], box[3], B; - mjtNum* inertia = m->body_inertia + 3*i; - - // equivalent inertia box - box[0] = mju_sqrt(mju_max(mjMINVAL, - (inertia[1] + inertia[2] - inertia[0])) / m->body_mass[i] * 6.0); - box[1] = mju_sqrt(mju_max(mjMINVAL, - (inertia[0] + inertia[2] - inertia[1])) / m->body_mass[i] * 6.0); - box[2] = mju_sqrt(mju_max(mjMINVAL, - (inertia[0] + inertia[1] - inertia[2])) / m->body_mass[i] * 6.0); - - // map from CoM-centered to local body-centered 6D velocity - mj_objectVelocity(m, d, mjOBJ_BODY, i, lvel, 1); - - // compute wind in local coordinates - mju_zero(wind, 6); - mju_copy3(wind+3, m->opt.wind); - mju_transformSpatial(lwind, wind, 0, d->xipos+3*i, - d->subtree_com+3*m->body_rootid[i], d->ximat+9*i); - - // subtract translational component from body velocity - mju_subFrom3(lvel+3, lwind+3); - - // get body global Jacobian: rotation then translation - mj_jacBodyCom(m, d, J+3*nv, J, i); - - // init with dense - int nnz = nv; - - // prepare for sparse - if (mj_isSparse(m)) { - // get sparse body Jacobian structure - nnz = bodyJacSparse(m, i, colind); - - // compress body Jacobian in-place - for (int j=0; j<6; j++) { - for (int k=0; kximat+9*i, J, 3, 3, nnz); - mju_copy(J, tmp, 3*nnz); - mju_mulMatTMat(tmp, d->ximat+9*i, J+3*nnz, 3, 3, nnz); - mju_copy(J+3*nnz, tmp, 3*nnz); - - // add viscous force and torque - if (m->opt.viscosity>0) { - // diameter of sphere approximation - mjtNum diam = (box[0] + box[1] + box[2])/3.0; - - // mju_scl3(lfrc, lvel, -mjPI*diam*diam*diam*m->opt.viscosity) - B = -mjPI*diam*diam*diam*m->opt.viscosity; - for (int j=0; j<3; j++) { - if (mj_isSparse(m)) { - addJTBJSparse(DfDv, J, &B, 1, nv, j, - rownnz, rowadr, colind); - } else { - addJTBJ(DfDv, J+j*nv, &B, 1, nv); - } - } - - // mju_scl3(lfrc+3, lvel+3, -3.0*mjPI*diam*m->opt.viscosity); - B = -3.0*mjPI*diam*m->opt.viscosity; - for (int j=0; j<3; j++) { - if (mj_isSparse(m)) { - addJTBJSparse(DfDv, J, &B, 1, nv, 3+j, - rownnz, rowadr, colind); - } else { - addJTBJ(DfDv, J+3*nv+j*nv, &B, 1, nv); - } - } - } - - // add lift and drag force and torque - if (m->opt.density>0) { - // lfrc[0] -= m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])* - // mju_abs(lvel[0])*lvel[0]/64.0; - B = -m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])* - 2*mju_abs(lvel[0])/64.0; - if (mj_isSparse(m)) { - addJTBJSparse(DfDv, J, &B, 1, nv, 0, - rownnz, rowadr, colind); - } else { - addJTBJ(DfDv, J, &B, 1, nv); - } - - // lfrc[1] -= m->opt.density*box[1]*(box[0]*box[0]*box[0]*box[0]+box[2]*box[2]*box[2]*box[2])* - // mju_abs(lvel[1])*lvel[1]/64.0; - B = -m->opt.density*box[1]*(box[0]*box[0]*box[0]*box[0]+box[2]*box[2]*box[2]*box[2])* - 2*mju_abs(lvel[1])/64.0; - if (mj_isSparse(m)) { - addJTBJSparse(DfDv, J, &B, 1, nv, 1, - rownnz, rowadr, colind); - } else { - addJTBJ(DfDv, J+nv, &B, 1, nv); - } - - // lfrc[2] -= m->opt.density*box[2]*(box[0]*box[0]*box[0]*box[0]+box[1]*box[1]*box[1]*box[1])* - // mju_abs(lvel[2])*lvel[2]/64.0; - B = -m->opt.density*box[2]*(box[0]*box[0]*box[0]*box[0]+box[1]*box[1]*box[1]*box[1])* - 2*mju_abs(lvel[2])/64.0; - if (mj_isSparse(m)) { - addJTBJSparse(DfDv, J, &B, 1, nv, 2, - rownnz, rowadr, colind); - } else { - addJTBJ(DfDv, J+2*nv, &B, 1, nv); - } - - // lfrc[3] -= 0.5*m->opt.density*box[1]*box[2]*mju_abs(lvel[3])*lvel[3]; - B = -0.5*m->opt.density*box[1]*box[2]*2*mju_abs(lvel[3]); - if (mj_isSparse(m)) { - addJTBJSparse(DfDv, J, &B, 1, nv, 3, - rownnz, rowadr, colind); - } else { - addJTBJ(DfDv, J+3*nv, &B, 1, nv); - } - - // lfrc[4] -= 0.5*m->opt.density*box[0]*box[2]*mju_abs(lvel[4])*lvel[4]; - B = -0.5*m->opt.density*box[0]*box[2]*2*mju_abs(lvel[4]); - if (mj_isSparse(m)) { - addJTBJSparse(DfDv, J, &B, 1, nv, 4, - rownnz, rowadr, colind); - } else { - addJTBJ(DfDv, J+4*nv, &B, 1, nv); - } - - // lfrc[5] -= 0.5*m->opt.density*box[0]*box[1]*mju_abs(lvel[5])*lvel[5]; - B = -0.5*m->opt.density*box[0]*box[1]*2*mju_abs(lvel[5]); - if (mj_isSparse(m)) { - addJTBJSparse(DfDv, J, &B, 1, nv, 5, - rownnz, rowadr, colind); - } else { - addJTBJ(DfDv, J+5*nv, &B, 1, nv); - } - } - } - } - } - mjFREESTACK; -} - - - -// add forward fin-diff approximation of (d qfrc_passive / d qvel) to DfDv -void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps, mjtNum* DfDv) { - int nv = m->nv; - - mjMARKSTACK; - mjtNum* qfrc_passive = mj_stackAlloc(d, nv); - mjtNum* fd = mj_stackAlloc(d, nv); - - // save qfrc_passive, assume mj_fwdVelocity was called - mju_copy(qfrc_passive, d->qfrc_passive, nv); - - // loop over dofs - for (int i=0; iqvel[i]; - - // eval at qvel[i]+eps - d->qvel[i] = saveqvel + eps; - mj_fwdVelocity(m, d); - - // restore qvel[i] - d->qvel[i] = saveqvel; - - // finite difference result in fd - mju_sub(fd, d->qfrc_passive, qfrc_passive, nv); - mju_scl(fd, fd, 1/eps, nv); - - // copy to i-th column of DfDv - for (int j=0; jnv; + int nnz = nv; + int rownnz[6], rowadr[6]; + mjtNum* J = mj_stackAlloc(d, 6*nv); + mjtNum* tmp = mj_stackAlloc(d, 3*nv); + int* colind = (int*) mj_stackAlloc(d, 6*nv); + int* colind_compressed = (int*) mj_stackAlloc(d, 6*nv); + + mjtNum lvel[6], wind[6], lwind[6]; + mjtNum geom_interaction_coef, magnus_lift_coef, kutta_lift_coef; + mjtNum semiaxes[3], virtual_mass[3], virtual_inertia[3]; + mjtNum blunt_drag_coef, slender_drag_coef, ang_drag_coef; + + if (mj_isSparse(m)) { + // get sparse body Jacobian structure + nnz = bodyJacSparse(m, bodyid, colind); + + // prepare rownnz, rowadr, colind for all 6 rows + for (int i=0; i<6; i++) { + rownnz[i] = nnz; + rowadr[i] = i == 0 ? 0 : rowadr[i-1] + nnz; + for (int k=0; kbody_geomnum[bodyid]; j++) { + const int geomid = m->body_geomadr[bodyid] + j; + + mju_geomSemiAxes(m, geomid, semiaxes); + + readFluidGeomInteraction( + m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef, + &blunt_drag_coef, &slender_drag_coef, &ang_drag_coef, + &kutta_lift_coef, &magnus_lift_coef, + virtual_mass, virtual_inertia); + + // scales all forces, read from MJCF as boolean (0.0 or 1.0) + if (geom_interaction_coef == 0.0) { + continue; + } + + // map from CoM-centered to local body-centered 6D velocity + mj_objectVelocity(m, d, mjOBJ_GEOM, geomid, lvel, 1); + // compute wind in local coordinates + mju_zero(wind, 6); + mju_copy3(wind+3, m->opt.wind); + mju_transformSpatial(lwind, wind, 0, + d->geom_xpos + 3*geomid, // Frame of ref's origin. + d->subtree_com + 3*m->body_rootid[bodyid], + d->geom_xmat + 9*geomid); // Frame of ref's orientation. + // subtract translational component from grom velocity + mju_subFrom3(lvel+3, lwind+3); + + // get body global Jacobian: rotation then translation + mj_jacGeom(m, d, J+3*nv, J, geomid); + + // compress geom Jacobian in-place + if (mj_isSparse(m)) { + for (int i=0; i<6; i++) { + for (int k=0; kgeom_xmat+9*geomid, J, 3, 3, nnz); + mju_copy(J, tmp, 3*nnz); + mju_mulMatTMat(tmp, d->geom_xmat+9*geomid, J+3*nnz, 3, 3, nnz); + mju_copy(J+3*nnz, tmp, 3*nnz); + + mjtNum B[36], D[9]; + mju_zero(B, 36); + mjd_magnus_force(B, lvel, m->opt.density, semiaxes, magnus_lift_coef); + + mjd_kutta_lift(D, lvel, m->opt.density, semiaxes, kutta_lift_coef); + addToQuadrant(B, D, 1, 1); + + mjd_viscous_drag(D, lvel, m->opt.density, m->opt.viscosity, semiaxes, + blunt_drag_coef, slender_drag_coef); + addToQuadrant(B, D, 1, 1); + + mjd_viscous_torque(D, lvel, m->opt.density, m->opt.viscosity, semiaxes, + slender_drag_coef, ang_drag_coef); + addToQuadrant(B, D, 0, 0); + + mjd_addedMassForces(B, lvel, m->opt.density, virtual_mass, virtual_inertia); + + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, J, B, 6, nv, 0, rownnz, rowadr, colind_compressed); + } else { + addJTBJ(DfDv, J, B, 6, nv); + } + } + + mjFREESTACK; +} + + +// fluid forces based on inertia-box approximation +void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, mjtNum* DfDv, int i) +{ + mjMARKSTACK; + + int nv = m->nv; + int rownnz[6], rowadr[6]; + mjtNum* J = mj_stackAlloc(d, 6*nv); + mjtNum* tmp = mj_stackAlloc(d, 3*nv); + int* colind = (int*) mj_stackAlloc(d, 6*nv); + + mjtNum lvel[6], wind[6], lwind[6], box[3], B; + mjtNum* inertia = m->body_inertia + 3*i; + + // equivalent inertia box + box[0] = mju_sqrt(mju_max(mjMINVAL, + (inertia[1] + inertia[2] - inertia[0])) / m->body_mass[i] * 6.0); + box[1] = mju_sqrt(mju_max(mjMINVAL, + (inertia[0] + inertia[2] - inertia[1])) / m->body_mass[i] * 6.0); + box[2] = mju_sqrt(mju_max(mjMINVAL, + (inertia[0] + inertia[1] - inertia[2])) / m->body_mass[i] * 6.0); + + // map from CoM-centered to local body-centered 6D velocity + mj_objectVelocity(m, d, mjOBJ_BODY, i, lvel, 1); + + // compute wind in local coordinates + mju_zero(wind, 6); + mju_copy3(wind+3, m->opt.wind); + mju_transformSpatial(lwind, wind, 0, d->xipos+3*i, + d->subtree_com+3*m->body_rootid[i], d->ximat+9*i); + + // subtract translational component from body velocity + mju_subFrom3(lvel+3, lwind+3); + + // get body global Jacobian: rotation then translation + mj_jacBodyCom(m, d, J+3*nv, J, i); + + // init with dense + int nnz = nv; + + // prepare for sparse + if (mj_isSparse(m)) { + // get sparse body Jacobian structure + nnz = bodyJacSparse(m, i, colind); + + // compress body Jacobian in-place + for (int j=0; j<6; j++) { + for (int k=0; kximat+9*i, J, 3, 3, nnz); + mju_copy(J, tmp, 3*nnz); + mju_mulMatTMat(tmp, d->ximat+9*i, J+3*nnz, 3, 3, nnz); + mju_copy(J+3*nnz, tmp, 3*nnz); + + // add viscous force and torque + if (m->opt.viscosity>0) { + // diameter of sphere approximation + mjtNum diam = (box[0] + box[1] + box[2])/3.0; + + // mju_scl3(lfrc, lvel, -mjPI*diam*diam*diam*m->opt.viscosity) + B = -mjPI*diam*diam*diam*m->opt.viscosity; + for (int j=0; j<3; j++) { + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, J, &B, 1, nv, j, rownnz, rowadr, colind); + } else { + addJTBJ(DfDv, J+j*nv, &B, 1, nv); + } + } + + // mju_scl3(lfrc+3, lvel+3, -3.0*mjPI*diam*m->opt.viscosity); + B = -3.0*mjPI*diam*m->opt.viscosity; + for (int j=0; j<3; j++) { + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, J, &B, 1, nv, 3+j, rownnz, rowadr, colind); + } else { + addJTBJ(DfDv, J+3*nv+j*nv, &B, 1, nv); + } + } + } + + // add lift and drag force and torque + if (m->opt.density>0) { + // lfrc[0] -= m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])* + // mju_abs(lvel[0])*lvel[0]/64.0; + B = -m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])* + 2*mju_abs(lvel[0])/64.0; + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, J, &B, 1, nv, 0, rownnz, rowadr, colind); + } else { + addJTBJ(DfDv, J, &B, 1, nv); + } + + // lfrc[1] -= m->opt.density*box[1]*(box[0]*box[0]*box[0]*box[0]+box[2]*box[2]*box[2]*box[2])* + // mju_abs(lvel[1])*lvel[1]/64.0; + B = -m->opt.density*box[1]*(box[0]*box[0]*box[0]*box[0]+box[2]*box[2]*box[2]*box[2])* + 2*mju_abs(lvel[1])/64.0; + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, J, &B, 1, nv, 1, rownnz, rowadr, colind); + } else { + addJTBJ(DfDv, J+nv, &B, 1, nv); + } + + // lfrc[2] -= m->opt.density*box[2]*(box[0]*box[0]*box[0]*box[0]+box[1]*box[1]*box[1]*box[1])* + // mju_abs(lvel[2])*lvel[2]/64.0; + B = -m->opt.density*box[2]*(box[0]*box[0]*box[0]*box[0]+box[1]*box[1]*box[1]*box[1])* + 2*mju_abs(lvel[2])/64.0; + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, J, &B, 1, nv, 2, rownnz, rowadr, colind); + } else { + addJTBJ(DfDv, J+2*nv, &B, 1, nv); + } + + // lfrc[3] -= 0.5*m->opt.density*box[1]*box[2]*mju_abs(lvel[3])*lvel[3]; + B = -0.5*m->opt.density*box[1]*box[2]*2*mju_abs(lvel[3]); + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, J, &B, 1, nv, 3, rownnz, rowadr, colind); + } else { + addJTBJ(DfDv, J+3*nv, &B, 1, nv); + } + + // lfrc[4] -= 0.5*m->opt.density*box[0]*box[2]*mju_abs(lvel[4])*lvel[4]; + B = -0.5*m->opt.density*box[0]*box[2]*2*mju_abs(lvel[4]); + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, J, &B, 1, nv, 4, rownnz, rowadr, colind); + } else { + addJTBJ(DfDv, J+4*nv, &B, 1, nv); + } + + // lfrc[5] -= 0.5*m->opt.density*box[0]*box[1]*mju_abs(lvel[5])*lvel[5]; + B = -0.5*m->opt.density*box[0]*box[1]*2*mju_abs(lvel[5]); + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, J, &B, 1, nv, 5, rownnz, rowadr, colind); + } else { + addJTBJ(DfDv, J+5*nv, &B, 1, nv); + } + } + + mjFREESTACK; +} + + + +//------------------------- derivatives of passive forces ------------------------------------------ + +// add (d qfrc_passive / d qvel) to DfDv +void mjd_passive_vel(const mjModel* m, mjData* d, mjtNum* DfDv) { + int nv = m->nv; + + // disabled: nothing to add + if (mjDISABLED(mjDSBL_PASSIVE)) { + return; + } + + // dof damping + for (int i=0; idof_damping[i]; + } + + // tendon damping + for (int i=0; intendon; i++) { + if (m->tendon_damping[i]>0) { + mjtNum B = -m->tendon_damping[i]; + + // add sparse or dense + if (mj_isSparse(m)) { + addJTBJSparse(DfDv, d->ten_J, &B, 1, nv, i, + d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind); + } else { + addJTBJ(DfDv, d->ten_J+i*nv, &B, 1, nv); + } + } + } + + // fluid drag model, either body-level (inertia box) or geom-level (ellipsoid) + if (m->opt.viscosity>0 || m->opt.density>0) { + for (int i=1; inbody; i++) { + if (m->body_mass[i]body_geomnum[i] && use_ellipsoid_model==0; j++) { + const int geomid = m->body_geomadr[i] + j; + use_ellipsoid_model += (m->geom_fluid[mjNFLUID*geomid] > 0); + } + if (use_ellipsoid_model) { + mjd_ellipsoidFluid(m, d, DfDv, i); + } else { + mjd_inertiaBoxFluid(m, d, DfDv, i); + } + } + } +} + + + +// add forward fin-diff approximation of (d qfrc_passive / d qvel) to DfDv +void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps, mjtNum* DfDv) { + int nv = m->nv; + + mjMARKSTACK; + mjtNum* qfrc_passive = mj_stackAlloc(d, nv); + mjtNum* fd = mj_stackAlloc(d, nv); + + // save qfrc_passive, assume mj_fwdVelocity was called + mju_copy(qfrc_passive, d->qfrc_passive, nv); + + // loop over dofs + for (int i=0; iqvel[i]; + + // eval at qvel[i]+eps + d->qvel[i] = saveqvel + eps; + mj_fwdVelocity(m, d); + + // restore qvel[i] + d->qvel[i] = saveqvel; + + // finite difference result in fd + mju_sub(fd, d->qfrc_passive, qfrc_passive, nv); + mju_scl(fd, fd, 1/eps, nv); + + // copy to i-th column of DfDv + for (int j=0; jnv; diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index e67fb758..31ea1be1 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -419,6 +419,37 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1, +// all 3 semi-axes of a geom +void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]) { + mjtNum* size = m->geom_size + 3*geom_id; + switch (m->geom_type[geom_id]) { + case mjGEOM_SPHERE: + semiaxes[0] = size[0]; + semiaxes[1] = size[0]; + semiaxes[2] = size[0]; + break; + + case mjGEOM_CAPSULE: + semiaxes[0] = size[0]; + semiaxes[1] = size[0]; + semiaxes[2] = size[1] + size[0]; + break; + + case mjGEOM_CYLINDER: + semiaxes[0] = size[0]; + semiaxes[1] = size[0]; + semiaxes[2] = size[1]; + break; + + default: + semiaxes[0] = size[0]; + semiaxes[1] = size[1]; + semiaxes[2] = size[2]; + } +} + + + //------------------------------ actuator models --------------------------------------------------- // muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax) diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index b498d0ba..93f6224c 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -40,7 +40,8 @@ MJAPI mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2], // muscle activation dynamics, prm = (tau_act, tau_deact) MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[2]); - +// all 3 semi-axes of a geom +MJAPI void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]); //------------------------------ misclellaneous ---------------------------------------------------- diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index cf045a1c..e15b562e 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -46,6 +46,7 @@ extern const mjMap coordinate_map[]; extern const mjMap angle_map[]; extern const mjMap enable_map[]; extern const mjMap bool_map[]; +extern const mjMap fluid_map[]; extern const mjMap TFAuto_map[]; extern const mjMap joint_map[]; extern const mjMap geom_map[]; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index dae0b24d..3d80e31d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -379,7 +379,7 @@ const mjMap bool_map[2] = { }; -// bool type +// fluidshape type const mjMap fluid_map[2] = { {"none", 0}, {"ellipsoid", 1} diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 27dc99d2..0f51d342 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -290,6 +290,8 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) { WriteAttr(elem, "margin", 1, &pgeom->margin, &def->geom.margin); WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap); WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap); + WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_switch, def->geom.fluid_switch); + WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->geom.fluid_coefs); if (mjuu_defined(pgeom->_mass)) { double mass = pgeom->GetVolume() * def->geom.density; WriteAttr(elem, "mass", 1, &pgeom->mass, &mass); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index cd94504e..10d7322a 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -89,6 +89,8 @@ static const char* const kEnergyConservingPendulumPath = "engine/testdata/derivative/energy_conserving_pendulum.xml"; static const char* const kTumblingThinObjectPath = "engine/testdata/derivative/tumbling_thin_object.xml"; +static const char* const kTumblingThinObjectEllipsoidPath = + "engine/testdata/derivative/tumbling_thin_object_ellipsoid.xml"; static const char* const kDampedActuatorsPath = "engine/testdata/derivative/damped_actuators.xml"; static const char* const kDamperActuatorsPath = @@ -150,42 +152,47 @@ TEST_F(DerivativeTest, SmoothDvel) { // compare analytic and fin-diff d_qfrc_passive/d_qvel TEST_F(DerivativeTest, PassiveDvel) { - const std::string xml_path = GetTestDataFilePath(kTumblingThinObjectPath); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); - int nv = model->nv; - mjData* data = mj_makeData(model); - // allocate d_qfrc_passive/d_qvel Jacobians - mjtNum* DfDv_analytic = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv); - mjtNum* DfDv_FD = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv); + for (const char* local_path : {kTumblingThinObjectPath, + kTumblingThinObjectEllipsoidPath}) { + // load model + const std::string xml_path = GetTestDataFilePath(local_path); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + int nv = model->nv; + mjData* data = mj_makeData(model); + // allocate Jacobians + mjtNum* DfDv_analytic = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv); + mjtNum* DfDv_FD = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv); - for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) { - // set sparsity - model->opt.jacobian = sparsity; + for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) { + // set sparsity + model->opt.jacobian = sparsity; - // take 100 steps so we have some velocities, then call forward - mj_resetData(model, data); - for (int i=0; i < 100; i++) { - mj_step(model, data); + // take 100 steps so we have some velocities, then call forward + mj_resetData(model, data); + for (int i=0; i < 100; i++) { + mj_step(model, data); + } + mj_forward(model, data); + + // clear DfDv, get analytic derivatives + mju_zero(DfDv_analytic, nv*nv); + mjd_passive_vel(model, data, DfDv_analytic); + + // clear DfDv, get finite-difference derivatives + mju_zero(DfDv_FD, nv*nv); + mjtNum eps = 1e-6; + mjd_passive_velFD(model, data, eps, DfDv_FD); + + // expect FD and analytic derivatives to be similar to tol precision + mjtNum tol = 1e-4; + CompareMatrices(DfDv_analytic, DfDv_FD, nv, nv, tol); } - mj_forward(model, data); - // clear DfDv, get analytic derivatives - mju_zero(DfDv_analytic, nv*nv); - mjd_passive_vel(model, data, DfDv_analytic); - - // clear DfDv, get finite-difference derivatives - mju_zero(DfDv_FD, nv*nv); - mjtNum eps = 1e-6; - mjd_passive_velFD(model, data, eps, DfDv_FD); - - // expect FD and analytic derivatives to be similar to eps precision - CompareMatrices(DfDv_analytic, DfDv_FD, nv, nv, eps); + mju_free(DfDv_FD); + mju_free(DfDv_analytic); + mj_deleteData(data); + mj_deleteModel(model); } - - mju_free(DfDv_FD); - mju_free(DfDv_analytic); - mj_deleteData(data); - mj_deleteModel(model); } // ----------------------- derivatives of mj_step() ---------------------------- @@ -336,8 +343,9 @@ TEST_F(DerivativeTest, LinearSystem) { LinearSystem(model, data, A, B); - PrintMatrix(A, 2*nv, 2*nv); - PrintMatrix(B, 2*nv, nu); + // uncomment for debugging: + // PrintMatrix(A, 2*nv, 2*nv); + // PrintMatrix(B, 2*nv, nu); // forward differenced A and B mjtNum eps = 1e-6; @@ -346,8 +354,9 @@ TEST_F(DerivativeTest, LinearSystem) { mjd_transitionFD(model, data, eps, /*centered=*/0, AFD, BFD); - PrintMatrix(AFD, 2*nv, 2*nv); - PrintMatrix(BFD, 2*nv, nu); + // uncomment for debugging: + // PrintMatrix(AFD, 2*nv, 2*nv); + // PrintMatrix(BFD, 2*nv, nu); // expect FD and analytic derivatives to be similar to eps precision CompareMatrices(A, AFD, 2*nv, 2*nv, eps); diff --git a/test/engine/testdata/derivative/tumbling_thin_object_ellipsoid.xml b/test/engine/testdata/derivative/tumbling_thin_object_ellipsoid.xml new file mode 100644 index 00000000..807c4e30 --- /dev/null +++ b/test/engine/testdata/derivative/tumbling_thin_object_ellipsoid.xml @@ -0,0 +1,13 @@ + +