Verified Commit 05657211 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

test/frames: fix unit test

parent e120157e
...@@ -174,13 +174,15 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -174,13 +174,15 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random(); const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
BOOST_CHECK(model.existFrame(frame_name));
se3::Data data(model); se3::Data data(model);
se3::Data data_ref(model); se3::Data data_ref(model);
VectorXd q = VectorXd::Ones(model.nq); model.lowerPositionLimit.head<7>().fill(-1.);
q.middleRows<4> (3).normalize(); model.upperPositionLimit.head<7>().fill( 1.);
VectorXd q_dot = VectorXd::Ones(model.nv); VectorXd q = randomConfiguration(model);
VectorXd v = VectorXd::Ones(model.nv);
/// In local frame /// In local frame
Model::Index idx = model.getFrameId(frame_name); Model::Index idx = model.getFrameId(frame_name);
...@@ -188,11 +190,14 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -188,11 +190,14 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement)); BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0); Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0); Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian<LOCAL>(model,data,idx,Jff); computeJointJacobians(model,data,q);
updateFramePlacement(model, data, idx);
getFrameJacobian<LOCAL>(model, data, idx, Jff);
computeJointJacobians(model,data_ref,q);
getJointJacobian<LOCAL>(model, data_ref, parent_idx, Jjj); getJointJacobian<LOCAL>(model, data_ref, parent_idx, Jjj);
Motion nu_frame = Motion(Jff*q_dot); Motion nu_frame = Motion(Jff*v);
Motion nu_joint = Motion(Jjj*q_dot); Motion nu_joint = Motion(Jjj*v);
const SE3::ActionMatrix_t jXf = frame.placement.toActionMatrix(); const SE3::ActionMatrix_t jXf = frame.placement.toActionMatrix();
Data::Matrix6x Jjj_from_frame(jXf * Jff); Data::Matrix6x Jjj_from_frame(jXf * Jff);
......
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