Commit 82a13aba authored by jcarpent's avatar jcarpent
Browse files

[Unit Test] Correct unit test on frame jacobians

parent b1908a1b
......@@ -64,46 +64,53 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
se3::Data data(model);
se3::Data data_ref(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd q_dot = VectorXd::Ones(model.nv);
framesForwardKinematics(model, data, q);
computeJacobians(model,data,q);
/// In global frame
Data::Matrix6x Joj(6,model.nv); Joj.fill(0);
Data::Matrix6x Jof(6,model.nv); Jof.fill(0);
Model::Index idx = model.getFrameId(frame_name);
const Frame & frame = model.frames[idx];
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
computeJacobians(model,data,q);
computeJacobians(model,data_ref,q);
framesForwardKinematics(model,data);
getFrameJacobian<false>(model,data,idx,Jof);
getJacobian<false>(model, data, parent_idx, Joj);
getJacobian<false>(model, data_ref, parent_idx, Joj);
Motion nu_frame(Jof*q_dot);
Motion nu_joint(Joj*q_dot);
SE3 translation(SE3::Identity());
translation.translation(data.oMi[parent_idx].rotation()*frame.placement.translation());
Motion nu_frame_from_nu_joint(translation.actInv(nu_joint));
Motion nu_frame_from_nu_joint(nu_joint);
nu_frame_from_nu_joint.linear() -= (data.oMi[parent_idx].rotation() *framePlacement.translation()).cross(nu_joint.angular());
BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));
BOOST_CHECK(nu_frame.isApprox(nu_frame_from_nu_joint, 1e-12));
/// In local frame
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian<true>(model,data,idx,Jff);
getJacobian<true>(model, data, parent_idx, Jjj);
nu_frame = Jff*q_dot;
nu_joint = Jjj*q_dot;
BOOST_CHECK(nu_frame.toVector().isApprox(framePlacement.actInv(nu_joint).toVector(), 1e-12));
getJacobian<true>(model, data_ref, parent_idx, Jjj);
nu_frame = Motion(Jff*q_dot);
nu_joint = Motion(Jjj*q_dot);
const SE3::ActionMatrix_t jXf = frame.placement.toActionMatrix();
Data::Matrix6x Jjj_from_frame(jXf * Jff);
BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));
nu_frame_from_nu_joint = frame.placement.act(nu_frame);
BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));
}
BOOST_AUTO_TEST_SUITE_END ()
......
Supports Markdown
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