Verified Commit 3844a751 authored by Gabriele Buondonno's avatar Gabriele Buondonno Committed by Justin Carpentier
Browse files

[unittest] Test kinematic getters

parent 5923f1ac
......@@ -207,4 +207,73 @@ BOOST_AUTO_TEST_CASE(test_get_classical_acceleration)
}
}
BOOST_AUTO_TEST_CASE(test_kinematic_getters)
{
using namespace Eigen;
using namespace pinocchio;
// Build a simple 2R planar model
Model model;
JointIndex jointId = 0;
jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1");
jointId = model.addJoint(jointId, JointModelRZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2");
Data data(model);
// Predetermined configuration values
VectorXd q(model.nq);
q << M_PI/2.0, 0.0;
VectorXd v(model.nv);
v << 1.0, 0.0;
VectorXd a(model.nv);
a << 0.0, 0.0;
// Expected velocity
Motion v_local;
v_local.linear() = Vector3d(0.0, 1.0, 0.0);
v_local.angular() = Vector3d(0.0, 0.0, 1.0);
Motion v_world;
v_world.linear() = Vector3d::Zero();
v_world.angular() = Vector3d(0.0, 0.0, 1.0);
Motion v_align;
v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
v_align.angular() = Vector3d(0.0, 0.0, 1.0);
// Expected classical acceleration
Motion ac_local;
ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
ac_local.angular() = Vector3d::Zero();
Motion ac_world = Motion::Zero();
Motion ac_align;
ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
ac_align.angular() = Vector3d::Zero();
// Perform kinematics
forwardKinematics(model,data,q,v,a);
// Check output velocity
BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId)));
BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId,ReferenceFrame::LOCAL)));
BOOST_CHECK(v_world.isApprox(getVelocity(model,data,jointId,ReferenceFrame::WORLD)));
BOOST_CHECK(v_align.isApprox(getVelocity(model,data,jointId,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
// Check output acceleration (all zero)
BOOST_CHECK(getAcceleration(model,data,jointId).isZero());
BOOST_CHECK(getAcceleration(model,data,jointId,ReferenceFrame::LOCAL).isZero());
BOOST_CHECK(getAcceleration(model,data,jointId,ReferenceFrame::WORLD).isZero());
BOOST_CHECK(getAcceleration(model,data,jointId,ReferenceFrame::LOCAL_WORLD_ALIGNED).isZero());
// Check output classical
BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId)));
BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId,ReferenceFrame::LOCAL)));
BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model,data,jointId,ReferenceFrame::WORLD)));
BOOST_CHECK(ac_align.isApprox(getClassicalAcceleration(model,data,jointId,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
}
BOOST_AUTO_TEST_SUITE_END()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment