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