9 #include <boost/variant.hpp>
11 #include <boost/test/unit_test.hpp>
12 #include <boost/utility/binary.hpp>
14 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
18 double r =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
19 double p =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX /
M_PI)) - (
M_PI / 2);
20 double y =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
25 (Eigen::AngleAxisd(
y, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(
p, Eigen::Vector3d::UnitY())
26 * Eigen::AngleAxisd(
r, Eigen::Vector3d::UnitX()))
47 for (
int k = 0; k <
n; ++k)
49 Eigen::Quaterniond
quat;
51 const Eigen::Matrix3d
R =
quat.toRotationMatrix();
69 for (
int k = 0; k < n2; ++k)
71 double r =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
72 double y =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
74 Rp << 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0;
75 const Eigen::Matrix3d
R = Eigen::AngleAxisd(
y, Eigen::Vector3d::UnitZ()).toRotationMatrix() * Rp
76 * Eigen::AngleAxisd(
r, Eigen::Vector3d::UnitX()).toRotationMatrix();
88 for (
int k = 0; k < n2; ++k)
90 double r =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
91 double y =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
93 Rp << 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0;
94 const Eigen::Matrix3d
R = Eigen::AngleAxisd(
y, Eigen::Vector3d::UnitZ()).toRotationMatrix() * Rp
95 * Eigen::AngleAxisd(
r, Eigen::Vector3d::UnitX()).toRotationMatrix();
110 Eigen::Vector3d
rpy(Eigen::Vector3d::Zero());
121 double r =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
122 double p =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX /
M_PI)) - (
M_PI / 2);
123 double y =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
124 rpy = Eigen::Vector3d(
r,
p,
y);
135 Eigen::Vector3d jL0Expected = Eigen::Vector3d::UnitX();
136 Eigen::Vector3d jL1Expected =
137 Eigen::AngleAxisd(
r, Eigen::Vector3d::UnitX()).toRotationMatrix().transpose().col(1);
138 Eigen::Vector3d jL2Expected = (Eigen::AngleAxisd(
p, Eigen::Vector3d::UnitY())
139 * Eigen::AngleAxisd(
r, Eigen::Vector3d::UnitX()))
147 Eigen::Vector3d jW0Expected = (Eigen::AngleAxisd(
y, Eigen::Vector3d::UnitZ())
148 * Eigen::AngleAxisd(
p, Eigen::Vector3d::UnitY()))
151 Eigen::Vector3d jW1Expected =
152 Eigen::AngleAxisd(
y, Eigen::Vector3d::UnitZ()).toRotationMatrix().col(1);
153 Eigen::Vector3d jW2Expected = Eigen::Vector3d::UnitZ();
159 Eigen::Vector3d rpydot = Eigen::Vector3d::Random();
160 double const eps = 1e-7;
161 double const tol = 1e-5;
166 Eigen::Matrix3d Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2];
168 Eigen::Vector3d omegaL = jL * rpydot;
171 Eigen::Vector3d omegaW = jW * rpydot;
178 double r =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
179 double p =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX /
M_PI)) - (
M_PI / 2);
181 double y =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
182 Eigen::Vector3d
rpy(
r,
p,
y);
197 Eigen::Matrix3d jAinv =
205 double r =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
206 double p =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX /
M_PI)) - (
M_PI / 2);
207 double y =
static_cast<double>(
rand()) / (
static_cast<double>(RAND_MAX / (2 *
M_PI))) -
M_PI;
208 Eigen::Vector3d
rpy(
r,
p,
y);
209 Eigen::Vector3d rpydot(Eigen::Vector3d::Zero());
212 Eigen::Matrix3d djL =
215 Eigen::Matrix3d djW =
218 Eigen::Matrix3d djA =
223 rpydot = Eigen::Vector3d::Random();
235 Eigen::Vector3d omegaL = jL * rpydot;
236 Eigen::Vector3d omegaW = jW * rpydot;
242 double const eps = 1e-7;
243 double const tol = 1e-5;
244 Eigen::Vector3d rpyEps =
rpy;
255 Eigen::Matrix3d djLf = djLdr * rpydot[0] + djLdp * rpydot[1] + djLdy * rpydot[2];
267 Eigen::Matrix3d djWf = djWdr * rpydot[0] + djWdp * rpydot[1] + djWdy * rpydot[2];
271 BOOST_AUTO_TEST_SUITE_END()