Commit 99ad179d authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++] correct computation of jacobian

parent c6bed93a
...@@ -119,14 +119,13 @@ inline void getFrameJacobian(const Model & model, const Data& data, ...@@ -119,14 +119,13 @@ 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;
Eigen::Matrix3d c_cross = skew(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>() += c_cross * J.col(j).bottomRows<3>(); J.col(j).topRows<3>() += model.operational_frames[frame_id].frame_placement.inverse().translation().cross( J.col(j).bottomRows<3>());
} }
else else
{ {
......
...@@ -91,7 +91,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -91,7 +91,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
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.translation().cross(nu_joint.angular()); nu_frame_from_nu_joint.linear() += frame_placement.inverse().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