14 #include <boost/test/unit_test.hpp>
31 model.appendBodyToJoint(idx,
Y);
34 BOOST_AUTO_TEST_SUITE(JointPrismatic)
44 const double displacement = 0.2;
47 TransformX Mx(displacement);
53 TransformY My(displacement);
59 TransformZ Mz(displacement);
107 Eigen::VectorXd
q(Eigen::VectorXd::Zero(1));
108 Eigen::VectorXd
q_dot(Eigen::VectorXd::Zero(1));
118 BOOST_CHECK(expected_v_J.toVector().isApprox(((
Motion)joint_data.
v).toVector(), 1e-12));
129 expected_v_J.linear() << 1., 0., 0.;
133 BOOST_CHECK(expected_v_J.toVector().isApprox(((
Motion)joint_data.
v).toVector(), 1e-12));
151 Eigen::VectorXd
q(Eigen::VectorXd::Zero(
model.nq));
152 Eigen::VectorXd
v(Eigen::VectorXd::Zero(
model.nv));
153 Eigen::VectorXd
a(Eigen::VectorXd::Zero(
model.nv));
157 Eigen::VectorXd tau_expected(Eigen::VectorXd::Zero(
model.nq));
163 q = Eigen::VectorXd::Ones(
model.nq);
164 v = Eigen::VectorXd::Ones(
model.nv);
165 a = Eigen::VectorXd::Ones(
model.nv);
173 v = Eigen::VectorXd::Ones(
model.nv);
174 a = Eigen::VectorXd::Ones(
model.nv);
197 Eigen::VectorXd
q(Eigen::VectorXd::Zero(
model.nq));
198 Eigen::MatrixXd M_expected(
model.nv,
model.nv);
205 q = Eigen::VectorXd::Ones(
model.nq);
218 BOOST_AUTO_TEST_SUITE_END()
220 BOOST_AUTO_TEST_SUITE(JointPrismaticUnaligned)
242 Eigen::Vector3d
axis;
243 axis << 1.0, 0.0, 0.0;
245 Model modelPX, modelPrismaticUnaligned;
249 pos.translation() = SE3::LinearType(1., 0., 0.);
254 addJointAndBody(modelPrismaticUnaligned, joint_model_PU, 0,
pos,
"prismatic-unaligned", inertia);
256 Data dataPX(modelPX);
257 Data dataPrismaticUnaligned(modelPrismaticUnaligned);
259 Eigen::VectorXd
q = Eigen::VectorXd::Ones(modelPX.
nq);
260 Eigen::VectorXd
v = Eigen::VectorXd::Ones(modelPX.
nv);
261 Eigen::VectorXd tauPX = Eigen::VectorXd::Ones(modelPX.
nv);
262 Eigen::VectorXd tauPrismaticUnaligned = Eigen::VectorXd::Ones(modelPrismaticUnaligned.
nv);
263 Eigen::VectorXd aPX = Eigen::VectorXd::Ones(modelPX.
nv);
264 Eigen::VectorXd aPrismaticUnaligned(aPX);
272 BOOST_CHECK(dataPrismaticUnaligned.oMi[1].isApprox(dataPX.oMi[1]));
273 BOOST_CHECK(dataPrismaticUnaligned.liMi[1].isApprox(dataPX.liMi[1]));
274 BOOST_CHECK(dataPrismaticUnaligned.Ycrb[1].matrix().isApprox(dataPX.Ycrb[1].matrix()));
275 BOOST_CHECK(dataPrismaticUnaligned.f[1].toVector().isApprox(dataPX.f[1].toVector()));
278 BOOST_CHECK(dataPrismaticUnaligned.com[0].isApprox(dataPX.com[0]));
281 tauPX =
rnea(modelPX, dataPX,
q,
v, aPX);
282 tauPrismaticUnaligned =
283 rnea(modelPrismaticUnaligned, dataPrismaticUnaligned,
q,
v, aPrismaticUnaligned);
285 BOOST_CHECK(tauPX.isApprox(tauPrismaticUnaligned));
289 Eigen::VectorXd aAbaPrismaticUnaligned =
aba(
290 modelPrismaticUnaligned, dataPrismaticUnaligned,
q,
v, tauPrismaticUnaligned,
293 BOOST_CHECK(aAbaPX.isApprox(aAbaPrismaticUnaligned));
302 Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;
303 jacobianPX.resize(6, 1);
304 jacobianPX.setZero();
305 Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;
306 jacobianPrismaticUnaligned.resize(6, 1);
307 jacobianPrismaticUnaligned.setZero();
312 modelPrismaticUnaligned, dataPrismaticUnaligned, 1,
LOCAL, jacobianPrismaticUnaligned);
314 BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
317 BOOST_AUTO_TEST_SUITE_END()