diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 23fe51ad..45296ecc 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -22,6 +22,7 @@ #include #include // IWYU pragma: keep #include +#include "engine/engine_init.h" #include "engine/engine_core_util.h" #include "engine/engine_core_smooth.h" #include "engine/engine_memory.h" diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index b846e609..7d2c17b1 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -726,27 +726,6 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) { } -// count warnings, print only the first time -void mj_warning(mjData* d, int warning, int info) { - // check type - if (warning < 0 || warning >= mjNWARNING) { - mjERROR("invalid warning type %d", warning); - } - - // save info (override previous) - d->warning[warning].lastinfo = info; - - // print message only the first time this warning is encountered - if (!d->warning[warning].number) { - mju_warning("%s Time = %.4f.", mju_warningText(warning, info), d->time); - } - - // increase counter - d->warning[warning].number++; -} - - - // compute object 6D velocity in object-centered frame, world/local orientation void mj_objectVelocity(const mjModel* m, const mjData* d, int objtype, int objid, mjtNum res[6], int flg_local) { @@ -857,27 +836,6 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d, } -// extract 6D force:torque for one contact, in contact frame -void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]) { - mjContact* con; - - // clear result - mju_zero(result, 6); - - // make sure contact is valid - if (id >= 0 && id < d->ncon && d->contact[id].efc_address >= 0) { - // get contact pointer - con = d->contact + id; - - if (mj_isPyramidal(m)) { - mju_decodePyramid(result, d->efc_force + con->efc_address, con->friction, con->dim); - } else { - mju_copy(result, d->efc_force + con->efc_address, con->dim); - } - } -} - - // map from body local to global Cartesian coordinates void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], const mjtNum pos[3], const mjtNum quat[4], @@ -923,3 +881,43 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], } +// extract 6D force:torque for one contact, in contact frame +void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]) { + mjContact* con; + + // clear result + mju_zero(result, 6); + + // make sure contact is valid + if (id >= 0 && id < d->ncon && d->contact[id].efc_address >= 0) { + // get contact pointer + con = d->contact + id; + + if (mj_isPyramidal(m)) { + mju_decodePyramid(result, d->efc_force + con->efc_address, con->friction, con->dim); + } else { + mju_copy(result, d->efc_force + con->efc_address, con->dim); + } + } +} + + +// count warnings, print only the first time +void mj_warning(mjData* d, int warning, int info) { + // check type + if (warning < 0 || warning >= mjNWARNING) { + mjERROR("invalid warning type %d", warning); + } + + // save info (override previous) + d->warning[warning].lastinfo = info; + + // print message only the first time this warning is encountered + if (!d->warning[warning].number) { + mju_warning("%s Time = %.4f.", mju_warningText(warning, info), d->time); + } + + // increase counter + d->warning[warning].number++; +} + diff --git a/src/engine/engine_core_util.h b/src/engine/engine_core_util.h index b39a67a3..91a735d6 100644 --- a/src/engine/engine_core_util.h +++ b/src/engine/engine_core_util.h @@ -23,6 +23,9 @@ extern "C" { #endif + +//-------------------------- model properties ------------------------------------------------------ + // determine type of friction cone MJAPI int mj_isPyramidal(const mjModel* m); @@ -125,11 +128,6 @@ MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum res // high-level warning function: count warnings in mjData, print only the first time MJAPI void mj_warning(mjData* d, int warning, int info); -// extract 6D force:torque for one contact, in contact frame -MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); - -// set default solver parameters -MJAPI void mj_defaultSolRefImp(mjtNum* solref, mjtNum* solimp); #ifdef __cplusplus } diff --git a/src/user/user_init.c b/src/user/user_init.c index f4ad79b4..acd9d3dd 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -16,7 +16,7 @@ #include #include #include -#include "engine/engine_core_util.h" +#include "engine/engine_init.h" #include "engine/engine_io.h" #include "user/user_api.h"