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()++;
};
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 sensor = reinterpret_cast<TestSensor*>(plugin_data);
sensor->Reset();
@@ -283,7 +283,7 @@ int RegisterActuatorPlugin() {
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) {
auto actuator = reinterpret_cast<TestActuator*>(plugin_data);
actuator->Reset();
@@ -338,7 +338,7 @@ int RegisterPassivePlugin() {
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 passive = reinterpret_cast<TestPassive*>(plugin_data);
passive->Reset();
+5 -5
View File
@@ -546,13 +546,13 @@ TEST_F(AddMTest, DenseSameAsSparse) {
}
// 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
std::vector<mjtNum> dst_dense = std::vector(nv * nv, 0.0);
std::vector<int> rownnz = std::vector(nv, nv);
std::vector<int> rowadr = std::vector(nv, 0);
std::vector<int> colind = std::vector(nv * nv, 0);
std::vector<mjtNum> dst_dense(nv * nv, 0.0);
std::vector<int> rownnz(nv, nv);
std::vector<int> rowadr(nv, 0);
std::vector<int> colind(nv * nv, 0);
// set sparse structure
for (int i = 0; i < nv; i++) {
+11 -11
View File
@@ -166,38 +166,38 @@ TEST_F(Euler2QuatTest, BadSeqLength) {
}
TEST_F(Euler2QuatTest, Euler2Quat) {
double quat[4] = {0};
double tol = 1e-14;
mjtNum quat[4] = {0};
mjtNum tol = 1e-14;
char seq[] = "xyz";
double euler[3] = {mjPI, 0, 0};
double expected[4] = {0, 1, 0, 0};
mjtNum euler[3] = {mjPI, 0, 0};
mjtNum expected[4] = {0, 1, 0, 0};
mju_euler2Quat(quat, euler, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected));
euler[1] = mjPI;
double expected2[4] = {0, 0, 0, 1};
mjtNum expected2[4] = {0, 0, 0, 1};
mju_euler2Quat(quat, euler, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected2));
char seq2[] = "XYZ";
double expected3[4] = {0, 0, 0, -1};
mjtNum expected3[4] = {0, 0, 0, -1};
mju_euler2Quat(quat, euler, seq2);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected3));
double euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI};
double expected4[4] = {-1, 0, 0, 0};
mjtNum euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI};
mjtNum expected4[4] = {-1, 0, 0, 0};
mju_euler2Quat(quat, euler2, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4));
mju_euler2Quat(quat, euler2, seq2);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4));
double euler3[3] = {mjPI/2, mjPI/2, mjPI/2};
double expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)};
mjtNum euler3[3] = {mjPI/2, mjPI/2, mjPI/2};
mjtNum expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)};
mju_euler2Quat(quat, euler3, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected5));
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));
}
+15 -4
View File
@@ -177,6 +177,17 @@ std::vector<mjtNum> GetCtrlNoise(const mjModel* m, int nsteps,
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
// adjacent normalised number is 2 epsilon |x|; a factor 10 is added accounting
// for losses during non-idempotent operations such as vector normalizations.
@@ -185,13 +196,13 @@ auto Compare(T val1, T val2) {
using ReturnType =
std::conditional_t<std::is_same_v<T, float>, float, double>;
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
error = mju_abs(val1-val2);
error = std::abs(val1-val2);
} else {
// Relative precision for larger numbers
ReturnType magnitude = mju_abs(val1) + mju_abs(val2);
error = mju_abs(val1/magnitude - val2/magnitude) / magnitude;
ReturnType magnitude = std::abs(val1) + std::abs(val2);
error = std::abs(val1/magnitude - val2/magnitude) / magnitude;
}
ReturnType safety_factor = 200;
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
// quat. It should match the originally specified values.
double recovered_pos[3];
double recovered_quat[4];
mjtNum recovered_pos[3];
mjtNum recovered_quat[4];
mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[0], &model->geom_quat[0],
inverse_mesh_pos, inverse_mesh_quat);