/* static */ bool ocraWbiConversions::wbiToOcraSegJacobian(const Eigen::MatrixXd &jac, Eigen::MatrixXd &J)
{
int dof = DIM_T + DIM_R;
if(dof != jac.rows() || dof != J.rows()||jac.cols() != J.cols())
{
std::cout<<"ERROR: Input and output matrices dimensions should be the same" <<std::endl;
return false;
}
// FOR FULL n+6 Jacobian ONLY
Eigen::MatrixXd jac5,jac6;
Eigen::Matrix3d jac1,jac2,jac3,jac4;
jac5.resize(3,jac.cols()-6);
jac6.resize(3,jac.cols()-6);
jac1 = jac.topLeftCorner(3,3);
jac2 = jac.block<3,3>(0,3);
jac3 = jac.bottomLeftCorner(3,3);
jac4 = jac.block<3,3>(3,3);
jac5 = jac.topRightCorner(3,jac.cols()-6);
jac6 = jac.bottomRightCorner(3,jac.cols()-6);
J.topLeftCorner(3,3) = jac4;
J.block<3,3>(0,3) = jac3;
J.bottomLeftCorner(3,3) = jac2;
J.block<3,3>(3,3) = jac1;
J.topRightCorner(3,jac.cols()-6) = jac6;
J.bottomRightCorner(3,jac.cols()-6) = jac5;
return true;
}
