Break out passive force functions into a new file engine_passive.c
PiperOrigin-RevId: 513834952 Change-Id: I5347440f24d767a7dc81a34e48e1e17d8c10173f
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
fd1070cf03
commit
222ee00a66
@@ -40,6 +40,8 @@ set(MUJOCO_ENGINE_SRCS
|
||||
engine_io.c
|
||||
engine_io.h
|
||||
engine_macro.h
|
||||
engine_passive.c
|
||||
engine_passive.h
|
||||
engine_plugin.cc
|
||||
engine_plugin.h
|
||||
engine_print.c
|
||||
|
||||
@@ -19,12 +19,10 @@
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include "engine/engine_callback.h"
|
||||
#include "engine/engine_core_constraint.h"
|
||||
#include "engine/engine_crossplatform.h"
|
||||
#include "engine/engine_io.h"
|
||||
#include "engine/engine_macro.h"
|
||||
#include "engine/engine_plugin.h"
|
||||
#include "engine/engine_support.h"
|
||||
#include "engine/engine_util_blas.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
@@ -1321,155 +1319,6 @@ void mj_comVel(const mjModel* m, mjData* d) {
|
||||
|
||||
|
||||
|
||||
// passive forces
|
||||
void mj_passive(const mjModel* m, mjData* d) {
|
||||
int issparse = mj_isSparse(m);
|
||||
int nv = m->nv;
|
||||
mjtNum dif[3], frc, stiffness, damping;
|
||||
|
||||
// clear passive force
|
||||
mju_zero(d->qfrc_passive, m->nv);
|
||||
|
||||
// disabled: return
|
||||
if (mjDISABLED(mjDSBL_PASSIVE)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// joint-level springs
|
||||
for (int i=0; i<m->njnt; i++) {
|
||||
stiffness = m->jnt_stiffness[i];
|
||||
|
||||
int padr = m->jnt_qposadr[i];
|
||||
int dadr = m->jnt_dofadr[i];
|
||||
|
||||
switch (m->jnt_type[i]) {
|
||||
case mjJNT_FREE:
|
||||
// apply force
|
||||
d->qfrc_passive[dadr+0] -= stiffness*(d->qpos[padr+0] - m->qpos_spring[padr+0]);
|
||||
d->qfrc_passive[dadr+1] -= stiffness*(d->qpos[padr+1] - m->qpos_spring[padr+1]);
|
||||
d->qfrc_passive[dadr+2] -= stiffness*(d->qpos[padr+2] - m->qpos_spring[padr+2]);
|
||||
|
||||
// continue with rotations
|
||||
dadr += 3;
|
||||
padr += 3;
|
||||
mjFALLTHROUGH;
|
||||
|
||||
case mjJNT_BALL:
|
||||
// covert quatertion difference into angular "velocity"
|
||||
mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr);
|
||||
|
||||
// apply torque
|
||||
d->qfrc_passive[dadr+0] -= stiffness*dif[0];
|
||||
d->qfrc_passive[dadr+1] -= stiffness*dif[1];
|
||||
d->qfrc_passive[dadr+2] -= stiffness*dif[2];
|
||||
break;
|
||||
|
||||
case mjJNT_SLIDE:
|
||||
case mjJNT_HINGE:
|
||||
// apply force or torque
|
||||
d->qfrc_passive[dadr] -= stiffness*(d->qpos[padr] - m->qpos_spring[padr]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// dof-level dampers
|
||||
for (int i=0; i<m->nv; i++) {
|
||||
damping = m->dof_damping[i];
|
||||
d->qfrc_passive[i] -= damping*d->qvel[i];
|
||||
}
|
||||
|
||||
// tendon-level spring-dampers
|
||||
for (int i=0; i<m->ntendon; i++) {
|
||||
stiffness = m->tendon_stiffness[i];
|
||||
damping = m->tendon_damping[i];
|
||||
|
||||
// compute spring force along tendon
|
||||
mjtNum length = d->ten_length[i];
|
||||
mjtNum lower = m->tendon_lengthspring[2*i];
|
||||
mjtNum upper = m->tendon_lengthspring[2*i+1];
|
||||
if (length > upper) {
|
||||
frc = stiffness * (upper - length);
|
||||
} else if (length < lower) {
|
||||
frc = stiffness * (lower - length);
|
||||
} else {
|
||||
frc = 0;
|
||||
}
|
||||
|
||||
// compute damper linear force along tendon
|
||||
frc -= damping * d->ten_velocity[i];
|
||||
|
||||
// transform to joint torque, add to qfrc_passive: dense or sparse
|
||||
if (issparse) {
|
||||
int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i];
|
||||
for (int j=d->ten_J_rowadr[i]; j<end; j++) {
|
||||
d->qfrc_passive[d->ten_J_colind[j]] += d->ten_J[j] * frc;
|
||||
}
|
||||
} else {
|
||||
mju_addToScl(d->qfrc_passive, d->ten_J+i*nv, frc, nv);
|
||||
}
|
||||
}
|
||||
|
||||
// body-level gravity compensation
|
||||
if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
|
||||
mjtNum force[3], torque[3]={0};
|
||||
|
||||
// apply per-body gravity compensation
|
||||
for (int i=1; i<m->nbody; i++) {
|
||||
if (m->body_gravcomp[i]) {
|
||||
mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i]));
|
||||
mj_applyFT(m, d, force, torque, d->xipos+3*i, i, d->qfrc_passive);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// body-level viscosity, lift and drag
|
||||
if (m->opt.viscosity>0 || m->opt.density>0) {
|
||||
for (int i=1; i<m->nbody; i++) {
|
||||
if (m->body_mass[i]<mjMINVAL) {
|
||||
continue;
|
||||
}
|
||||
|
||||
int use_ellipsoid_model = 0;
|
||||
// if any child geom uses the ellipsoid model, inertia-box model is disabled for parent body
|
||||
for (int j=0; j<m->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) {
|
||||
mj_ellipsoidFluidModel(m, d, i);
|
||||
} else {
|
||||
mj_inertiaBoxFluidModel(m, d, i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// user callback: add custom passive forces
|
||||
if (mjcb_passive) {
|
||||
mjcb_passive(m, d);
|
||||
}
|
||||
|
||||
// plugin
|
||||
if (m->nplugin) {
|
||||
const int nslot = mjp_pluginCount();
|
||||
// iterate over plugins, call compute if type is mjPLUGIN_PASSIVE
|
||||
for (int i=0; i<m->nplugin; i++) {
|
||||
const int slot = m->plugin[i];
|
||||
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
|
||||
if (!plugin) {
|
||||
mju_error_i("invalid plugin slot: %d", slot);
|
||||
}
|
||||
if (plugin->capabilityflags & mjPLUGIN_PASSIVE) {
|
||||
if (!plugin->compute) {
|
||||
mju_error_i("`compute` is a null function pointer for plugin at slot %d", slot);
|
||||
}
|
||||
plugin->compute(m, d, i, mjPLUGIN_PASSIVE);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// subtree linear velocity and angular momentum
|
||||
void mj_subtreeVel(const mjModel* m, mjData* d) {
|
||||
mjtNum dx[3], dv[3], dp[3], dL[3];
|
||||
@@ -1534,335 +1383,6 @@ 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;
|
||||
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);
|
||||
mju_zero(lfrc, 6);
|
||||
|
||||
// set viscous force and torque
|
||||
if (m->opt.viscosity>0) {
|
||||
// diameter of sphere approximation
|
||||
diam = (box[0] + box[1] + box[2])/3.0;
|
||||
|
||||
// angular viscosity
|
||||
mju_scl3(lfrc, lvel, -mjPI*diam*diam*diam*m->opt.viscosity);
|
||||
|
||||
// linear viscosity
|
||||
mju_scl3(lfrc+3, lvel+3, -3.0*mjPI*diam*m->opt.viscosity);
|
||||
}
|
||||
|
||||
// add lift and drag force and torque
|
||||
if (m->opt.density>0) {
|
||||
// force
|
||||
lfrc[3] -= 0.5*m->opt.density*box[1]*box[2]*mju_abs(lvel[3])*lvel[3];
|
||||
lfrc[4] -= 0.5*m->opt.density*box[0]*box[2]*mju_abs(lvel[4])*lvel[4];
|
||||
lfrc[5] -= 0.5*m->opt.density*box[0]*box[1]*mju_abs(lvel[5])*lvel[5];
|
||||
|
||||
// torque
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
}
|
||||
// rotate to global orientation: lfrc -> bfrc
|
||||
mju_rotVecMat(bfrc, lfrc, d->ximat+9*i);
|
||||
mju_rotVecMat(bfrc+3, lfrc+3, d->ximat+9*i);
|
||||
|
||||
// apply force and torque to body com
|
||||
mj_applyFT(m, d, bfrc+3, bfrc, d->xipos+3*i, i, d->qfrc_passive);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 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;
|
||||
mjtNum semiaxes[3], virtual_mass[3], virtual_inertia[3];
|
||||
mjtNum blunt_drag_coef, slender_drag_coef, ang_drag_coef;
|
||||
|
||||
for (int j=0; j<m->body_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);
|
||||
|
||||
// initialize viscous force and torque
|
||||
mju_zero(lfrc, 6);
|
||||
|
||||
// added-mass forces and torques
|
||||
mj_addedMassForces(lvel, NULL, m->opt.density, virtual_mass, virtual_inertia, lfrc);
|
||||
|
||||
// lift force orthogonal to lvel from Kutta-Joukowski theorem
|
||||
mj_viscousForces(lvel, m->opt.density, m->opt.viscosity, semiaxes, magnus_lift_coef,
|
||||
kutta_lift_coef, blunt_drag_coef, slender_drag_coef, ang_drag_coef, lfrc);
|
||||
|
||||
// scale by geom_interaction_coef (1.0 by default)
|
||||
mju_scl(lfrc, lfrc, geom_interaction_coef, 6);
|
||||
|
||||
// rotate to global orientation: lfrc -> bfrc
|
||||
mju_rotVecMat(bfrc, lfrc, d->geom_xmat + 9*geomid);
|
||||
mju_rotVecMat(bfrc+3, lfrc+3, d->geom_xmat + 9*geomid);
|
||||
|
||||
// apply force and torque to body com
|
||||
mj_applyFT(m, d, bfrc+3, bfrc,
|
||||
d->geom_xpos + 3*geomid, // point where FT is generated
|
||||
bodyid, d->qfrc_passive);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// compute forces due to fluid mass moving with the body
|
||||
void mj_addedMassForces(const mjtNum local_vels[6], const mjtNum local_accels[6],
|
||||
const mjtNum fluid_density, const mjtNum virtual_mass[3],
|
||||
const mjtNum virtual_inertia[3], mjtNum local_force[6])
|
||||
{
|
||||
const mjtNum lin_vel[3] = {local_vels[3], local_vels[4], local_vels[5]};
|
||||
const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]};
|
||||
const mjtNum virtual_lin_mom[3] = {
|
||||
fluid_density * virtual_mass[0] * lin_vel[0],
|
||||
fluid_density * virtual_mass[1] * lin_vel[1],
|
||||
fluid_density * virtual_mass[2] * lin_vel[2]
|
||||
};
|
||||
const mjtNum virtual_ang_mom[3] = {
|
||||
fluid_density * virtual_inertia[0] * ang_vel[0],
|
||||
fluid_density * virtual_inertia[1] * ang_vel[1],
|
||||
fluid_density * virtual_inertia[2] * ang_vel[2]
|
||||
};
|
||||
|
||||
// disabled due to dependency on qacc but included for completeness
|
||||
if (local_accels) {
|
||||
local_force[0] -= fluid_density * virtual_inertia[0] * local_accels[0];
|
||||
local_force[1] -= fluid_density * virtual_inertia[1] * local_accels[1];
|
||||
local_force[2] -= fluid_density * virtual_inertia[2] * local_accels[2];
|
||||
local_force[3] -= fluid_density * virtual_mass[0] * local_accels[3];
|
||||
local_force[4] -= fluid_density * virtual_mass[1] * local_accels[4];
|
||||
local_force[5] -= fluid_density * virtual_mass[2] * local_accels[5];
|
||||
}
|
||||
|
||||
mjtNum added_mass_force[3], added_mass_torque1[3], added_mass_torque2[3];
|
||||
mju_cross(added_mass_force, virtual_lin_mom, ang_vel);
|
||||
mju_cross(added_mass_torque1, virtual_lin_mom, lin_vel);
|
||||
mju_cross(added_mass_torque2, virtual_ang_mom, ang_vel);
|
||||
|
||||
mju_addTo3(local_force, added_mass_torque1);
|
||||
mju_addTo3(local_force, added_mass_torque2);
|
||||
mju_addTo3(local_force+3, added_mass_force);
|
||||
}
|
||||
|
||||
|
||||
// inlined helper functions
|
||||
static inline mjtNum mji_pow4(const mjtNum val) {
|
||||
return (val*val)*(val*val);
|
||||
}
|
||||
|
||||
static inline mjtNum mji_pow2(const mjtNum val) {
|
||||
return val*val;
|
||||
}
|
||||
|
||||
static inline mjtNum mji_ellipsoid_max_moment(const mjtNum size[3], const int dir) {
|
||||
const mjtNum d0 = size[dir], d1 = size[(dir+1) % 3], d2 = size[(dir+2) % 3];
|
||||
return 8.0/15.0 * mjPI * d0 * mji_pow4(mju_max(d1, d2));
|
||||
}
|
||||
|
||||
|
||||
|
||||
// lift and drag forces due to motion in the fluid
|
||||
void mj_viscousForces(
|
||||
const mjtNum local_vels[6], const mjtNum fluid_density,
|
||||
const mjtNum fluid_viscosity, const mjtNum size[3],
|
||||
const mjtNum magnus_lift_coef, const mjtNum kutta_lift_coef,
|
||||
const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef,
|
||||
const mjtNum ang_drag_coef, mjtNum local_force[6])
|
||||
{
|
||||
const mjtNum lin_vel[3] = {local_vels[3], local_vels[4], local_vels[5]};
|
||||
const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]};
|
||||
const mjtNum volume = 4.0/3.0 * mjPI * size[0] * size[1] * size[2];
|
||||
const mjtNum d_max = mju_max(mju_max(size[0], size[1]), size[2]);
|
||||
const mjtNum d_min = mju_min(mju_min(size[0], size[1]), size[2]);
|
||||
const mjtNum d_mid = size[0] + size[1] + size[2] - d_max - d_min;
|
||||
const mjtNum A_max = mjPI * d_max * d_mid;
|
||||
|
||||
mjtNum magnus_force[3];
|
||||
mju_cross(magnus_force, ang_vel, lin_vel);
|
||||
magnus_force[0] *= magnus_lift_coef * fluid_density * volume;
|
||||
magnus_force[1] *= magnus_lift_coef * fluid_density * volume;
|
||||
magnus_force[2] *= magnus_lift_coef * fluid_density * volume;
|
||||
|
||||
// the dot product between velocity and the normal to the cross-section that
|
||||
// defines the body's projection along velocity is proj_num/sqrt(proj_denom)
|
||||
const mjtNum proj_denom = mji_pow4(size[1] * size[2]) * mji_pow2(lin_vel[0]) +
|
||||
mji_pow4(size[2] * size[0]) * mji_pow2(lin_vel[1]) +
|
||||
mji_pow4(size[0] * size[1]) * mji_pow2(lin_vel[2]);
|
||||
const mjtNum proj_num = mji_pow2(size[1] * size[2] * lin_vel[0]) +
|
||||
mji_pow2(size[2] * size[0] * lin_vel[1]) +
|
||||
mji_pow2(size[0] * size[1] * lin_vel[2]);
|
||||
|
||||
// projected surface in the direction of the velocity
|
||||
const mjtNum A_proj = mjPI * mju_sqrt(proj_denom/mju_max(mjMINVAL, proj_num));
|
||||
|
||||
// not-unit normal to ellipsoid's projected area in the direction of velocity
|
||||
const mjtNum norm[3] = {
|
||||
mji_pow2(size[1] * size[2]) * lin_vel[0],
|
||||
mji_pow2(size[2] * size[0]) * lin_vel[1],
|
||||
mji_pow2(size[0] * size[1]) * lin_vel[2]
|
||||
};
|
||||
|
||||
// cosine between velocity and normal to the surface
|
||||
// divided by proj_denom instead of sqrt(proj_denom) to account for skipped normalization in norm
|
||||
const mjtNum cos_alpha = proj_num / mju_max(
|
||||
mjMINVAL, mju_norm3(lin_vel) * proj_denom);
|
||||
mjtNum kutta_circ[3];
|
||||
mju_cross(kutta_circ, norm, lin_vel);
|
||||
kutta_circ[0] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj;
|
||||
kutta_circ[1] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj;
|
||||
kutta_circ[2] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj;
|
||||
mjtNum kutta_force[3];
|
||||
mju_cross(kutta_force, kutta_circ, lin_vel);
|
||||
|
||||
// viscous force and torque in Stokes flow, analytical for spherical bodies
|
||||
const mjtNum eq_sphere_D = 2.0/3.0 * (size[0] + size[1] + size[2]);
|
||||
const mjtNum lin_visc_force_coef = 3.0 * mjPI * eq_sphere_D;
|
||||
const mjtNum lin_visc_torq_coef = mjPI * eq_sphere_D*eq_sphere_D*eq_sphere_D;
|
||||
|
||||
// moments of inertia used to compute angular quadratic drag
|
||||
const mjtNum I_max = 8.0/15.0 * mjPI * d_mid * mji_pow4(d_max);
|
||||
const mjtNum II[3] = {
|
||||
mji_ellipsoid_max_moment(size, 0),
|
||||
mji_ellipsoid_max_moment(size, 1),
|
||||
mji_ellipsoid_max_moment(size, 2)
|
||||
};
|
||||
const mjtNum mom_visc[3] = {
|
||||
ang_vel[0] * (ang_drag_coef*II[0] + slender_drag_coef*(I_max - II[0])),
|
||||
ang_vel[1] * (ang_drag_coef*II[1] + slender_drag_coef*(I_max - II[1])),
|
||||
ang_vel[2] * (ang_drag_coef*II[2] + slender_drag_coef*(I_max - II[2]))
|
||||
};
|
||||
|
||||
const mjtNum drag_lin_coef = // linear plus quadratic
|
||||
fluid_viscosity*lin_visc_force_coef + fluid_density*mju_norm3(lin_vel)*(
|
||||
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);
|
||||
|
||||
local_force[0] -= drag_ang_coef * ang_vel[0];
|
||||
local_force[1] -= drag_ang_coef * ang_vel[1];
|
||||
local_force[2] -= drag_ang_coef * ang_vel[2];
|
||||
local_force[3] += magnus_force[0] + kutta_force[0] - drag_lin_coef*lin_vel[0];
|
||||
local_force[4] += magnus_force[1] + kutta_force[1] - drag_lin_coef*lin_vel[1];
|
||||
local_force[5] += magnus_force[2] + kutta_force[2] - drag_lin_coef*lin_vel[2];
|
||||
}
|
||||
|
||||
|
||||
|
||||
// read the geom_fluid_coefs array into its constituent parts
|
||||
void readFluidGeomInteraction(const mjtNum* geom_fluid_coefs,
|
||||
mjtNum* geom_fluid_coef,
|
||||
mjtNum* blunt_drag_coef,
|
||||
mjtNum* slender_drag_coef,
|
||||
mjtNum* ang_drag_coef,
|
||||
mjtNum* kutta_lift_coef,
|
||||
mjtNum* magnus_lift_coef,
|
||||
mjtNum virtual_mass[3],
|
||||
mjtNum virtual_inertia[3]) {
|
||||
int i = 0;
|
||||
geom_fluid_coef[0] = geom_fluid_coefs[i++];
|
||||
blunt_drag_coef[0] = geom_fluid_coefs[i++];
|
||||
slender_drag_coef[0] = geom_fluid_coefs[i++];
|
||||
ang_drag_coef[0] = geom_fluid_coefs[i++];
|
||||
kutta_lift_coef[0] = geom_fluid_coefs[i++];
|
||||
magnus_lift_coef[0] = geom_fluid_coefs[i++];
|
||||
virtual_mass[0] = geom_fluid_coefs[i++];
|
||||
virtual_mass[1] = geom_fluid_coefs[i++];
|
||||
virtual_mass[2] = geom_fluid_coefs[i++];
|
||||
virtual_inertia[0] = geom_fluid_coefs[i++];
|
||||
virtual_inertia[1] = geom_fluid_coefs[i++];
|
||||
virtual_inertia[2] = geom_fluid_coefs[i++];
|
||||
if (i != mjNFLUID) {
|
||||
mju_error("Error in reading geom_fluid_coefs: wrong number of entries.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// write components into geom_fluid_coefs array
|
||||
void writeFluidGeomInteraction (mjtNum* geom_fluid_coefs,
|
||||
const mjtNum* geom_fluid_coef,
|
||||
const mjtNum* blunt_drag_coef,
|
||||
const mjtNum* slender_drag_coef,
|
||||
const mjtNum* ang_drag_coef,
|
||||
const mjtNum* kutta_lift_coef,
|
||||
const mjtNum* magnus_lift_coef,
|
||||
const mjtNum virtual_mass[3],
|
||||
const mjtNum virtual_inertia[3]) {
|
||||
int i = 0;
|
||||
geom_fluid_coefs[i++] = geom_fluid_coef[0];
|
||||
geom_fluid_coefs[i++] = blunt_drag_coef[0];
|
||||
geom_fluid_coefs[i++] = slender_drag_coef[0];
|
||||
geom_fluid_coefs[i++] = ang_drag_coef[0];
|
||||
geom_fluid_coefs[i++] = kutta_lift_coef[0];
|
||||
geom_fluid_coefs[i++] = magnus_lift_coef[0];
|
||||
geom_fluid_coefs[i++] = virtual_mass[0];
|
||||
geom_fluid_coefs[i++] = virtual_mass[1];
|
||||
geom_fluid_coefs[i++] = virtual_mass[2];
|
||||
geom_fluid_coefs[i++] = virtual_inertia[0];
|
||||
geom_fluid_coefs[i++] = virtual_inertia[1];
|
||||
geom_fluid_coefs[i++] = virtual_inertia[2];
|
||||
if (i != mjNFLUID) {
|
||||
mju_error("Error in writing geom_fluid_coefs: wrong number of entries.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
//---------------------------------- RNE -----------------------------------------------------------
|
||||
|
||||
// RNE: compute M(qpos)*qacc + C(qpos,qvel); flg_acc=0 removes inertial term
|
||||
|
||||
@@ -71,56 +71,10 @@ MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, i
|
||||
// compute cvel, cdof_dot
|
||||
MJAPI void mj_comVel(const mjModel* m, mjData* d);
|
||||
|
||||
// passive forces
|
||||
MJAPI void mj_passive(const mjModel* m, mjData* d);
|
||||
|
||||
// subtree linear velocity and angular momentum
|
||||
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)
|
||||
void mj_addedMassForces(
|
||||
const mjtNum local_vels[6], const mjtNum local_accels[6],
|
||||
const mjtNum fluid_density, const mjtNum virtual_mass[3],
|
||||
const mjtNum virtual_inertia[3], mjtNum local_force[6]);
|
||||
|
||||
// compute forces due to viscous effects
|
||||
void mj_viscousForces(
|
||||
const mjtNum local_vels[6], const mjtNum fluid_density,
|
||||
const mjtNum fluid_viscosity, const mjtNum size[3],
|
||||
const mjtNum magnus_lift_coef, const mjtNum kutta_lift_coef,
|
||||
const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef,
|
||||
const mjtNum ang_drag_coef, mjtNum local_force[6]);
|
||||
|
||||
void readFluidGeomInteraction(const mjtNum * geom_fluid_coefs,
|
||||
mjtNum * geom_fluid_coef,
|
||||
mjtNum * blunt_drag_coef,
|
||||
mjtNum * slender_drag_coef,
|
||||
mjtNum * ang_drag_coef,
|
||||
mjtNum * kutta_lift_coef,
|
||||
mjtNum * magnus_lift_coef,
|
||||
mjtNum virtual_mass[3],
|
||||
mjtNum virtual_inertia[3]);
|
||||
|
||||
void writeFluidGeomInteraction (mjtNum * geom_fluid_coefs,
|
||||
const mjtNum * geom_fluid_coef,
|
||||
const mjtNum * blunt_drag_coef,
|
||||
const mjtNum * slender_drag_coef,
|
||||
const mjtNum * ang_drag_coef,
|
||||
const mjtNum * kutta_lift_coef,
|
||||
const mjtNum * magnus_lift_coef,
|
||||
const mjtNum virtual_mass[3],
|
||||
const mjtNum virtual_inertia[3]);
|
||||
|
||||
|
||||
//-------------------------- RNE -------------------------------------------------------------------
|
||||
|
||||
// RNE: compute M(qpos)*qacc + C(qpos,qvel); flg_acc=0 removes inertial term
|
||||
|
||||
@@ -19,13 +19,13 @@
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include "engine/engine_core_smooth.h"
|
||||
#include "engine/engine_forward.h"
|
||||
#include "engine/engine_core_constraint.h"
|
||||
#include "engine/engine_crossplatform.h"
|
||||
#include "engine/engine_io.h"
|
||||
#include "engine/engine_inverse.h"
|
||||
#include "engine/engine_macro.h"
|
||||
#include "engine/engine_passive.h"
|
||||
#include "engine/engine_support.h"
|
||||
#include "engine/engine_util_blas.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
|
||||
@@ -28,6 +28,7 @@
|
||||
#include "engine/engine_inverse.h"
|
||||
#include "engine/engine_io.h"
|
||||
#include "engine/engine_macro.h"
|
||||
#include "engine/engine_passive.h"
|
||||
#include "engine/engine_plugin.h"
|
||||
#include "engine/engine_sensor.h"
|
||||
#include "engine/engine_solver.h"
|
||||
|
||||
@@ -23,6 +23,7 @@
|
||||
#include "engine/engine_core_smooth.h"
|
||||
#include "engine/engine_io.h"
|
||||
#include "engine/engine_macro.h"
|
||||
#include "engine/engine_passive.h"
|
||||
#include "engine/engine_sensor.h"
|
||||
#include "engine/engine_support.h"
|
||||
#include "engine/engine_util_blas.h"
|
||||
|
||||
@@ -0,0 +1,509 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "engine/engine_passive.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include "engine/engine_callback.h"
|
||||
#include "engine/engine_core_constraint.h"
|
||||
#include "engine/engine_crossplatform.h"
|
||||
#include "engine/engine_macro.h"
|
||||
#include "engine/engine_plugin.h"
|
||||
#include "engine/engine_support.h"
|
||||
#include "engine/engine_util_blas.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
#include "engine/engine_util_misc.h"
|
||||
#include "engine/engine_util_spatial.h"
|
||||
|
||||
|
||||
//----------------------------- passive forces -----------------------------------------------------
|
||||
|
||||
// all passive forces
|
||||
void mj_passive(const mjModel* m, mjData* d) {
|
||||
int issparse = mj_isSparse(m);
|
||||
int nv = m->nv;
|
||||
mjtNum dif[3], frc, stiffness, damping;
|
||||
|
||||
// clear passive force
|
||||
mju_zero(d->qfrc_passive, m->nv);
|
||||
|
||||
// disabled: return
|
||||
if (mjDISABLED(mjDSBL_PASSIVE)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// joint-level springs
|
||||
for (int i=0; i<m->njnt; i++) {
|
||||
stiffness = m->jnt_stiffness[i];
|
||||
|
||||
int padr = m->jnt_qposadr[i];
|
||||
int dadr = m->jnt_dofadr[i];
|
||||
|
||||
switch (m->jnt_type[i]) {
|
||||
case mjJNT_FREE:
|
||||
// apply force
|
||||
d->qfrc_passive[dadr+0] -= stiffness*(d->qpos[padr+0] - m->qpos_spring[padr+0]);
|
||||
d->qfrc_passive[dadr+1] -= stiffness*(d->qpos[padr+1] - m->qpos_spring[padr+1]);
|
||||
d->qfrc_passive[dadr+2] -= stiffness*(d->qpos[padr+2] - m->qpos_spring[padr+2]);
|
||||
|
||||
// continue with rotations
|
||||
dadr += 3;
|
||||
padr += 3;
|
||||
mjFALLTHROUGH;
|
||||
|
||||
case mjJNT_BALL:
|
||||
// covert quatertion difference into angular "velocity"
|
||||
mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr);
|
||||
|
||||
// apply torque
|
||||
d->qfrc_passive[dadr+0] -= stiffness*dif[0];
|
||||
d->qfrc_passive[dadr+1] -= stiffness*dif[1];
|
||||
d->qfrc_passive[dadr+2] -= stiffness*dif[2];
|
||||
break;
|
||||
|
||||
case mjJNT_SLIDE:
|
||||
case mjJNT_HINGE:
|
||||
// apply force or torque
|
||||
d->qfrc_passive[dadr] -= stiffness*(d->qpos[padr] - m->qpos_spring[padr]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// dof-level dampers
|
||||
for (int i=0; i<m->nv; i++) {
|
||||
damping = m->dof_damping[i];
|
||||
d->qfrc_passive[i] -= damping*d->qvel[i];
|
||||
}
|
||||
|
||||
// tendon-level spring-dampers
|
||||
for (int i=0; i<m->ntendon; i++) {
|
||||
stiffness = m->tendon_stiffness[i];
|
||||
damping = m->tendon_damping[i];
|
||||
|
||||
// compute spring force along tendon
|
||||
mjtNum length = d->ten_length[i];
|
||||
mjtNum lower = m->tendon_lengthspring[2*i];
|
||||
mjtNum upper = m->tendon_lengthspring[2*i+1];
|
||||
if (length > upper) {
|
||||
frc = stiffness * (upper - length);
|
||||
} else if (length < lower) {
|
||||
frc = stiffness * (lower - length);
|
||||
} else {
|
||||
frc = 0;
|
||||
}
|
||||
|
||||
// compute damper linear force along tendon
|
||||
frc -= damping * d->ten_velocity[i];
|
||||
|
||||
// transform to joint torque, add to qfrc_passive: dense or sparse
|
||||
if (issparse) {
|
||||
int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i];
|
||||
for (int j=d->ten_J_rowadr[i]; j<end; j++) {
|
||||
d->qfrc_passive[d->ten_J_colind[j]] += d->ten_J[j] * frc;
|
||||
}
|
||||
} else {
|
||||
mju_addToScl(d->qfrc_passive, d->ten_J+i*nv, frc, nv);
|
||||
}
|
||||
}
|
||||
|
||||
// body-level gravity compensation
|
||||
if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
|
||||
mjtNum force[3], torque[3]={0};
|
||||
|
||||
// apply per-body gravity compensation
|
||||
for (int i=1; i<m->nbody; i++) {
|
||||
if (m->body_gravcomp[i]) {
|
||||
mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i]));
|
||||
mj_applyFT(m, d, force, torque, d->xipos+3*i, i, d->qfrc_passive);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// body-level viscosity, lift and drag
|
||||
if (m->opt.viscosity>0 || m->opt.density>0) {
|
||||
for (int i=1; i<m->nbody; i++) {
|
||||
if (m->body_mass[i]<mjMINVAL) {
|
||||
continue;
|
||||
}
|
||||
|
||||
int use_ellipsoid_model = 0;
|
||||
// if any child geom uses the ellipsoid model, inertia-box model is disabled for parent body
|
||||
for (int j=0; j<m->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) {
|
||||
mj_ellipsoidFluidModel(m, d, i);
|
||||
} else {
|
||||
mj_inertiaBoxFluidModel(m, d, i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// user callback: add custom passive forces
|
||||
if (mjcb_passive) {
|
||||
mjcb_passive(m, d);
|
||||
}
|
||||
|
||||
// plugin
|
||||
if (m->nplugin) {
|
||||
const int nslot = mjp_pluginCount();
|
||||
// iterate over plugins, call compute if type is mjPLUGIN_PASSIVE
|
||||
for (int i=0; i<m->nplugin; i++) {
|
||||
const int slot = m->plugin[i];
|
||||
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
|
||||
if (!plugin) {
|
||||
mju_error_i("invalid plugin slot: %d", slot);
|
||||
}
|
||||
if (plugin->capabilityflags & mjPLUGIN_PASSIVE) {
|
||||
if (!plugin->compute) {
|
||||
mju_error_i("`compute` is a null function pointer for plugin at slot %d", slot);
|
||||
}
|
||||
plugin->compute(m, d, i, mjPLUGIN_PASSIVE);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
//---------------------------------- 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;
|
||||
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);
|
||||
mju_zero(lfrc, 6);
|
||||
|
||||
// set viscous force and torque
|
||||
if (m->opt.viscosity>0) {
|
||||
// diameter of sphere approximation
|
||||
diam = (box[0] + box[1] + box[2])/3.0;
|
||||
|
||||
// angular viscosity
|
||||
mju_scl3(lfrc, lvel, -mjPI*diam*diam*diam*m->opt.viscosity);
|
||||
|
||||
// linear viscosity
|
||||
mju_scl3(lfrc+3, lvel+3, -3.0*mjPI*diam*m->opt.viscosity);
|
||||
}
|
||||
|
||||
// add lift and drag force and torque
|
||||
if (m->opt.density>0) {
|
||||
// force
|
||||
lfrc[3] -= 0.5*m->opt.density*box[1]*box[2]*mju_abs(lvel[3])*lvel[3];
|
||||
lfrc[4] -= 0.5*m->opt.density*box[0]*box[2]*mju_abs(lvel[4])*lvel[4];
|
||||
lfrc[5] -= 0.5*m->opt.density*box[0]*box[1]*mju_abs(lvel[5])*lvel[5];
|
||||
|
||||
// torque
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
}
|
||||
// rotate to global orientation: lfrc -> bfrc
|
||||
mju_rotVecMat(bfrc, lfrc, d->ximat+9*i);
|
||||
mju_rotVecMat(bfrc+3, lfrc+3, d->ximat+9*i);
|
||||
|
||||
// apply force and torque to body com
|
||||
mj_applyFT(m, d, bfrc+3, bfrc, d->xipos+3*i, i, d->qfrc_passive);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 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;
|
||||
mjtNum semiaxes[3], virtual_mass[3], virtual_inertia[3];
|
||||
mjtNum blunt_drag_coef, slender_drag_coef, ang_drag_coef;
|
||||
|
||||
for (int j=0; j<m->body_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);
|
||||
|
||||
// initialize viscous force and torque
|
||||
mju_zero(lfrc, 6);
|
||||
|
||||
// added-mass forces and torques
|
||||
mj_addedMassForces(lvel, NULL, m->opt.density, virtual_mass, virtual_inertia, lfrc);
|
||||
|
||||
// lift force orthogonal to lvel from Kutta-Joukowski theorem
|
||||
mj_viscousForces(lvel, m->opt.density, m->opt.viscosity, semiaxes, magnus_lift_coef,
|
||||
kutta_lift_coef, blunt_drag_coef, slender_drag_coef, ang_drag_coef, lfrc);
|
||||
|
||||
// scale by geom_interaction_coef (1.0 by default)
|
||||
mju_scl(lfrc, lfrc, geom_interaction_coef, 6);
|
||||
|
||||
// rotate to global orientation: lfrc -> bfrc
|
||||
mju_rotVecMat(bfrc, lfrc, d->geom_xmat + 9*geomid);
|
||||
mju_rotVecMat(bfrc+3, lfrc+3, d->geom_xmat + 9*geomid);
|
||||
|
||||
// apply force and torque to body com
|
||||
mj_applyFT(m, d, bfrc+3, bfrc,
|
||||
d->geom_xpos + 3*geomid, // point where FT is generated
|
||||
bodyid, d->qfrc_passive);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// compute forces due to fluid mass moving with the body
|
||||
void mj_addedMassForces(const mjtNum local_vels[6], const mjtNum local_accels[6],
|
||||
const mjtNum fluid_density, const mjtNum virtual_mass[3],
|
||||
const mjtNum virtual_inertia[3], mjtNum local_force[6])
|
||||
{
|
||||
const mjtNum lin_vel[3] = {local_vels[3], local_vels[4], local_vels[5]};
|
||||
const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]};
|
||||
const mjtNum virtual_lin_mom[3] = {
|
||||
fluid_density * virtual_mass[0] * lin_vel[0],
|
||||
fluid_density * virtual_mass[1] * lin_vel[1],
|
||||
fluid_density * virtual_mass[2] * lin_vel[2]
|
||||
};
|
||||
const mjtNum virtual_ang_mom[3] = {
|
||||
fluid_density * virtual_inertia[0] * ang_vel[0],
|
||||
fluid_density * virtual_inertia[1] * ang_vel[1],
|
||||
fluid_density * virtual_inertia[2] * ang_vel[2]
|
||||
};
|
||||
|
||||
// disabled due to dependency on qacc but included for completeness
|
||||
if (local_accels) {
|
||||
local_force[0] -= fluid_density * virtual_inertia[0] * local_accels[0];
|
||||
local_force[1] -= fluid_density * virtual_inertia[1] * local_accels[1];
|
||||
local_force[2] -= fluid_density * virtual_inertia[2] * local_accels[2];
|
||||
local_force[3] -= fluid_density * virtual_mass[0] * local_accels[3];
|
||||
local_force[4] -= fluid_density * virtual_mass[1] * local_accels[4];
|
||||
local_force[5] -= fluid_density * virtual_mass[2] * local_accels[5];
|
||||
}
|
||||
|
||||
mjtNum added_mass_force[3], added_mass_torque1[3], added_mass_torque2[3];
|
||||
mju_cross(added_mass_force, virtual_lin_mom, ang_vel);
|
||||
mju_cross(added_mass_torque1, virtual_lin_mom, lin_vel);
|
||||
mju_cross(added_mass_torque2, virtual_ang_mom, ang_vel);
|
||||
|
||||
mju_addTo3(local_force, added_mass_torque1);
|
||||
mju_addTo3(local_force, added_mass_torque2);
|
||||
mju_addTo3(local_force+3, added_mass_force);
|
||||
}
|
||||
|
||||
|
||||
// inlined helper functions
|
||||
static inline mjtNum mji_pow4(const mjtNum val) {
|
||||
return (val*val)*(val*val);
|
||||
}
|
||||
|
||||
static inline mjtNum mji_pow2(const mjtNum val) {
|
||||
return val*val;
|
||||
}
|
||||
|
||||
static inline mjtNum mji_ellipsoid_max_moment(const mjtNum size[3], const int dir) {
|
||||
const mjtNum d0 = size[dir], d1 = size[(dir+1) % 3], d2 = size[(dir+2) % 3];
|
||||
return 8.0/15.0 * mjPI * d0 * mji_pow4(mju_max(d1, d2));
|
||||
}
|
||||
|
||||
|
||||
|
||||
// lift and drag forces due to motion in the fluid
|
||||
void mj_viscousForces(
|
||||
const mjtNum local_vels[6], const mjtNum fluid_density,
|
||||
const mjtNum fluid_viscosity, const mjtNum size[3],
|
||||
const mjtNum magnus_lift_coef, const mjtNum kutta_lift_coef,
|
||||
const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef,
|
||||
const mjtNum ang_drag_coef, mjtNum local_force[6])
|
||||
{
|
||||
const mjtNum lin_vel[3] = {local_vels[3], local_vels[4], local_vels[5]};
|
||||
const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]};
|
||||
const mjtNum volume = 4.0/3.0 * mjPI * size[0] * size[1] * size[2];
|
||||
const mjtNum d_max = mju_max(mju_max(size[0], size[1]), size[2]);
|
||||
const mjtNum d_min = mju_min(mju_min(size[0], size[1]), size[2]);
|
||||
const mjtNum d_mid = size[0] + size[1] + size[2] - d_max - d_min;
|
||||
const mjtNum A_max = mjPI * d_max * d_mid;
|
||||
|
||||
mjtNum magnus_force[3];
|
||||
mju_cross(magnus_force, ang_vel, lin_vel);
|
||||
magnus_force[0] *= magnus_lift_coef * fluid_density * volume;
|
||||
magnus_force[1] *= magnus_lift_coef * fluid_density * volume;
|
||||
magnus_force[2] *= magnus_lift_coef * fluid_density * volume;
|
||||
|
||||
// the dot product between velocity and the normal to the cross-section that
|
||||
// defines the body's projection along velocity is proj_num/sqrt(proj_denom)
|
||||
const mjtNum proj_denom = mji_pow4(size[1] * size[2]) * mji_pow2(lin_vel[0]) +
|
||||
mji_pow4(size[2] * size[0]) * mji_pow2(lin_vel[1]) +
|
||||
mji_pow4(size[0] * size[1]) * mji_pow2(lin_vel[2]);
|
||||
const mjtNum proj_num = mji_pow2(size[1] * size[2] * lin_vel[0]) +
|
||||
mji_pow2(size[2] * size[0] * lin_vel[1]) +
|
||||
mji_pow2(size[0] * size[1] * lin_vel[2]);
|
||||
|
||||
// projected surface in the direction of the velocity
|
||||
const mjtNum A_proj = mjPI * mju_sqrt(proj_denom/mju_max(mjMINVAL, proj_num));
|
||||
|
||||
// not-unit normal to ellipsoid's projected area in the direction of velocity
|
||||
const mjtNum norm[3] = {
|
||||
mji_pow2(size[1] * size[2]) * lin_vel[0],
|
||||
mji_pow2(size[2] * size[0]) * lin_vel[1],
|
||||
mji_pow2(size[0] * size[1]) * lin_vel[2]
|
||||
};
|
||||
|
||||
// cosine between velocity and normal to the surface
|
||||
// divided by proj_denom instead of sqrt(proj_denom) to account for skipped normalization in norm
|
||||
const mjtNum cos_alpha = proj_num / mju_max(
|
||||
mjMINVAL, mju_norm3(lin_vel) * proj_denom);
|
||||
mjtNum kutta_circ[3];
|
||||
mju_cross(kutta_circ, norm, lin_vel);
|
||||
kutta_circ[0] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj;
|
||||
kutta_circ[1] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj;
|
||||
kutta_circ[2] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj;
|
||||
mjtNum kutta_force[3];
|
||||
mju_cross(kutta_force, kutta_circ, lin_vel);
|
||||
|
||||
// viscous force and torque in Stokes flow, analytical for spherical bodies
|
||||
const mjtNum eq_sphere_D = 2.0/3.0 * (size[0] + size[1] + size[2]);
|
||||
const mjtNum lin_visc_force_coef = 3.0 * mjPI * eq_sphere_D;
|
||||
const mjtNum lin_visc_torq_coef = mjPI * eq_sphere_D*eq_sphere_D*eq_sphere_D;
|
||||
|
||||
// moments of inertia used to compute angular quadratic drag
|
||||
const mjtNum I_max = 8.0/15.0 * mjPI * d_mid * mji_pow4(d_max);
|
||||
const mjtNum II[3] = {
|
||||
mji_ellipsoid_max_moment(size, 0),
|
||||
mji_ellipsoid_max_moment(size, 1),
|
||||
mji_ellipsoid_max_moment(size, 2)
|
||||
};
|
||||
const mjtNum mom_visc[3] = {
|
||||
ang_vel[0] * (ang_drag_coef*II[0] + slender_drag_coef*(I_max - II[0])),
|
||||
ang_vel[1] * (ang_drag_coef*II[1] + slender_drag_coef*(I_max - II[1])),
|
||||
ang_vel[2] * (ang_drag_coef*II[2] + slender_drag_coef*(I_max - II[2]))
|
||||
};
|
||||
|
||||
const mjtNum drag_lin_coef = // linear plus quadratic
|
||||
fluid_viscosity*lin_visc_force_coef + fluid_density*mju_norm3(lin_vel)*(
|
||||
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);
|
||||
|
||||
local_force[0] -= drag_ang_coef * ang_vel[0];
|
||||
local_force[1] -= drag_ang_coef * ang_vel[1];
|
||||
local_force[2] -= drag_ang_coef * ang_vel[2];
|
||||
local_force[3] += magnus_force[0] + kutta_force[0] - drag_lin_coef*lin_vel[0];
|
||||
local_force[4] += magnus_force[1] + kutta_force[1] - drag_lin_coef*lin_vel[1];
|
||||
local_force[5] += magnus_force[2] + kutta_force[2] - drag_lin_coef*lin_vel[2];
|
||||
}
|
||||
|
||||
|
||||
|
||||
// read the geom_fluid_coefs array into its constituent parts
|
||||
void readFluidGeomInteraction(const mjtNum* geom_fluid_coefs,
|
||||
mjtNum* geom_fluid_coef,
|
||||
mjtNum* blunt_drag_coef,
|
||||
mjtNum* slender_drag_coef,
|
||||
mjtNum* ang_drag_coef,
|
||||
mjtNum* kutta_lift_coef,
|
||||
mjtNum* magnus_lift_coef,
|
||||
mjtNum virtual_mass[3],
|
||||
mjtNum virtual_inertia[3]) {
|
||||
int i = 0;
|
||||
geom_fluid_coef[0] = geom_fluid_coefs[i++];
|
||||
blunt_drag_coef[0] = geom_fluid_coefs[i++];
|
||||
slender_drag_coef[0] = geom_fluid_coefs[i++];
|
||||
ang_drag_coef[0] = geom_fluid_coefs[i++];
|
||||
kutta_lift_coef[0] = geom_fluid_coefs[i++];
|
||||
magnus_lift_coef[0] = geom_fluid_coefs[i++];
|
||||
virtual_mass[0] = geom_fluid_coefs[i++];
|
||||
virtual_mass[1] = geom_fluid_coefs[i++];
|
||||
virtual_mass[2] = geom_fluid_coefs[i++];
|
||||
virtual_inertia[0] = geom_fluid_coefs[i++];
|
||||
virtual_inertia[1] = geom_fluid_coefs[i++];
|
||||
virtual_inertia[2] = geom_fluid_coefs[i++];
|
||||
if (i != mjNFLUID) {
|
||||
mju_error("Error in reading geom_fluid_coefs: wrong number of entries.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// write components into geom_fluid_coefs array
|
||||
void writeFluidGeomInteraction (mjtNum* geom_fluid_coefs,
|
||||
const mjtNum* geom_fluid_coef,
|
||||
const mjtNum* blunt_drag_coef,
|
||||
const mjtNum* slender_drag_coef,
|
||||
const mjtNum* ang_drag_coef,
|
||||
const mjtNum* kutta_lift_coef,
|
||||
const mjtNum* magnus_lift_coef,
|
||||
const mjtNum virtual_mass[3],
|
||||
const mjtNum virtual_inertia[3]) {
|
||||
int i = 0;
|
||||
geom_fluid_coefs[i++] = geom_fluid_coef[0];
|
||||
geom_fluid_coefs[i++] = blunt_drag_coef[0];
|
||||
geom_fluid_coefs[i++] = slender_drag_coef[0];
|
||||
geom_fluid_coefs[i++] = ang_drag_coef[0];
|
||||
geom_fluid_coefs[i++] = kutta_lift_coef[0];
|
||||
geom_fluid_coefs[i++] = magnus_lift_coef[0];
|
||||
geom_fluid_coefs[i++] = virtual_mass[0];
|
||||
geom_fluid_coefs[i++] = virtual_mass[1];
|
||||
geom_fluid_coefs[i++] = virtual_mass[2];
|
||||
geom_fluid_coefs[i++] = virtual_inertia[0];
|
||||
geom_fluid_coefs[i++] = virtual_inertia[1];
|
||||
geom_fluid_coefs[i++] = virtual_inertia[2];
|
||||
if (i != mjNFLUID) {
|
||||
mju_error("Error in writing geom_fluid_coefs: wrong number of entries.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_SRC_ENGINE_ENGINE_PASSIVE_H_
|
||||
#define MUJOCO_SRC_ENGINE_ENGINE_PASSIVE_H_
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjexport.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
//------------------------- passive forces ---------------------------------------------------------
|
||||
|
||||
// all passive forces
|
||||
MJAPI void mj_passive(const mjModel* m, mjData* d);
|
||||
|
||||
|
||||
//------------------------- fluid models -----------------------------------------------------------
|
||||
|
||||
// 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)
|
||||
void mj_addedMassForces(
|
||||
const mjtNum local_vels[6], const mjtNum local_accels[6],
|
||||
mjtNum fluid_density, const mjtNum virtual_mass[3],
|
||||
const mjtNum virtual_inertia[3], mjtNum local_force[6]);
|
||||
|
||||
// compute forces due to viscous effects
|
||||
void mj_viscousForces(
|
||||
const mjtNum local_vels[6], mjtNum fluid_density,
|
||||
mjtNum fluid_viscosity, const mjtNum size[3],
|
||||
mjtNum magnus_lift_coef, mjtNum kutta_lift_coef,
|
||||
mjtNum blunt_drag_coef, mjtNum slender_drag_coef,
|
||||
mjtNum ang_drag_coef, mjtNum local_force[6]);
|
||||
|
||||
void readFluidGeomInteraction(const mjtNum* geom_fluid_coefs,
|
||||
mjtNum* geom_fluid_coef,
|
||||
mjtNum* blunt_drag_coef,
|
||||
mjtNum* slender_drag_coef,
|
||||
mjtNum* ang_drag_coef,
|
||||
mjtNum* kutta_lift_coef,
|
||||
mjtNum* magnus_lift_coef,
|
||||
mjtNum virtual_mass[3],
|
||||
mjtNum virtual_inertia[3]);
|
||||
|
||||
void writeFluidGeomInteraction (mjtNum* geom_fluid_coefs,
|
||||
const mjtNum* geom_fluid_coef,
|
||||
const mjtNum* blunt_drag_coef,
|
||||
const mjtNum* slender_drag_coef,
|
||||
const mjtNum* ang_drag_coef,
|
||||
const mjtNum* kutta_lift_coef,
|
||||
const mjtNum* magnus_lift_coef,
|
||||
const mjtNum virtual_mass[3],
|
||||
const mjtNum virtual_inertia[3]);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // MUJOCO_SRC_ENGINE_ENGINE_PASSIVE_H_
|
||||
@@ -32,6 +32,7 @@
|
||||
#include "engine/engine_file.h"
|
||||
#include "engine/engine_io.h"
|
||||
#include "engine/engine_macro.h"
|
||||
#include "engine/engine_passive.h"
|
||||
#include "engine/engine_plugin.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
#include "engine/engine_util_misc.h"
|
||||
|
||||
@@ -46,6 +46,9 @@ target_link_libraries(
|
||||
absl::str_format
|
||||
)
|
||||
|
||||
mujoco_test(engine_passive_test)
|
||||
target_link_libraries(engine_passive_test fixture gmock)
|
||||
|
||||
mujoco_test(engine_print_test)
|
||||
target_link_libraries(engine_print_test fixture gmock)
|
||||
|
||||
|
||||
@@ -243,201 +243,5 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
|
||||
// ------------------------ ellipsoid fluid model ------------------------------
|
||||
|
||||
using EllipsoidFluidTest = MujocoTest;
|
||||
|
||||
TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
|
||||
static constexpr char two_bodies_xml[] = R"(
|
||||
<mujoco>
|
||||
<option wind="5 5 0" density="10"/>
|
||||
<worldbody>
|
||||
<body>
|
||||
<freejoint/>
|
||||
<body>
|
||||
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
mjModel* m2 = LoadModelFromString(two_bodies_xml);
|
||||
mjData* d2 = mj_makeData(m2);
|
||||
for (int i = 0; i < 6; i++) {
|
||||
d2->qvel[i] = (mjtNum) i+1;
|
||||
}
|
||||
d2->qpos[3] = 0.5;
|
||||
d2->qpos[4] = 0.5;
|
||||
d2->qpos[5] = 0.5;
|
||||
d2->qpos[6] = 0.5;
|
||||
|
||||
static constexpr char one_body_xml[] = R"(
|
||||
<mujoco>
|
||||
<option wind="5 5 0" density="10"/>
|
||||
<worldbody>
|
||||
<body pos="1 2 3">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
|
||||
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
mjModel* m1 = LoadModelFromString(one_body_xml);
|
||||
mjData* d1 = mj_makeData(m1);
|
||||
for (int i = 0; i < 6; i++) {
|
||||
d1->qvel[i] = (mjtNum) i+1;
|
||||
}
|
||||
d1->qpos[3] = 0.5;
|
||||
d1->qpos[4] = 0.5;
|
||||
d1->qpos[5] = 0.5;
|
||||
d1->qpos[6] = 0.5;
|
||||
|
||||
const mjtNum tol = 1e-14; // tolerance for floating point numbers
|
||||
|
||||
EXPECT_EQ(m1->nv, m2->nv);
|
||||
|
||||
mj_forward(m2, d2);
|
||||
mj_forward(m1, d1);
|
||||
for (int i = 0; i < m1->nv; i++) {
|
||||
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
|
||||
}
|
||||
|
||||
mj_deleteData(d1);
|
||||
mj_deleteModel(m1);
|
||||
mj_deleteData(d2);
|
||||
mj_deleteModel(m2);
|
||||
}
|
||||
|
||||
TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option wind="5 5 0" density="10"/>
|
||||
<default>
|
||||
<geom fluidshape="ellipsoid" fluidcoef="2 3 4 5 6"/>
|
||||
<default class="test_class">
|
||||
<geom fluidshape="none" fluidcoef="5 4 3 2 1"/>
|
||||
</default>
|
||||
</default>
|
||||
<worldbody>
|
||||
<body>
|
||||
<freejoint/>
|
||||
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" class="test_class"/>
|
||||
<geom type="box" size=".1 .01 0.01" pos="-0.1 0 0"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
mjModel* model = LoadModelFromString(xml);
|
||||
EXPECT_THAT(GetVector(model->geom_fluid, 6),
|
||||
ElementsAre(0, 0, 0, 0, 0, 0));
|
||||
EXPECT_THAT(GetVector(model->geom_fluid + mjNFLUID, 6),
|
||||
ElementsAre(1, 2, 3, 4, 5, 6));
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// -------------------------- adhesion actuators -------------------------------
|
||||
|
||||
using AdhesionTest = MujocoTest;
|
||||
|
||||
TEST_F(AdhesionTest, ExpectedAdhesionForce) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option gravity="0 0 -1"/>
|
||||
|
||||
<worldbody>
|
||||
<body name="static">
|
||||
<!-- small increase to size to ensure contact -->
|
||||
<geom size=".02001" pos=" .01 .01 .07"/>
|
||||
<geom size=".02001" pos="-.01 .01 .07"/>
|
||||
<geom size=".02001" pos=" .01 -.01 .07"/>
|
||||
<geom size=".02001" pos="-.01 -.01 .07"/>
|
||||
</body>
|
||||
<body name="free">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".05 .05 .05" mass="1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<adhesion body="static" ctrlrange="0 2"/>
|
||||
<adhesion body="free" ctrlrange="0 2"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
mjModel* model = LoadModelFromString(xml);
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
// iterate over cone type
|
||||
for (mjtCone cone : {mjCONE_ELLIPTIC, mjCONE_PYRAMIDAL}) {
|
||||
// set cone
|
||||
model->opt.cone = cone;
|
||||
// iterate over condim
|
||||
for (int condim : {1, 3, 4, 6}) {
|
||||
// set condim
|
||||
for (int id=0; id < model->ngeom; id++) {
|
||||
model->geom_condim[id] = condim;
|
||||
}
|
||||
// iterate over actuators
|
||||
for (int id=0; id < 2; id++) {
|
||||
// set ctrl > 1, expect free body to not fall
|
||||
mj_resetData(model, data);
|
||||
data->ctrl[id] = 1.01;
|
||||
for (int i = 0; i < 100; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
// moved down at most 10 microns
|
||||
EXPECT_GT(data->qpos[2], -1e-5);
|
||||
|
||||
// set ctrl < 1, expect free body to fall below 1cm
|
||||
mj_resetData(model, data);
|
||||
data->ctrl[id] = 0.99;
|
||||
for (int i = 0; i < 100; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
// fell lower than 1cm
|
||||
EXPECT_LT(data->qpos[2], -0.01);
|
||||
}
|
||||
}
|
||||
}
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
|
||||
// ------------------------------ tendons --------------------------------------
|
||||
|
||||
using TendonTest = MujocoTest;
|
||||
|
||||
// check tendon spring deadband using example model
|
||||
TEST_F(TendonTest, SpringrangeDeadband) {
|
||||
const std::string xml_path =
|
||||
GetTestDataFilePath("engine/testdata/tendon_springlength.xml");
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||
ASSERT_THAT(model, NotNull());
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
// initial state outside deadband: spring is active
|
||||
mj_forward(model, data);
|
||||
mjtNum expected_force = model->tendon_stiffness[0] *
|
||||
(model->tendon_lengthspring[1] - data->ten_length[0]);
|
||||
EXPECT_EQ(expected_force, data->qfrc_passive[0]);
|
||||
|
||||
// put body inside deadband: spring is inactive
|
||||
data->qpos[0] = -1;
|
||||
mj_forward(model, data);
|
||||
EXPECT_EQ(0, data->qfrc_passive[0]);
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -492,5 +492,74 @@ TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) {
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// -------------------------- adhesion actuators -------------------------------
|
||||
|
||||
using AdhesionTest = MujocoTest;
|
||||
|
||||
TEST_F(AdhesionTest, ExpectedAdhesionForce) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option gravity="0 0 -1"/>
|
||||
|
||||
<worldbody>
|
||||
<body name="static">
|
||||
<!-- small increase to size to ensure contact -->
|
||||
<geom size=".02001" pos=" .01 .01 .07"/>
|
||||
<geom size=".02001" pos="-.01 .01 .07"/>
|
||||
<geom size=".02001" pos=" .01 -.01 .07"/>
|
||||
<geom size=".02001" pos="-.01 -.01 .07"/>
|
||||
</body>
|
||||
<body name="free">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".05 .05 .05" mass="1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<adhesion body="static" ctrlrange="0 2"/>
|
||||
<adhesion body="free" ctrlrange="0 2"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
mjModel* model = LoadModelFromString(xml);
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
// iterate over cone type
|
||||
for (mjtCone cone : {mjCONE_ELLIPTIC, mjCONE_PYRAMIDAL}) {
|
||||
// set cone
|
||||
model->opt.cone = cone;
|
||||
// iterate over condim
|
||||
for (int condim : {1, 3, 4, 6}) {
|
||||
// set condim
|
||||
for (int id=0; id < model->ngeom; id++) {
|
||||
model->geom_condim[id] = condim;
|
||||
}
|
||||
// iterate over actuators
|
||||
for (int id=0; id < 2; id++) {
|
||||
// set ctrl > 1, expect free body to not fall
|
||||
mj_resetData(model, data);
|
||||
data->ctrl[id] = 1.01;
|
||||
for (int i = 0; i < 100; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
// moved down at most 10 microns
|
||||
EXPECT_GT(data->qpos[2], -1e-5);
|
||||
|
||||
// set ctrl < 1, expect free body to fall below 1cm
|
||||
mj_resetData(model, data);
|
||||
data->ctrl[id] = 0.99;
|
||||
for (int i = 0; i < 100; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
// fell lower than 1cm
|
||||
EXPECT_LT(data->qpos[2], -0.01);
|
||||
}
|
||||
}
|
||||
}
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -0,0 +1,166 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
// Tests for engine/engine_core_smooth.c.
|
||||
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "test/fixture.h"
|
||||
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::testing::ElementsAre;
|
||||
using ::testing::NotNull;
|
||||
using CoreSmoothTest = MujocoTest;
|
||||
|
||||
static std::vector<mjtNum> GetVector(const mjtNum* array, int length) {
|
||||
return std::vector<mjtNum>(array, array + length);
|
||||
}
|
||||
|
||||
|
||||
// ------------------------ ellipsoid fluid model ------------------------------
|
||||
|
||||
using EllipsoidFluidTest = MujocoTest;
|
||||
|
||||
TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
|
||||
static constexpr char two_bodies_xml[] = R"(
|
||||
<mujoco>
|
||||
<option wind="5 5 0" density="10"/>
|
||||
<worldbody>
|
||||
<body>
|
||||
<freejoint/>
|
||||
<body>
|
||||
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
mjModel* m2 = LoadModelFromString(two_bodies_xml);
|
||||
mjData* d2 = mj_makeData(m2);
|
||||
for (int i = 0; i < 6; i++) {
|
||||
d2->qvel[i] = (mjtNum) i+1;
|
||||
}
|
||||
d2->qpos[3] = 0.5;
|
||||
d2->qpos[4] = 0.5;
|
||||
d2->qpos[5] = 0.5;
|
||||
d2->qpos[6] = 0.5;
|
||||
|
||||
static constexpr char one_body_xml[] = R"(
|
||||
<mujoco>
|
||||
<option wind="5 5 0" density="10"/>
|
||||
<worldbody>
|
||||
<body pos="1 2 3">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
|
||||
<geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
mjModel* m1 = LoadModelFromString(one_body_xml);
|
||||
mjData* d1 = mj_makeData(m1);
|
||||
for (int i = 0; i < 6; i++) {
|
||||
d1->qvel[i] = (mjtNum) i+1;
|
||||
}
|
||||
d1->qpos[3] = 0.5;
|
||||
d1->qpos[4] = 0.5;
|
||||
d1->qpos[5] = 0.5;
|
||||
d1->qpos[6] = 0.5;
|
||||
|
||||
const mjtNum tol = 1e-14; // tolerance for floating point numbers
|
||||
|
||||
EXPECT_EQ(m1->nv, m2->nv);
|
||||
|
||||
mj_forward(m2, d2);
|
||||
mj_forward(m1, d1);
|
||||
for (int i = 0; i < m1->nv; i++) {
|
||||
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
|
||||
}
|
||||
|
||||
mj_deleteData(d1);
|
||||
mj_deleteModel(m1);
|
||||
mj_deleteData(d2);
|
||||
mj_deleteModel(m2);
|
||||
}
|
||||
|
||||
TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option wind="5 5 0" density="10"/>
|
||||
<default>
|
||||
<geom fluidshape="ellipsoid" fluidcoef="2 3 4 5 6"/>
|
||||
<default class="test_class">
|
||||
<geom fluidshape="none" fluidcoef="5 4 3 2 1"/>
|
||||
</default>
|
||||
</default>
|
||||
<worldbody>
|
||||
<body>
|
||||
<freejoint/>
|
||||
<geom type="box" size=".1 .01 0.01" pos="0.1 0 0" class="test_class"/>
|
||||
<geom type="box" size=".1 .01 0.01" pos="-0.1 0 0"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
mjModel* model = LoadModelFromString(xml);
|
||||
EXPECT_THAT(GetVector(model->geom_fluid, 6),
|
||||
ElementsAre(0, 0, 0, 0, 0, 0));
|
||||
EXPECT_THAT(GetVector(model->geom_fluid + mjNFLUID, 6),
|
||||
ElementsAre(1, 2, 3, 4, 5, 6));
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
|
||||
// ------------------------------ tendons --------------------------------------
|
||||
|
||||
using TendonTest = MujocoTest;
|
||||
|
||||
// check tendon spring deadband using example model
|
||||
TEST_F(TendonTest, SpringrangeDeadband) {
|
||||
const std::string xml_path =
|
||||
GetTestDataFilePath("engine/testdata/tendon_springlength.xml");
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||
ASSERT_THAT(model, NotNull());
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
// initial state outside deadband: spring is active
|
||||
mj_forward(model, data);
|
||||
mjtNum expected_force = model->tendon_stiffness[0] *
|
||||
(model->tendon_lengthspring[1] - data->ten_length[0]);
|
||||
EXPECT_EQ(expected_force, data->qfrc_passive[0]);
|
||||
|
||||
// put body inside deadband: spring is inactive
|
||||
data->qpos[0] = -1;
|
||||
mj_forward(model, data);
|
||||
EXPECT_EQ(0, data->qfrc_passive[0]);
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
Reference in New Issue
Block a user