Commit a4454e84 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #392 from jcarpent/topic/devel

Correct frameJacobians
parents 8c95e5c7 82a13aba
......@@ -129,20 +129,19 @@ namespace se3
const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
// Lever between the joint center and the frame center expressed in the global frame
const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
getJacobian<local_frame>(model, data, parent, J);
if(!local_frame)
getJacobian<local_frame>(model, data, parent, J);
if (!frame.placement.isIdentity())
// Lever between the joint center and the frame center expressed in the global frame
const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
if(!local_frame)
J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
else
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
if(!local_frame)
J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
else
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
}
......
......@@ -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