14 #include <boost/test/unit_test.hpp>
15 #include <boost/utility/binary.hpp>
17 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21 using namespace Eigen;
29 model.lowerPositionLimit.head<3>().
fill(-1.);
30 model.upperPositionLimit.head<3>().
fill(1.);
38 using namespace Eigen;
46 model.lowerPositionLimit.head<3>().
fill(-1.);
47 model.upperPositionLimit.head<3>().
fill(1.);
63 using namespace Eigen;
71 model.lowerPositionLimit.head<3>().
fill(-1.);
72 model.upperPositionLimit.head<3>().
fill(1.);
74 VectorXd
v(VectorXd::Zero(
model.nv));
86 using namespace Eigen;
94 model.lowerPositionLimit.head<3>().
fill(-1.);
95 model.upperPositionLimit.head<3>().
fill(1.);
97 VectorXd
v(VectorXd::Zero(
model.nv));
98 VectorXd
a(VectorXd::Zero(
model.nv));
111 using namespace Eigen;
119 model.lowerPositionLimit.head<3>().
fill(-1.);
120 model.upperPositionLimit.head<3>().
fill(1.);
122 VectorXd
v(VectorXd::Random(
model.nv));
139 using namespace Eigen;
147 model.lowerPositionLimit.head<3>().
fill(-1.);
148 model.upperPositionLimit.head<3>().
fill(1.);
150 VectorXd
v(VectorXd::Random(
model.nv));
151 VectorXd
a(VectorXd::Random(
model.nv));
168 using namespace Eigen;
176 model.lowerPositionLimit.head<3>().
fill(-1.);
177 model.upperPositionLimit.head<3>().
fill(1.);
179 VectorXd
v(VectorXd::Random(
model.nv));
180 VectorXd
a(VectorXd::Random(
model.nv));
192 linear =
acc.linear() + vel.angular().cross(vel.linear());
193 acc_classical_local.linear() = linear;
200 linear = acc_classical_world.linear() + vel_world.angular().cross(vel_world.linear());
201 acc_classical_world.linear() = linear;
205 Motion vel_aligned =
SE3(
data.oMi[
i].rotation(), Eigen::Vector3d::Zero()).act(vel);
206 Motion acc_classical_aligned =
SE3(
data.oMi[
i].rotation(), Eigen::Vector3d::Zero()).act(
acc);
207 linear = acc_classical_aligned.linear() + vel_aligned.angular().cross(vel_aligned.linear());
208 acc_classical_aligned.linear() = linear;
217 using namespace Eigen;
224 jointId =
model.addJoint(
225 jointId,
JointModelRZ(),
SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)),
"Joint2");
231 q <<
M_PI / 2.0, 0.0;
241 v_local.linear() = Vector3d(0.0, 1.0, 0.0);
242 v_local.angular() = Vector3d(0.0, 0.0, 1.0);
245 v_world.linear() = Vector3d::Zero();
246 v_world.angular() = Vector3d(0.0, 0.0, 1.0);
249 v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
250 v_align.angular() = Vector3d(0.0, 0.0, 1.0);
254 ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
255 ac_local.angular() = Vector3d::Zero();
257 Motion ac_world = Motion::Zero();
260 ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
261 ac_align.angular() = Vector3d::Zero();
286 BOOST_AUTO_TEST_SUITE_END()