Fix a couple of mjtNum build errors.
PiperOrigin-RevId: 645024056 Change-Id: Id283f550a4e2d927c214a86e77d42b90fb02824e
This commit is contained in:
committed by
Copybara-Service
parent
18634b6726
commit
73cd15344e
@@ -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();
|
||||
|
||||
@@ -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++) {
|
||||
|
||||
@@ -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
@@ -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()
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user