From 19388eee0ca0f80b0f413b93cf5a8d54bec4838f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 16 Jun 2024 12:34:08 -0700 Subject: [PATCH] Fix type errors in engine and plugins. - Preparation for a float32 build of MuJoCo. - Fix use of `float`-typed `fabs` in `mju_eig3`. PiperOrigin-RevId: 643804942 Change-Id: I89f64e8fd39f4e70e78d7607816283e217912383 --- doc/includes/references.h | 2 +- include/mujoco/mjplugin.h | 2 +- plugin/actuator/pid.cc | 2 +- plugin/sdf/sdf.cc | 4 ++-- plugin/sensor/touch_grid.cc | 16 +++++++++------- src/engine/engine_util_solve.c | 10 +++++----- src/engine/engine_util_spatial.c | 2 +- 7 files changed, 20 insertions(+), 18 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 1a92cc05..9bdb0b1f 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1436,7 +1436,7 @@ struct mjpPlugin_ { void (*copy)(mjData* dest, const mjModel* m, const mjData* src, int instance); // called when an mjData is being reset (required) - void (*reset)(const mjModel* m, double* plugin_state, void* plugin_data, int instance); + void (*reset)(const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance); // called when the plugin needs to update its outputs (required) void (*compute)(const mjModel* m, mjData* d, int instance, int capability_bit); diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index b6897080..dc9ce674 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -99,7 +99,7 @@ struct mjpPlugin_ { void (*copy)(mjData* dest, const mjModel* m, const mjData* src, int instance); // called when an mjData is being reset (required) - void (*reset)(const mjModel* m, double* plugin_state, void* plugin_data, int instance); + void (*reset)(const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance); // called when the plugin needs to update its outputs (required) void (*compute)(const mjModel* m, mjData* d, int instance, int capability_bit); diff --git a/plugin/actuator/pid.cc b/plugin/actuator/pid.cc index d99ceb15..e73c769e 100644 --- a/plugin/actuator/pid.cc +++ b/plugin/actuator/pid.cc @@ -256,7 +256,7 @@ void Pid::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto* pid = reinterpret_cast(plugin_data); pid->Reset(plugin_state); diff --git a/plugin/sdf/sdf.cc b/plugin/sdf/sdf.cc index 56c84864..c881c2c6 100644 --- a/plugin/sdf/sdf.cc +++ b/plugin/sdf/sdf.cc @@ -104,8 +104,8 @@ void SdfVisualizer::Visualize(const mjModel* m, const mjData* d, if (k == 0) { float rgba[4] = {static_cast(j > 0), 0, static_cast(j == 0), 1}; - mjtNum size[] = {.2*m->stat.meansize}; - mjv_initGeom(thisgeom, mjGEOM_SPHERE, size, from, geom_xmat, rgba); + mjtNum size = 0.2 * m->stat.meansize; + mjv_initGeom(thisgeom, mjGEOM_SPHERE, &size, from, geom_xmat, rgba); } else { mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL); thisgeom->objtype = mjOBJ_UNKNOWN; diff --git a/plugin/sensor/touch_grid.cc b/plugin/sensor/touch_grid.cc index e9a6402b..1dfdfd02 100644 --- a/plugin/sensor/touch_grid.cc +++ b/plugin/sensor/touch_grid.cc @@ -432,15 +432,17 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, return; } else { // size - mjtNum size[3] = {dist*0.5*(x_edges[i+1]-x_edges[i]), - dist*0.5*(y_edges[j+1]-y_edges[j]), - dist*kRelativeThickness}; + mjtNum size[3]; + size[0] = dist*0.5*(x_edges[i+1]-x_edges[i]); + size[1] = dist*0.5*(y_edges[j+1]-y_edges[j]); + size[2] = dist*kRelativeThickness; // position mjtNum pos[3]; - mjtNum aer[3] = {0.5*(x_edges[i+1]+x_edges[i]), - 0.5*(y_edges[j+1]+y_edges[j]), - dist*(1-kRelativeThickness)}; + mjtNum aer[3]; + aer[0] = 0.5*(x_edges[i+1]+x_edges[i]); + aer[1] = 0.5*(y_edges[j+1]+y_edges[j]); + aer[2] = dist*(1-kRelativeThickness); SphericalToCartesian(aer, pos); mju_mulMatVec3(pos, site_mat, pos); mju_addTo3(pos, site_pos); @@ -525,7 +527,7 @@ void TouchGrid::RegisterPlugin() { }; // Reset callback. - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto* TouchGrid = reinterpret_cast(plugin_data); TouchGrid->Reset(m, instance); diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 6b50cbff..f6da3133 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -707,7 +707,7 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n, //--------------------------- eigen decomposition -------------------------------------------------- // eigenvalue decomposition of symmetric 3x3 matrix -static const mjtNum eigEPS = 1E-12; +static const mjtNum eigEPS = mjMINVAL * 1000; int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]) { mjtNum D[9], tmp[9]; mjtNum tau, t, c; @@ -730,11 +730,11 @@ int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum ma eigval[2] = D[8]; // find max off-diagonal element, set indices - if (fabs(D[1]) > fabs(D[2]) && fabs(D[1]) > fabs(D[5])) { + if (mju_abs(D[1]) > mju_abs(D[2]) && mju_abs(D[1]) > mju_abs(D[5])) { rk = 0; // row ck = 1; // column rotk = 2; // rotation axis - } else if (fabs(D[2]) > fabs(D[5])) { + } else if (mju_abs(D[2]) > mju_abs(D[5])) { rk = 0; ck = 2; rotk = 1; @@ -745,7 +745,7 @@ int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum ma } // terminate if max off-diagonal element too small - if (fabs(D[3*rk+ck]) < eigEPS) { + if (mju_abs(D[3*rk+ck]) < eigEPS) { break; } @@ -789,7 +789,7 @@ int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum ma eigval[j1+1] = t; // rotate quaternion - tmp[0] = 0.707106781186548; // mju_cos(pi/4) = mju_sin(pi/4) + tmp[0] = 0.707106781186548; // = cos(pi/4) = sin(pi/4) tmp[1] = tmp[2] = tmp[3] = 0; tmp[(j1+2)%3+1] = tmp[0]; mju_mulQuat(quat, quat, tmp); diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index e2bdc8e7..60c4af9d 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -522,7 +522,7 @@ void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq) { } // init - double tmp[4] = {1, 0, 0, 0}; + mjtNum tmp[4] = {1, 0, 0, 0}; // loop over euler angles, accumulate rotations for (int i=0; i<3; i++) {