Commit d2db106f authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++][unittest] Reworked computations of frame Jacobian in global frame....

[C++][unittest] Reworked computations of frame Jacobian in global frame. Reworked unittest. Change argument from Matrix6x to MatrixXd to follow getJacobian convention
parent a4fe120f
......@@ -20,8 +20,10 @@
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/spatial/skew.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
namespace se3
{
......@@ -107,20 +109,29 @@ inline void framesForwardKinematic(const Model & model,
template<bool localFrame>
inline void getFrameJacobian(const Model & model, const Data& data,
Model::Index frame_id, Data::Matrix6x & J)
Model::Index frame_id, Eigen::MatrixXd & J)
{
assert( J.rows() == data.J.rows() );
assert( J.cols() == data.J.cols() );
const Model::Index & parent = model.operational_frames[frame_id].parent_id;
const SE3 & oMframe = data.oMof[frame_id];
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);
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
{
if(! localFrame ) J.col(j) = model.operational_frames[frame_id].frame_placement.actInv(Motion(data.J.col(j))).toVector();
else J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
if(! localFrame )
{
J.col(j).topRows<3>() += c_cross * J.col(j).bottomRows<3>();
}
else
{
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
}
}
......
......@@ -70,34 +70,44 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd q_dot = VectorXd::Ones(model.nv);
framesForwardKinematic(model, data, q);
computeJacobians(model,data,q);
MatrixXd expected(6,model.nv); expected.fill(0);
Matrix6x Jof(6,model.nv); Jof.fill(0);
/// In global frame
MatrixXd Joj(6,model.nv); Joj.fill(0);
MatrixXd Jof(6,model.nv); Jof.fill(0);
Model::Index idx = model.getFrameId(frame_name);
getFrameJacobian<false>(model,data,idx,Jof);
getJacobian<false>(model, data, parent_idx, expected);
expected = frame_placement.inverse().toActionMatrix() * expected;
getJacobian<false>(model, data, parent_idx, Joj);
// expected = frame_placement.inverse().toActionMatrix() * expected;
Motion nu_frame(Jof*q_dot);
Motion nu_joint(Joj*q_dot);
Motion nu_frame_from_nu_joint(nu_joint);
nu_frame_from_nu_joint.linear() += frame_placement.translation().cross(nu_joint.angular());
BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));
BOOST_CHECK(Jof.isApprox(expected, 1e-12));
/// In local frame
expected.fill(0); Jof.fill(0);
getFrameJacobian<true>(model,data,idx,Jof);
getJacobian<true>(model, data, parent_idx, expected);
MatrixXd Jjj(6,model.nv); Jjj.fill(0);
MatrixXd Jff(6,model.nv); Jff.fill(0);
getFrameJacobian<true>(model,data,idx,Jff);
getJacobian<true>(model, data, parent_idx, Jjj);
expected = frame_placement.inverse().toActionMatrix() * expected;
nu_frame = Jff*q_dot;
nu_joint = Jjj*q_dot;
// std::cout << Jof << std::endl;
// std::cout << "----" << std::endl;
// std::cout << frame_placement << std::endl;
// std::cout << expected << std::endl;
BOOST_CHECK(Jof.isApprox(expected, 1e-12));
BOOST_CHECK(nu_frame.toVector().isApprox(frame_placement.actInv(nu_joint).toVector(), 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