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()++;
|
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();
|
||||||
|
|||||||
@@ -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++) {
|
||||||
|
|||||||
@@ -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
@@ -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()
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
Reference in New Issue
Block a user