From ac1f41160dbe2e837c2ba4cc3cfd333704e598da Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 19 Jun 2023 03:28:44 -0700 Subject: [PATCH] Add engine internal error macro mjERROR to prepend calling function to error function. PiperOrigin-RevId: 541584847 Change-Id: I57447e64375413ee07aa96372a402e1ced59ff4a --- python/mujoco/bindings_test.py | 3 +- python/mujoco/rollout_test.py | 4 +- src/engine/engine_collision_convex.c | 2 +- src/engine/engine_collision_driver.c | 29 +++++++++----- src/engine/engine_core_constraint.c | 20 +++++----- src/engine/engine_core_smooth.c | 10 ++--- src/engine/engine_derivative.c | 4 +- src/engine/engine_derivative_fd.c | 8 ++-- src/engine/engine_forward.c | 14 +++---- src/engine/engine_io.c | 60 ++++++++++++++-------------- src/engine/engine_passive.c | 8 ++-- src/engine/engine_print.c | 2 +- src/engine/engine_ray.c | 20 +++++----- src/engine/engine_resource.c | 6 +-- src/engine/engine_sensor.c | 24 +++++------ src/engine/engine_setconst.c | 2 +- src/engine/engine_solver.c | 6 +-- src/engine/engine_support.c | 23 +++++------ src/engine/engine_util_errmem.c | 17 +++++--- src/engine/engine_util_errmem.h | 41 +++++++++++++++++++ src/engine/engine_util_misc.c | 2 +- src/engine/engine_util_solve.c | 25 ++++++------ src/engine/engine_util_spatial.c | 2 +- src/engine/engine_vfs.c | 8 ++-- src/engine/engine_vis_init.c | 4 +- src/engine/engine_vis_interact.c | 18 ++++----- src/engine/engine_vis_state.c | 6 +-- src/engine/engine_vis_visualize.c | 12 +++--- 28 files changed, 218 insertions(+), 162 deletions(-) diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 8813e294..d016c355 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -926,7 +926,8 @@ Euler integrator, semi-implicit in velocity. def test_can_raise_error(self): self.data.pstack = self.data.nstack - with self.assertRaisesRegex(mujoco.FatalError, r'\Astack overflow'): + with self.assertRaisesRegex(mujoco.FatalError, + r'\Amj_stackAlloc: stack overflow'): mujoco.mj_forward(self.model, self.data) def test_mjcb_time(self): diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index fc34f590..50ca4568 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -369,8 +369,8 @@ class MuJoCoRolloutTest(parameterized.TestCase): ctrl = np.zeros((3, model.nu)) model.opt.solver = 10 # invalid solver type - with self.assertRaisesWithLiteralMatch(mujoco.FatalError, - 'Unknown solver type 10'): + with self.assertRaisesWithLiteralMatch( + mujoco.FatalError, 'mj_fwdConstraint: unknown solver type 10'): state, sensordata = rollout.rollout(model, data, initial_state, ctrl) def test_invalid(self): diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 497a5b22..2f8472f1 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -191,7 +191,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { break; default: - mju_error("ccd support function is undefined for geom type %d", m->geom_type[g]); + mjERROR("ccd support function is undefined for geom type %d", m->geom_type[g]); } // add dir*margin/2 to result diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 229aeefe..980f25bf 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -274,7 +274,9 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2, if (!isleaf1 && isleaf2) { for (int i=0; i < 2; i++) { if (child1[2*node1+i] != -1) { - if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR + if (nstack >= max_stack) { + mjERROR("BVH stack depth exceeded."); // SHOULD NOT OCCUR + } stack[nstack].node1 = child1[2*node1+i]; stack[nstack].node2 = node2; nstack++; @@ -283,7 +285,9 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2, } else if (isleaf1 && !isleaf2) { for (int i=0; i < 2; i++) { if (child2[2*node2+i] != -1) { - if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR + if (nstack >= max_stack) { + mjERROR("BVH stack depth exceeded."); // SHOULD NOT OCCUR + } stack[nstack].node1 = node1; stack[nstack].node2 = child2[2*node2+i]; nstack++; @@ -304,7 +308,9 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2, if (surface1 > surface2) { for (int i = 0; i < 2; i++) { if (child1[2 * node1 + i] != -1) { - if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR + if (nstack >= max_stack) { + mjERROR("BVH stack depth exceeded."); // SHOULD NOT OCCUR + } stack[nstack].node1 = child1[2 * node1 + i]; stack[nstack].node2 = node2; nstack++; @@ -313,7 +319,9 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2, } else { for (int i = 0; i < 2; i++) { if (child2[2 * node2 + i] != -1) { - if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR + if (nstack >= max_stack) { + mjERROR("BVH stack depth exceeded."); // SHOULD NOT OCCUR + } stack[nstack].node1 = node1; stack[nstack].node2 = child2[2*node2+i]; nstack++; @@ -590,7 +598,7 @@ static void add_pair(const mjModel* m, int b1, int b2, int* npair, int* pair, in (*npair)++; } else { - mju_error("Broadphase buffer full"); + mjERROR("broadphase buffer full"); } } @@ -767,7 +775,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { // sanity check; SHOULD NOT OCCUR if (k != bufcnt) { - mju_error("Internal error in broadphase: unexpected bufcnt"); + mjERROR("internal error: unexpected bufcnt"); } // sort along axis0 @@ -939,7 +947,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, // check number of contacts, SHOULD NOT OCCUR if (num > mjMAXCONPAIR) { - mju_error("Too many contacts returned by collision function"); + mjERROR("too many contacts returned by collision function"); } // remove repeated contacts in box-box @@ -1065,10 +1073,11 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, // add contact returned by collision detector for (int i=0; i < num; i++) { - // set contact data - if (condim > 6 || condim < 1) { // SHOULD NOT OCCUR - mju_error("Invalid condim value: %d", i); + if (condim > 6 || condim < 1) { + mjERROR("invalid condim value: %d", i); // SHOULD NOT OCCUR } + + // set contact data con[i].dim = condim; con[i].geom1 = g1; con[i].geom2 = g2; diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 7b37377b..16660554 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -203,7 +203,7 @@ int mj_addConstraint(const mjModel* m, mjData* d, // chain required in sparse mode if (NV && !chain) { - mju_error("Sparse mj_addConstraint called with dense arguments"); + mjERROR("called with dense arguments"); } // process size elements @@ -556,7 +556,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; default: // SHOULD NOT OCCUR - mju_error("Invalid equality constraint type %d", m->eq_type[i]); + mjERROR("invalid equality constraint type %d", m->eq_type[i]); } // add constraint @@ -932,7 +932,7 @@ void mj_diagApprox(const mjModel* m, mjData* d) { break; default: - mju_error("Unknown constraint type type %d", d->efc_type[i]); // SHOULD NOT OCCUR + mjERROR("unknown constraint type type %d", d->efc_type[i]); // SHOULD NOT OCCUR } break; @@ -1646,29 +1646,29 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { // check sparse allocation if (mj_isSparse(m)) { if (d->ne != ne_allocated) { - mju_error("ne mis-allocation: found ne=%d but allocated %d", d->ne, ne_allocated); + mjERROR("ne mis-allocation: found ne=%d but allocated %d", d->ne, ne_allocated); } if (d->nf != nf_allocated) { - mju_error("nf mis-allocation: found nf=%d but allocated %d", d->nf, nf_allocated); + mjERROR("nf mis-allocation: found nf=%d but allocated %d", d->nf, nf_allocated); } // check that nefc was computed correctly if (d->nefc != nefc_allocated) { - mju_error("nefc mis-allocation: found nefc=%d but allocated %d", d->nefc, nefc_allocated); + mjERROR("nefc mis-allocation: found nefc=%d but allocated %d", d->nefc, nefc_allocated); } // check that nnzJ was computed correctly if (d->nefc > 0) { int nnzJ = d->efc_J_rownnz[d->nefc - 1] + d->efc_J_rowadr[d->nefc - 1]; if (d->nnzJ != nnzJ) { - mju_error("constraint Jacobian mis-allocation: found nnzJ=%d but allocated %d", - nnzJ, d->nnzJ); + mjERROR("constraint Jacobian mis-allocation: found nnzJ=%d but allocated %d", + nnzJ, d->nnzJ); } } } else if (d->nefc > nefc_allocated) { - mju_error("nefc under-allocation: found nefc=%d but allocated only %d", - d->nefc, nefc_allocated); + mjERROR("nefc under-allocation: found nefc=%d but allocated only %d", + d->nefc, nefc_allocated); } // collect memory use statistics diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 2297d609..6572f4f2 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -135,7 +135,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { break; default: - mju_error("Unknown joint type %d", jtype); // SHOULD NOT OCCUR + mjERROR("unknown joint type %d", jtype); // SHOULD NOT OCCUR } // assign xanchor and xaxis @@ -514,7 +514,7 @@ void mj_tendon(const mjModel* m, mjData* d) { // do wrapping, possibly get 2 extra points (wlen>=0) sideid = mju_round(m->wrap_prm[adr+j+1]); if (sideid < -1 || sideid >= m->nsite) { - mju_error("Invalid sideid %d in wrap_prm", sideid); // SHOULD NOT OCCUR + mjERROR("invalid sideid %d in wrap_prm", sideid); // SHOULD NOT OCCUR } wlen = mju_wrap(wpnt+3, d->site_xpos+3*id0, d->site_xpos+3*id1, @@ -943,7 +943,7 @@ void mj_transmission(const mjModel* m, mjData* d) { break; default: - mju_error("Unknown transmission type %d", m->actuator_trntype[i]); // SHOULD NOT OCCUR + mjERROR("unknown transmission type %d", m->actuator_trntype[i]); // SHOULD NOT OCCUR } } @@ -1503,7 +1503,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { int i = 0; while (i < d->ne) { if (d->efc_type[i] != mjCNSTR_EQUALITY) - mju_error("Row %d of efc is not an equality constraint", i); // SHOULD NOT OCCUR + mjERROR("row %d of efc is not an equality constraint", i); // SHOULD NOT OCCUR int id = d->efc_id[i]; mjtNum* eq_data = m->eq_data + mjNEQDATA*id; @@ -1555,7 +1555,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { break; default: - mju_error("Unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR + mjERROR("unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR } } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index da1771c9..cb14f667 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -471,7 +471,7 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { // child nonzeroes must be subset of parent; SHOULD NOT OCCUR else { - mju_error("Error in addToParent: child nonzeroes must be subset of parent"); + mjERROR("child nonzeroes must be subset of parent"); } } } @@ -539,7 +539,7 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D break; default: - mju_error("mjd_comVel_vel: Unknown joint type"); + mjERROR("unknown joint type"); } } } diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index 141e0d26..3f89e983 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -169,7 +169,7 @@ void mj_stepSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) { break; default: - mju_error("Invalid integrator"); + mjERROR("invalid integrator"); } TM_END(mjTIMER_STEP); @@ -294,7 +294,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { // make sure final row counters equal rownnz for (int i=0; i < nv; i++) { if (cnt[i] != d->D_rownnz[i]) { - mju_error("error in constructing FD sparse derivative"); + mjERROR("error in constructing FD sparse derivative"); } } @@ -637,11 +637,11 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio mjMARKSTACK; if (m->opt.integrator == mjINT_RK4) { - mju_error("RK4 integrator is not supported by mjd_inverseFD"); + mjERROR("RK4 integrator is not supported"); } if (m->opt.noslip_iterations) { - mju_error("The noslip solver is not supported by mjd_inverseFD"); + mjERROR("noslip solver is not supported"); } // skip sensor computations if no sensor Jacobians requested diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index dab719f0..242eb35a 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -279,11 +279,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { - mju_error("invalid plugin slot: %d", slot); + mjERROR("invalid plugin slot: %d", slot); } if (plugin->capabilityflags & mjPLUGIN_ACTUATOR) { if (!plugin->compute) { - mju_error("`compute` is a null function pointer for plugin at slot %d", slot); + mjERROR("`compute` is a null function pointer for plugin at slot %d", slot); } plugin->compute(m, d, i, mjPLUGIN_ACTUATOR); } @@ -481,7 +481,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) { break; default: - mju_error("Unknown solver type %d", m->opt.solver); + mjERROR("unknown solver type %d", m->opt.solver); } // save result for next step warmstart @@ -534,7 +534,7 @@ static void mj_advance(const mjModel* m, mjData* d, const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { - mju_error("invalid plugin slot: %d", slot); + mjERROR("invalid plugin slot: %d", slot); } if (plugin->advance) { plugin->advance(m, d, i); @@ -624,7 +624,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { // check order if (!A) { - mju_error("Supported RK orders: N=4"); + mjERROR("supported RK orders: N=4"); } // allocate space for intermediate solutions @@ -758,7 +758,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mju_copy(qacc, qfrc, m->nv); mj_solveLD(m, qacc, 1, d->qH, d->qHDiagInv); } else { - mju_error("mj_implicitSkip: integrator must be implicit or implicitfast"); + mjERROR("integrator must be implicit or implicitfast"); } // advance state and time @@ -859,7 +859,7 @@ void mj_step(const mjModel* m, mjData* d) { break; default: - mju_error("Invalid integrator"); + mjERROR("invalid integrator"); } TM_END(mjTIMER_STEP); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 4fb089cd..8622a47a 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -281,12 +281,12 @@ static int getnptr(void) { static void bufwrite(const void* src, int num, int szbuf, void* buf, int* ptrbuf) { // check pointers if (!src || !buf || !ptrbuf) { - mju_error("NULL pointer passed to bufwrite"); + mjERROR("NULL pointer passed to bufwrite"); } // check size if (*ptrbuf+num > szbuf) { - mju_error("Attempting to write outside model buffer"); + mjERROR("attempting to write outside model buffer"); } // write, advance pointer @@ -300,12 +300,12 @@ static void bufwrite(const void* src, int num, int szbuf, void* buf, int* ptrbuf static void bufread(void* dest, int num, int szbuf, const void* buf, int* ptrbuf) { // check pointers if (!dest || !buf || !ptrbuf) { - mju_error("NULL pointer passed to bufread"); + mjERROR("NULL pointer passed to bufread"); } // check size if (*ptrbuf+num > szbuf) { - mju_error("Attempting to read outside model buffer"); + mjERROR("attempting to read outside model buffer"); } // read, advance pointer @@ -347,7 +347,7 @@ static void mj_setPtrModel(mjModel* m) { sz = (int)(ptr - (char*)m->buffer); if (m->nbuffer != sz) { printf("expected size: %d, actual size: %d\n", m->nbuffer, sz); - mju_error("mjModel buffer size mismatch"); + mjERROR("mjModel buffer size mismatch"); } } @@ -399,7 +399,7 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n // allocate mjModel mjModel* m = (mjModel*)mju_malloc(sizeof(mjModel)); if (!m) { - mju_error("Could not allocate mjModel"); + mjERROR("could not allocate mjModel"); } memset(m, 0, sizeof(mjModel)); @@ -502,7 +502,7 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n m->buffer = mju_malloc(m->nbuffer); if (!m->buffer) { mju_free(m); - mju_error("Could not allocate mjModel buffer"); + mjERROR("could not allocate mjModel buffer"); } // clear, set pointers in buffer @@ -543,13 +543,13 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { src->nuser_sensor, src->nnames); } if (!dest) { - mju_error("Failed to make mjModel. Invalid sizes."); + mjERROR("failed to make mjModel. Invalid sizes."); } // check sizes if (dest->nbuffer != src->nbuffer) { mj_deleteModel(dest); - mju_error("dest and src models have different buffer size"); + mjERROR("dest and src models have different buffer size"); } // save buffer ptr, copy everything, restore buffer and other pointers @@ -745,7 +745,7 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) { int index = mj_registerVfsProvider(vfs); if (index < 1) { - mju_error("mj_loadModel: could not allocate memory"); + mjERROR("could not allocate memory"); return NULL; } @@ -840,7 +840,7 @@ static void makeDSparse(const mjModel* m, mjData* d) { // sanity check; SHOULD NOT OCCUR for (int i = 0; i < nv; i++) { if (remaining[i] != 0) { - mju_error("Error in mj_makeDSparse: unexpected remaining"); + mjERROR("unexpected remaining"); } } @@ -865,7 +865,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { // sanity check; SHOULD NOT OCCUR if (rownnz[0] != nv) { - mju_error("Error in mj_makeBSparse: rownnz[0] different from nv"); + mjERROR("rownnz[0] different from nv"); } // add dofs in ancestors bodies @@ -885,7 +885,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { // sanity check; SHOULD NOT OCCUR if (m->nB != rowadr[nbody - 1] + rownnz[nbody - 1]) { - mju_error("Error in mj_makeBSparse: sum of rownnz different from nB"); + mjERROR("sum of rownnz different from nB"); } // allocate and clear incremental row counts @@ -928,7 +928,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { for (int i = 0; i < nbody; i++) { // make sure cnt = rownnz; SHOULD NOT OCCUR if (rownnz[i] != cnt[i]) { - mju_error("Error in mj_makeBSparse: cnt different from rownnz"); + mjERROR("cnt different from rownnz"); } // sort colind in each row @@ -951,11 +951,11 @@ static void checkDBSparse(const mjModel* m, mjData* d) { // D[row j] and B[row i] should be identical if (d->D_rownnz[j] != d->B_rownnz[i]) { - mju_error("Error in checkDBSparse: rows have different nnz"); + mjERROR("rows have different nnz"); } for (int k = 0; k < d->D_rownnz[j]; k++) { if (d->D_colind[d->D_rowadr[j] + k] != d->B_colind[d->B_rowadr[i] + k]) { - mju_error("Error in checkDBSparse: rows have different colind"); + mjERROR("rows have different colind"); } } } @@ -985,7 +985,7 @@ static void mj_setPtrData(const mjModel* m, mjData* d) { // check size sz = (int)(ptr - (char*)d->buffer); if (d->nbuffer != sz) { - mju_error("mjData buffer size mismatch"); + mjERROR("mjData buffer size mismatch"); } // zero-initialize arena pointers @@ -1005,7 +1005,7 @@ static mjData* _makeData(const mjModel* m) { // allocate mjData mjData* d = (mjData*) mju_malloc(sizeof(mjData)); if (!d) { - mju_error("Could not allocate mjData"); + mjERROR("could not allocate mjData"); } // prepare symbols needed by xmacro @@ -1031,7 +1031,7 @@ static mjData* _makeData(const mjModel* m) { d->buffer = mju_malloc(d->nbuffer); if (!d->buffer) { mju_free(d); - mju_error("Could not allocate mjData buffer"); + mjERROR("could not allocate mjData buffer"); } // allocate arena @@ -1039,7 +1039,7 @@ static mjData* _makeData(const mjModel* m) { if (!d->arena) { mju_free(d->buffer); mju_free(d); - mju_error("Could not allocate mjData arena"); + mjERROR("could not allocate mjData arena"); } // set pointers into buffer, reset data @@ -1055,7 +1055,7 @@ static mjData* _makeData(const mjModel* m) { mju_free(d->buffer); mju_free(d->arena); mju_free(d); - mju_error("plugin->init failed for plugin id %d", i); + mjERROR("plugin->init failed for plugin id %d", i); } } } @@ -1084,15 +1084,15 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { // check sizes if (dest->nbuffer != src->nbuffer) { - mju_error("dest and src data buffers have different size"); + mjERROR("dest and src data buffers have different size"); } if (dest->nstack != src->nstack) { - mju_error("dest and src stacks have different size"); + mjERROR("dest and src stacks have different size"); } // stack is in use if (src->pstack) { - mju_error("Attempting to copy mjData while stack is in use"); + mjERROR("attempting to copy mjData while stack is in use"); } // save pointers, copy everything, restore pointers @@ -1109,7 +1109,7 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { if (plugin_data_size) { save_plugin_data = (uintptr_t*)mju_malloc(plugin_data_size); if (!save_plugin_data) { - mju_error("failed to allocate temporary memory for plugin_data"); + mjERROR("failed to allocate temporary memory for plugin_data"); } memcpy(save_plugin_data, dest->plugin_data, plugin_data_size); } @@ -1200,7 +1200,7 @@ mjtNum* mj_stackAlloc(mjData* d, int size) { size_t stack_available_bytes = d->nstack * sizeof(mjtNum) - d->parena; size_t stack_required_bytes = (d->pstack + size + 2*mjREDZONE) * sizeof(mjtNum); if (stack_required_bytes > stack_available_bytes) { - mju_error("stack overflow: max = %zu, available = %zu, requested = %zu " + mjERROR("stack overflow: max = %zu, available = %zu, requested = %zu " "(ne = %d, nf = %d, nefc = %d, ncon = %d)", d->nstack * sizeof(mjtNum), stack_available_bytes, stack_required_bytes, d->ne, d->nf, d->nefc, d->ncon); @@ -1217,7 +1217,7 @@ mjtNum* mj_stackAlloc(mjData* d, int size) { #ifdef ADDRESS_SANITIZER if ((uintptr_t)result % sizeof(mjtNum)) { - mju_error("mj_stackAlloc fails to align to sizeof(mjtNum)"); + mjERROR("mj_stackAlloc fails to align to sizeof(mjtNum)"); } // actual stack usage (without red zone bytes) is stored in the red zone @@ -1775,7 +1775,7 @@ const char* mj_validateReferences(const mjModel* m) { break; default: - mju_error("mj_validateReferences: unknown equality constraint type."); + mjERROR("unknown equality constraint type."); } } for (int i=0; i < m->nwrap; i++) { @@ -1843,8 +1843,8 @@ const char* mj_validateReferences(const mjModel* m) { if (sensor_type == mjSENS_PLUGIN) { const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[m->sensor_plugin[i]]); if (!plugin->nsensordata) { - mju_error("`nsensordata` is a null function pointer for plugin at slot %d", - m->plugin[m->sensor_plugin[i]]); + mjERROR("`nsensordata` is a null function pointer for plugin at slot %d", + m->plugin[m->sensor_plugin[i]]); } sensor_size = plugin->nsensordata(m, m->sensor_plugin[i], i); } else { diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 597cdb0a..95fa5b0a 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -168,11 +168,11 @@ void mj_passive(const mjModel* m, mjData* d) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { - mju_error("invalid plugin slot: %d", slot); + mjERROR("invalid plugin slot: %d", slot); } if (plugin->capabilityflags & mjPLUGIN_PASSIVE) { if (!plugin->compute) { - mju_error("`compute` is a null function pointer for plugin at slot %d", slot); + mjERROR("`compute` is a null function pointer for plugin at slot %d", slot); } plugin->compute(m, d, i, mjPLUGIN_PASSIVE); } @@ -474,7 +474,7 @@ void readFluidGeomInteraction(const mjtNum* geom_fluid_coefs, 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."); + mjERROR("wrong number of entries."); } } @@ -504,6 +504,6 @@ void writeFluidGeomInteraction (mjtNum* geom_fluid_coefs, 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."); + mjERROR("wrong number of entries."); } } diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 0de90613..564559f6 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -735,7 +735,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, // stack in use, SHOULD NOT OCCUR if (d->pstack) { - mju_error("Attempting to print mjData when stack is in use"); + mjERROR("attempting to print mjData when stack is in use"); } // get file diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 83893350..d892d72e 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -452,7 +452,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, const mjtNum* vec) { // check geom type if (m->geom_type[id] != mjGEOM_HFIELD) { - mju_error("mj_rayHfield: geom with hfield type expected"); + mjERROR("geom with hfield type expected"); } // hfield id and dimensions @@ -632,7 +632,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, const int* child = m->bvh_child + 2*bvhadr; if (meshid == -1) { - mju_error("mju_rayTree: mesh id of geom %d is -1", meshid); // SHOULD NOT OCCUR + mjERROR("mesh id of geom %d is -1", meshid); // SHOULD NOT OCCUR } // initialize stack @@ -709,7 +709,9 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, // add children to the stack for (int i=0; i < 2; i++) { if (child[2*node+i] != -1) { - if (nstack >= mjMAXTREEDEPTH) mju_error("BVH stack depth exceeded in geom %d.", id); + if (nstack >= mjMAXTREEDEPTH) { + mjERROR("BVH stack depth exceeded in geom %d.", id); + } stack[nstack] = child[2*node+i]; nstack++; } @@ -724,7 +726,7 @@ mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, const mjtNum* vec) { // check geom type if (m->geom_type[id] != mjGEOM_MESH) { - mju_error("mj_rayMesh: geom with mesh type expected"); + mjERROR("geom with mesh type expected"); } // bounding box test @@ -760,7 +762,7 @@ mjtNum mju_rayGeom(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, return ray_box(pos, mat, size, pnt, vec, NULL); default: - mju_error("mju_rayGeom: unexpected geom type %d", geomtype); + mjERROR("unexpected geom type %d", geomtype); return -1; } } @@ -894,7 +896,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum // check vector length if (mju_norm3(vec) < mjMINVAL) { - mju_error("mj_ray: vector length is too small"); + mjERROR("vector length is too small"); } // clear result @@ -934,7 +936,7 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum* ray_xmat, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, mjtNum cutoff, mjtNum* geom_ba, int* geom_eliminate) { if (ray_xmat) { - mju_error("ray_xmat is currently unused, should be NULL"); + mjERROR("ray_xmat is currently unused, should be NULL"); } // compute eliminate flag for all geoms @@ -1005,7 +1007,7 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], } if (AABB[3]-AABB[1] > mjPI) { // SHOULD NOT OCCUR - mju_error("mj_ray: discontinuity in azimuth angle"); + mjERROR("discontinuity in azimuth angle"); } mju_copy(geom_ba+4*g, AABB, 4); @@ -1021,7 +1023,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co // check vector length if (mju_norm3(vec) < mjMINVAL) { - mju_error("mj_ray: vector length is too small"); + mjERROR("vector length is too small"); } // clear result diff --git a/src/engine/engine_resource.c b/src/engine/engine_resource.c index 538f3520..eba0ad62 100644 --- a/src/engine/engine_resource.c +++ b/src/engine/engine_resource.c @@ -53,7 +53,7 @@ mjResource* mju_openResource(const char* name, int default_provider) { mjResource* resource = (mjResource*) mju_malloc(sizeof(mjResource)); const mjpResourceProvider* provider = NULL; if (resource == NULL) { - mju_error("mju_openResource: could not allocate memory"); + mjERROR("could not allocate memory"); return NULL; } @@ -61,7 +61,7 @@ mjResource* mju_openResource(const char* name, int default_provider) { resource->name = mju_malloc(sizeof(char) * (strlen(name) + 1)); if (resource->name == NULL) { mju_free(resource); - mju_error("mju_openResource: could not allocate memory"); + mjERROR("could not allocate memory"); return NULL; } strcpy(resource->name, name); @@ -249,7 +249,7 @@ void* mju_fileToMemory(const char* filename, int* filesize) { // allocate and read void* buffer = mju_malloc(*filesize); if (!buffer) { - mju_error("mjFileToMemory: could not allocate memory"); + mjERROR("could not allocate memory"); } size_t bytes_read = fread(buffer, 1, *filesize, fp); diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index dd0d7551..da6ee152 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -96,7 +96,7 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) { // unknown datatype else { - mju_error("Unknown datatype in sensor %d", i); + mjERROR("unknown datatype in sensor %d", i); } } } @@ -158,7 +158,7 @@ static void get_xpos_xmat(const mjData* d, int type, int id, int sensor_id, *xmat = d->cam_xmat + 9*id; break; default: - mju_error("Invalid object type in sensor %d", sensor_id); + mjERROR("invalid object type in sensor %d", sensor_id); } } @@ -182,7 +182,7 @@ static void get_xquat(const mjModel* m, const mjData* d, int type, int id, int s mju_mulQuat(quat, d->xquat+4*m->cam_bodyid[id], m->cam_quat+4*id); break; default: - mju_error("Invalid object type in sensor %d", sensor_id); + mjERROR("invalid object type in sensor %d", sensor_id); } } @@ -334,7 +334,7 @@ void mj_sensorPos(const mjModel* m, mjData* d) { break; default: - mju_error("Invalid sensor type in POS stage, sensor %d", i); + mjERROR("invalid sensor type in POS stage, sensor %d", i); } } } @@ -356,12 +356,12 @@ void mj_sensorPos(const mjModel* m, mjData* d) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { - mju_error("invalid plugin slot: %d", slot); + mjERROR("invalid plugin slot: %d", slot); } if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && (plugin->needstage == mjSTAGE_POS || plugin->needstage == mjSTAGE_NONE)) { if (!plugin->compute) { - mju_error("`compute` is a null function pointer for plugin at slot %d", slot); + mjERROR("`compute` is a null function pointer for plugin at slot %d", slot); } plugin->compute(m, d, i, mjPLUGIN_SENSOR); } @@ -515,7 +515,7 @@ void mj_sensorVel(const mjModel* m, mjData* d) { break; default: - mju_error("Invalid type in VEL stage, sensor %d", i); + mjERROR("invalid type in VEL stage, sensor %d", i); } } } @@ -537,11 +537,11 @@ void mj_sensorVel(const mjModel* m, mjData* d) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { - mju_error("invalid plugin slot: %d", slot); + mjERROR("invalid plugin slot: %d", slot); } if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage == mjSTAGE_VEL) { if (!plugin->compute) { - mju_error("`compute` is null for plugin at slot %d", slot); + mjERROR("`compute` is null for plugin at slot %d", slot); } if (subtreeVel == 0) { // compute subtree_linvel, subtree_angmom @@ -724,7 +724,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { break; default: - mju_error("Invalid type in ACC stage, sensor %d", i); + mjERROR("invalid type in ACC stage, sensor %d", i); } } } @@ -746,11 +746,11 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { - mju_error("invalid plugin slot: %d", slot); + mjERROR("invalid plugin slot: %d", slot); } if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage == mjSTAGE_ACC) { if (!plugin->compute) { - mju_error("`compute` is null for plugin at slot %d", slot); + mjERROR("`compute` is null for plugin at slot %d", slot); } if (rnePost == 0) { // compute cacc, cfrc_int, cfrc_ext diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 1fbd1172..44b72d30 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -436,7 +436,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index, const mjLROpt* opt, char* error, int error_sz) { // check index if (index < 0 || index >= m->nu) { - mju_error("Invalid actuator index in mj_setLengthRange"); + mjERROR("invalid actuator index"); } // skip depending on mode and type diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 7e3784ba..f8fd516f 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -132,7 +132,7 @@ static void extractBlock(const mjModel* m, mjData* d, mjtNum* Ac, // sanity check; SHOULD NOT OCCUR if (k >= rownnz[start]) { - mju_error("Internal error in extractComponent"); + mjERROR("internal error"); } // copy rows @@ -1384,7 +1384,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { // rank-defficient, SHOULD NOT OCCUR if (rank != nv) { - mju_error("Rank-defficient Hessian in HessianDirect"); + mjERROR("rank-defficient Hessian"); } // compress layout of H @@ -1396,7 +1396,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { ctx->nnz += ctx->rownnz[i]; } if (ctx->nnz > nv*nv) { // SHOULD NOT OCCUR - mju_error("More nonzero values than elements in sparse direct-solver Hessian"); + mjERROR("more nonzero values than elements in sparse direct-solver Hessian"); } } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index bc8a2411..96f3ecd9 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -107,7 +107,7 @@ static inline int mj_stateElemSize(const mjModel* m, mjtState spec) { case mjSTATE_USERDATA: return m->nuserdata; case mjSTATE_PLUGIN: return m->npluginstate; default: - mju_error("mju_stateElementSize: invalid state element %u", spec); + mjERROR("invalid state element %u", spec); return 0; } } @@ -130,7 +130,7 @@ static inline mjtNum* mj_stateElemPtr(const mjModel* m, mjData* d, mjtState spec case mjSTATE_USERDATA: return d->userdata; case mjSTATE_PLUGIN: return d->plugin_state; default: - mju_error("mju_stateElemPtr: invalid state element %u", spec); + mjERROR("invalid state element %u", spec); return NULL; } } @@ -146,7 +146,7 @@ static inline const mjtNum* mj_stateElemConstPtr(const mjModel* m, const mjData* // get size of state specification int mj_stateSize(const mjModel* m, unsigned int spec) { if (spec >= (1<= 2^mjNSTATE", spec); + mjERROR("invalid state spec %u >= 2^mjNSTATE", spec); } int size = 0; @@ -165,7 +165,7 @@ int mj_stateSize(const mjModel* m, unsigned int spec) { // get state void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigned int spec) { if (spec >= (1<= 2^mjNSTATE", spec); + mjERROR("invalid state spec %u >= 2^mjNSTATE", spec); } int adr = 0; @@ -185,7 +185,7 @@ void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigned int // set state void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int spec) { if (spec >= (1<= 2^mjNSTATE", spec); + mjERROR("invalid state spec %u >= 2^mjNSTATE", spec); } int adr = 0; @@ -383,7 +383,7 @@ void mj_jacSparse(const mjModel* m, const mjData* d, // make sure we found it; SHOULD NOT OCCUR if (chain[ci] != da) { - mju_error("dof index %d not found in chain", da); + mjERROR("dof index %d not found in chain", da); } // construct rotation jacobian @@ -993,7 +993,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst, // not found: error if (adr >= end) { - mju_error("mj_addM sparse: dst row expected to be empty"); + mjERROR("dst row expected to be empty"); } } } @@ -1184,7 +1184,7 @@ void mj_applyFT(const mjModel* m, mjData* d, // make sure body is in range if (body < 0 || body >= m->nbody) { - mju_error("Invalid body %d in applyFT", body); + mjERROR("invalid body %d", body); } // compute Jacobians @@ -1259,7 +1259,7 @@ void mj_objectVelocity(const mjModel* m, const mjData* d, // object without spatial frame else { - mju_error("Invalid object type %d in mj_objectVelocity", objtype); + mjERROR("invalid object type %d", objtype); } // transform velocity @@ -1312,7 +1312,7 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d, // object without spatial frame else { - mju_error("Invalid object type %d in mj_objectAcceleration", objtype); + mjERROR("invalid object type %d", objtype); } // transform com-based velocity to local frame @@ -1514,10 +1514,9 @@ void mj_setTotalmass(mjModel* m, mjtNum newmass) { // count warnings, print only the first time void mj_warning(mjData* d, int warning, int info) { - // check type if (warning < 0 || warning >= mjNWARNING) { - mju_error("Invalid warning type %d", warning); + mjERROR("invalid warning type %d", warning); } // save info (override previous) diff --git a/src/engine/engine_util_errmem.c b/src/engine/engine_util_errmem.c index a858bdcd..125a64d8 100644 --- a/src/engine/engine_util_errmem.c +++ b/src/engine/engine_util_errmem.c @@ -123,16 +123,11 @@ void mju_writeLog(const char* type, const char* msg) { } } - -// write message to logfile and console, pause and exit -void mju_error(const char* msg, ...) { +void mju_error_v(const char* msg, va_list args) { char errmsg[1000]; // Format msg into errmsg - va_list args; - va_start(args, msg); vsnprintf(errmsg, mjSIZEOFARRAY(errmsg), msg, args); - va_end(args); if (_mjPRIVATE_tls_error_fn) { _mjPRIVATE_tls_error_fn(errmsg); @@ -150,6 +145,16 @@ void mju_error(const char* msg, ...) { } +// write message to logfile and console, pause and exit +void mju_error(const char* msg, ...) { + va_list args; + va_start(args, msg); + mju_error_v(msg, args); + va_end(args); +} + + + // write message to logfile and console void mju_warning(const char* msg, ...) { char wrnmsg[1000]; diff --git a/src/engine/engine_util_errmem.h b/src/engine/engine_util_errmem.h index 493137b0..44f1aec5 100644 --- a/src/engine/engine_util_errmem.h +++ b/src/engine/engine_util_errmem.h @@ -15,7 +15,9 @@ #ifndef MUJOCO_SRC_ENGINE_ENGINE_UTIL_ERRMEM_H_ #define MUJOCO_SRC_ENGINE_ENGINE_UTIL_ERRMEM_H_ +#include #include +#include #include #include @@ -53,6 +55,7 @@ MJAPI void _mjPRIVATE__set_tls_warning_fn(void (*h)(const char*)); // errors MJAPI void mju_error(const char* msg, ...) mjPRINTFLIKE(1, 2); +MJAPI void mju_error_v(const char* msg, va_list args); MJAPI void mju_error_i(const char* msg, int i); MJAPI void mju_error_s(const char* msg, const char* text); @@ -64,6 +67,44 @@ MJAPI void mju_warning_s(const char* msg, const char* text); // write [datetime, type: message] to MUJOCO_LOG.TXT MJAPI void mju_writeLog(const char* type, const char* msg); +//------------------------------ internal error macros -------------------------------------------- + +// need at least c99 or c++11 +#if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L) || \ + (defined(__cplusplus) && __cplusplus >= 201103L) + + // macro to get the first argument + #define _GET_MSG(msg, ...) msg + + // helper function for the mjERROR macro + // formats buf as '{prefix}: {msg}' and passes along to mju_error_v + static inline void _mju_error_prefix(char *buf, size_t nbuf, const char* prefix, + const char* msg, ...) mjPRINTFLIKE(4, 5); + + static inline void _mju_error_prefix(char *buf, size_t nbuf, const char* prefix, + const char* msg, ...) { + snprintf(buf, nbuf, "%s: %s", prefix, msg); + va_list args; + va_start(args, msg); + mju_error_v(buf, args); + va_end(args); + } + + // macro to get first argument + #define _GET_MSG(msg, ...) msg + + // internal macro to prepend the calling function name to the error message + // standard support for variadic macros with zero arguments is only now + // supported in C23 and C++20 so we rely on a helper function to get around this + // in a portable way + #define mjERROR(...) { \ + char _buf[sizeof(_GET_MSG(__VA_ARGS__)) + sizeof(__func__) + 1]; \ + _mju_error_prefix(_buf, sizeof(_buf), __func__, __VA_ARGS__); \ + } + +#else + #define mjERROR mju_error +#endif // c99 or c++11 //------------------------------ malloc and free --------------------------------------------------- diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index befc92fe..44dca4b4 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -287,7 +287,7 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1, // check object type; SHOULD NOT OCCUR if (type != mjWRAP_SPHERE && type != mjWRAP_CYLINDER) { - mju_error("mju_wrap: unknown wrapping object type %d", type); + mjERROR("unknown wrapping object type %d", type); } // map sites to wrap object's local frame diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 8c6a51fd..2d6c5b90 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -161,7 +161,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, // check if (rownnz[r] == 0 || colind[rowadr[r]+rownnz[r]-1] != r) { - mju_error("Matrix must have non-zero diagonal in mju_cholFactorSparse"); + mjERROR("matrix must have non-zero diagonal"); } } @@ -284,7 +284,7 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, // check for size change if (new_nnz != nnz-1) { - mju_error("Varying sparsity pattern in mju_cholUpdateSparse"); + mjERROR("varying sparsity pattern"); } // update x: x(1:r-1) = c*x(1:r-1) - s*mat(r,1:r-1) @@ -603,12 +603,12 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch, // make sure ii is on diagonal if (colind[ii] != i) { - mju_error("missing diagonal element in mju_factorLUSparse"); + mjERROR("missing diagonal element"); } // make sure diagonal is not too small if (mju_abs(LU[ii]) < mjMINVAL) { - mju_error("diagonal element too small in mju_factorLUSparse"); + mjERROR("diagonal element too small"); } // rows j above i @@ -642,13 +642,13 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch, // only (i,k) non-zero else { - mju_error("mju_factorLUSparse requires fill-in"); + mjERROR("requires fill-in"); } } // make sure both rows fully processed if (icnt != rowadr[i]+remaining[i] || jcnt != rowadr[j]+remaining[j]) { - mju_error("row processing incomplete in mju_factorLUSparse"); + mjERROR("row processing incomplete"); } } } @@ -657,7 +657,7 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch, // make sure remaining points to diagonal for (int i=0; i < n; i++) { if (remaining[i] < 0 || colind[rowadr[i]+remaining[i]] != i) { - mju_error("unexpected sparse matrix structure in mju_factorLUSparse"); + mjERROR("unexpected sparse matrix structure"); } } } @@ -681,7 +681,7 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n, // make sure j points to diagonal if (colind[rowadr[i]+j] != i) { - mju_error("diagonal of U not reached in mju_factorLUSparse"); + mjERROR("diagonal of U not reached"); } } @@ -699,7 +699,7 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n, // make sure j points to diagonal if (colind[rowadr[i]+j] != i) { - mju_error("diagonal of L not reached in mju_factorLUSparse"); + mjERROR("diagonal of L not reached"); } } } @@ -975,7 +975,7 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, // check size if (n > 5) { - mju_error("mju_QCQP supports n up to 5"); + mjERROR("n is only supported up to 5"); } // scale A,b so that constraint becomes x'*x <= r*r @@ -1151,12 +1151,12 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs // basic checks if (n <= 0) { - mju_error("mju_boxQP: problem size n must be positive"); + mjERROR("problem size n must be positive"); } if (upper && lower) { for (int i=0; i < n; i++) { if (lower[i] >= upper[i]) { - mju_error("mju_boxQP: upper bounds must be stricly larger than lower bounds"); + mjERROR("upper bounds must be stricly larger than lower bounds"); } } } @@ -1390,4 +1390,3 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs // return nf or -1 if failure return (status == mjBOXQP_NO_DESCENT || status == mjBOXQP_NOT_SPD) ? -1 : nfree; } - diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index 29a4fbae..247a44d2 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -477,7 +477,7 @@ void mju_makeFrame(mjtNum frame[9]) { // normalize xaxis if (mju_normalize3(frame) < 0.5) { - mju_error("xaxis of contact frame undefined"); + mjERROR("xaxis of contact frame undefined"); } // if yaxis undefined, set yaxis to (0,1,0) if possible, otherwise (0,0,1) diff --git a/src/engine/engine_vfs.c b/src/engine/engine_vfs.c index 6371283e..5dcf1ad2 100644 --- a/src/engine/engine_vfs.c +++ b/src/engine/engine_vfs.c @@ -37,10 +37,10 @@ static void vfs_strippath(char* newname, const char* oldname) { // check resulting length if (sz-(i+1) >= mjMAXVFSNAME) { - mju_error("Filename too long in VFS"); + mjERROR("filename too long"); } if (sz-(i+1) <= 0) { - mju_error("Empty filename in VFS"); + mjERROR("empty filename"); } // copy @@ -117,7 +117,7 @@ int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) { // check filesize if (filesize <= 0) { - mju_error("mj_makeEmptyFileVFS expects positive filesize"); + mjERROR("expects positive filesize"); } // strip path @@ -137,7 +137,7 @@ int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) { // allocate and clear vfs->filedata[vfs->nfile] = mju_malloc(filesize); if (!vfs->filedata[vfs->nfile]) { - mju_error("mj_makeEmptyFileVFS: could not allocate memory"); + mjERROR("could not allocate memory"); } memset(vfs->filedata[vfs->nfile], 0, filesize); diff --git a/src/engine/engine_vis_init.c b/src/engine/engine_vis_init.c index c2a9c975..a581c9c1 100644 --- a/src/engine/engine_vis_init.c +++ b/src/engine/engine_vis_init.c @@ -127,7 +127,7 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) { // check allocation if (!scn->geoms || !scn->geomorder) { - mju_error("Could not allocate geom buffers"); + mjERROR("could not allocate geom buffers"); } } @@ -169,7 +169,7 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) { !scn->skinvertnum || !scn->skinvert || !scn->skinnormal) { - mju_error("Could not allocate skin buffers"); + mjERROR("could not allocate skin buffers"); } // copy constant data diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index cdfc6279..81d5a6e5 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -38,7 +38,7 @@ void mjv_room2model(mjtNum* modelpos, mjtNum* modelquat, const mjtNum* roompos, // check scale if (scn->scale < mjMINVAL) { - mju_error("mjvScene scale too small in mjv_room2model"); + mjERROR("mjvScene scale too small"); } // enabled: transform @@ -73,7 +73,7 @@ void mjv_model2room(mjtNum* roompos, mjtNum* roomquat, const mjtNum* modelpos, // check scale if (scn->scale < mjMINVAL) { - mju_error("mjvScene scale too small in mjv_model2room"); + mjERROR("mjvScene scale too small"); } // enabled: transform @@ -105,7 +105,7 @@ void mjv_cameraInModel(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSc // check znear if (scn->camera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) { - mju_error("mjvScene frustum_near too small in mjv_cameraInModel"); + mjERROR("mjvScene frustum_near too small"); } // clear results @@ -178,7 +178,7 @@ void mjv_cameraInRoom(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSce // check znear if (scn->camera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) { - mju_error("mjvScene frustum_near too small in mjv_cameraInRoom"); + mjERROR("mjvScene frustum_near too small"); } // clear results @@ -228,7 +228,7 @@ mjtNum mjv_frustumHeight(const mjvScene* scn) { // check znear if (scn->camera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) { - mju_error("mjvScene frustum_near too small in mjv_frustumHeight"); + mjERROR("mjvScene frustum_near too small"); } // add normalized height for left and right cameras @@ -295,7 +295,7 @@ static void convert2D(mjtNum* res, int action, mjtNum dx, mjtNum dy, const mjtNu break; default: - mju_error("Unexpected mouse action %d in convert2D", action); + mjERROR("unexpected mouse action %d in convert2D", action); } // call 3D converter @@ -347,7 +347,7 @@ void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, break; default: - mju_error("Unexpected action %d in mjv_moveCamera", action); + mjERROR("unexpected action %d", action); } // clamp camera parameters @@ -434,7 +434,7 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx break; default: - mju_error("Unexpected mouse action %d in mjv_movePerturb", action); + mjERROR("unexpected mouse action %d", action); } } @@ -510,7 +510,7 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, break; default: - mju_error("Unexpected action %d in mjv_moveModel", action); + mjERROR("unexpected action %d", action); } } diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index 2020dc46..d10e1a66 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -118,7 +118,7 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta // should not occur if (ptr - (char*)scnstate->buffer != scnstate->nbuffer) { - mju_error("Unexpected error: mjvSceneState buffer is not fully used"); + mjERROR("mjvSceneState buffer is not fully used"); } mjv_makeScene(m, &scnstate->plugincache, maxgeom); @@ -236,7 +236,7 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt, #define X(var) #define XMJV(var) \ if (scnstate->model.var != m->var) { \ - mju_error("m->%s changed", #var); \ + mjERROR("m->%s changed", #var); \ } MJMODEL_INTS #undef XMJV @@ -251,7 +251,7 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt, const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { - mju_error("invalid plugin slot: %d", slot); + mjERROR("invalid plugin slot: %d", slot); } if (plugin->visualize) { plugin->visualize(m, d, opt, &scnstate->plugincache, i); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 19edfc0b..10d182fc 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -303,7 +303,7 @@ void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, if (type != mjGEOM_CAPSULE && type != mjGEOM_CYLINDER && type != mjGEOM_ARROW && type != mjGEOM_ARROW1 && type != mjGEOM_ARROW2 && type != mjGEOM_LINE) { - mju_error("Invalid geom type %d for connector", type); + mjERROR("invalid geom type %d for connector", type); } // assign type @@ -898,7 +898,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, break; default: - mju_error("Unknown joint type %d in mjv_visualize", m->jnt_type[i]); + mjERROR("unknown joint type %d", m->jnt_type[i]); } f2f(thisgeom->rgba, m->vis.rgba.joint, 4); @@ -1894,7 +1894,7 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn // get id and check int bid = cam->trackbodyid; if (bid < 0 || bid >= m->nbody) { - mju_error("Track body id is outside valid range"); + mjERROR("track body id is outside valid range"); } // smooth tracking of subtree com @@ -1923,7 +1923,7 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn // get id and check int cid = cam->fixedcamid; if (cid < 0 || cid >= m->ncam) { - mju_error("Fixed camera id is outside valid range"); + mjERROR("fixed camera id is outside valid range"); } // get camera-specific ipd and fovy @@ -1948,7 +1948,7 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn break; default: - mju_error("Unknown camera type in mjv_updateCamera"); + mjERROR("unknown camera type"); } // compute GL cameras @@ -2124,7 +2124,7 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, const int slot = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); if (!plugin) { - mju_error("invalid plugin slot: %d", slot); + mjERROR("invalid plugin slot: %d", slot); } if (plugin->visualize) { plugin->visualize(m, d, opt, scn, i);