Commit 076d0a34 authored by Valenza Florian's avatar Valenza Florian
Browse files

[frame jacobian] Express jacobian in correct frame

parent bf3e70a7
...@@ -119,13 +119,14 @@ inline void getFrameJacobian(const Model & model, const Data& data, ...@@ -119,13 +119,14 @@ inline void getFrameJacobian(const Model & model, const Data& data,
int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1; int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
SE3::Vector3 lever(data.oMi[parent].rotation() * (model.operational_frames[frame_id].frame_placement.translation()));
if (!localFrame) getJacobian<localFrame>(model, data, parent, J); if (!localFrame) getJacobian<localFrame>(model, data, parent, J);
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j]) for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
{ {
if(! localFrame ) if(! localFrame )
{ {
J.col(j).topRows<3>() += model.operational_frames[frame_id].frame_placement.inverse().translation().cross( J.col(j).bottomRows<3>()); J.col(j).topRows<3>() -= lever.cross( J.col(j).bottomRows<3>());
} }
else else
{ {
......
...@@ -85,13 +85,12 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -85,13 +85,12 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
getFrameJacobian<false>(model,data,idx,Jof); getFrameJacobian<false>(model,data,idx,Jof);
getJacobian<false>(model, data, parent_idx, Joj); getJacobian<false>(model, data, parent_idx, Joj);
// expected = frame_placement.inverse().toActionMatrix() * expected;
Motion nu_frame(Jof*q_dot); Motion nu_frame(Jof*q_dot);
Motion nu_joint(Joj*q_dot); Motion nu_joint(Joj*q_dot);
Motion nu_frame_from_nu_joint(nu_joint); Motion nu_frame_from_nu_joint(nu_joint);
nu_frame_from_nu_joint.linear() += frame_placement.inverse().translation().cross(nu_joint.angular()); nu_frame_from_nu_joint.linear() -= (data.oMi[parent_idx].rotation() *frame_placement.translation()).cross(nu_joint.angular());
BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12)); BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));
......
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