Fix a couple of mjtNum build errors.

PiperOrigin-RevId: 645024056
Change-Id: Id283f550a4e2d927c214a86e77d42b90fb02824e
This commit is contained in:
Nimrod Gileadi
2024-06-20 07:30:12 -07:00
committed by Copybara-Service
parent 18634b6726
commit 73cd15344e
5 changed files with 36 additions and 25 deletions
+3 -3
View File
@@ -240,7 +240,7 @@ int RegisterSensorPlugin() {
TestSensor::DestroyCount()++; TestSensor::DestroyCount()++;
}; };
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
int instance) { int instance) {
auto sensor = reinterpret_cast<TestSensor*>(plugin_data); auto sensor = reinterpret_cast<TestSensor*>(plugin_data);
sensor->Reset(); sensor->Reset();
@@ -283,7 +283,7 @@ int RegisterActuatorPlugin() {
TestActuator::DestroyCount()++; TestActuator::DestroyCount()++;
}; };
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
int instance) { int instance) {
auto actuator = reinterpret_cast<TestActuator*>(plugin_data); auto actuator = reinterpret_cast<TestActuator*>(plugin_data);
actuator->Reset(); actuator->Reset();
@@ -338,7 +338,7 @@ int RegisterPassivePlugin() {
d->plugin_data[instance] = 0; 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) { int instance) {
auto passive = reinterpret_cast<TestPassive*>(plugin_data); auto passive = reinterpret_cast<TestPassive*>(plugin_data);
passive->Reset(); passive->Reset();
+5 -5
View File
@@ -546,13 +546,13 @@ TEST_F(AddMTest, DenseSameAsSparse) {
} }
// dense zero matrix // dense zero matrix
std::vector<mjtNum> dst_sparse = std::vector(nv * nv, 0.0); std::vector<mjtNum> dst_sparse(nv * nv, 0.0);
// sparse zero matrix // sparse zero matrix
std::vector<mjtNum> dst_dense = std::vector(nv * nv, 0.0); std::vector<mjtNum> dst_dense(nv * nv, 0.0);
std::vector<int> rownnz = std::vector(nv, nv); std::vector<int> rownnz(nv, nv);
std::vector<int> rowadr = std::vector(nv, 0); std::vector<int> rowadr(nv, 0);
std::vector<int> colind = std::vector(nv * nv, 0); std::vector<int> colind(nv * nv, 0);
// set sparse structure // set sparse structure
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
+11 -11
View File
@@ -166,38 +166,38 @@ TEST_F(Euler2QuatTest, BadSeqLength) {
} }
TEST_F(Euler2QuatTest, Euler2Quat) { TEST_F(Euler2QuatTest, Euler2Quat) {
double quat[4] = {0}; mjtNum quat[4] = {0};
double tol = 1e-14; mjtNum tol = 1e-14;
char seq[] = "xyz"; char seq[] = "xyz";
double euler[3] = {mjPI, 0, 0}; mjtNum euler[3] = {mjPI, 0, 0};
double expected[4] = {0, 1, 0, 0}; mjtNum expected[4] = {0, 1, 0, 0};
mju_euler2Quat(quat, euler, seq); mju_euler2Quat(quat, euler, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected)); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected));
euler[1] = mjPI; euler[1] = mjPI;
double expected2[4] = {0, 0, 0, 1}; mjtNum expected2[4] = {0, 0, 0, 1};
mju_euler2Quat(quat, euler, seq); mju_euler2Quat(quat, euler, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected2)); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected2));
char seq2[] = "XYZ"; char seq2[] = "XYZ";
double expected3[4] = {0, 0, 0, -1}; mjtNum expected3[4] = {0, 0, 0, -1};
mju_euler2Quat(quat, euler, seq2); mju_euler2Quat(quat, euler, seq2);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected3)); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected3));
double euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI}; mjtNum euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI};
double expected4[4] = {-1, 0, 0, 0}; mjtNum expected4[4] = {-1, 0, 0, 0};
mju_euler2Quat(quat, euler2, seq); mju_euler2Quat(quat, euler2, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4)); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4));
mju_euler2Quat(quat, euler2, seq2); mju_euler2Quat(quat, euler2, seq2);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4)); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4));
double euler3[3] = {mjPI/2, mjPI/2, mjPI/2}; mjtNum euler3[3] = {mjPI/2, mjPI/2, mjPI/2};
double expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)}; mjtNum expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)};
mju_euler2Quat(quat, euler3, seq); mju_euler2Quat(quat, euler3, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected5)); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected5));
mju_euler2Quat(quat, euler3, seq2); mju_euler2Quat(quat, euler3, seq2);
double expected6[4] = {mju_sqrt(.5), 0, mju_sqrt(.5), 0}; mjtNum expected6[4] = {mju_sqrt(.5), 0, mju_sqrt(.5), 0};
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected6)); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected6));
} }
+15 -4
View File
@@ -177,6 +177,17 @@ std::vector<mjtNum> GetCtrlNoise(const mjModel* m, int nsteps,
return ctrl; return ctrl;
} }
template <typename T>
auto Compare(T val1, T val2);
auto Compare(char val1, char val2) {
return val1 != val2;
}
auto Compare(unsigned char val1, unsigned char val2) {
return val1 != val2;
}
// The maximum spacing between a normalised floating point number x and an // The maximum spacing between a normalised floating point number x and an
// adjacent normalised number is 2 epsilon |x|; a factor 10 is added accounting // adjacent normalised number is 2 epsilon |x|; a factor 10 is added accounting
// for losses during non-idempotent operations such as vector normalizations. // for losses during non-idempotent operations such as vector normalizations.
@@ -185,13 +196,13 @@ auto Compare(T val1, T val2) {
using ReturnType = using ReturnType =
std::conditional_t<std::is_same_v<T, float>, float, double>; std::conditional_t<std::is_same_v<T, float>, float, double>;
ReturnType error; ReturnType error;
if (mju_abs(val1) <= 1 || mju_abs(val2) <= 1) { if (std::abs(val1) <= 1 || std::abs(val2) <= 1) {
// Absolute precision for small numbers // Absolute precision for small numbers
error = mju_abs(val1-val2); error = std::abs(val1-val2);
} else { } else {
// Relative precision for larger numbers // Relative precision for larger numbers
ReturnType magnitude = mju_abs(val1) + mju_abs(val2); ReturnType magnitude = std::abs(val1) + std::abs(val2);
error = mju_abs(val1/magnitude - val2/magnitude) / magnitude; error = std::abs(val1/magnitude - val2/magnitude) / magnitude;
} }
ReturnType safety_factor = 200; ReturnType safety_factor = 200;
return error < safety_factor * std::numeric_limits<ReturnType>::epsilon() return error < safety_factor * std::numeric_limits<ReturnType>::epsilon()
+2 -2
View File
@@ -873,8 +873,8 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
// Apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and // Apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
// quat. It should match the originally specified values. // quat. It should match the originally specified values.
double recovered_pos[3]; mjtNum recovered_pos[3];
double recovered_quat[4]; mjtNum recovered_quat[4];
mju_mulPose(recovered_pos, recovered_quat, mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[0], &model->geom_quat[0], &model->geom_pos[0], &model->geom_quat[0],
inverse_mesh_pos, inverse_mesh_quat); inverse_mesh_pos, inverse_mesh_quat);