Eigen::VectorXd moveit_fk(Eigen::VectorXd jointState)
{
kinematic_state->setJointGroupPositions(joint_model_group, jointState);
const Eigen::Affine3d &end_effector_state = kinematic_state->
getGlobalLinkTransform(kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()));
//moveit_ik(end_effector_state);
//Eigen::Quaterniond quaternion (end_effector_state.rotation());
Eigen::VectorXd eulerAngles = end_effector_state.rotation().eulerAngles(0,1,2);
Eigen::VectorXd eefPose(6);
//eefPose << end_effector_state.translation(), quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z();
eefPose << end_effector_state.translation(), eulerAngles;
//ROS_INFO_STREAM("end_eff:\n " << eefPose);
return eefPose;
}
Eigen::MatrixXd moveit_jacobian_inv(Eigen::VectorXd jointState)
{
kinematic_state->setJointGroupPositions(joint_model_group, jointState);
Eigen::Vector3d reference_point_position(0.0,0.0,0.0);
kinematic_state->getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
reference_point_position,jacobian);
Eigen::MatrixXd matrix_temp = jacobian * jacobian.transpose();
if(matrix_temp.determinant() != 0)
{
jacPseudoInv = jacobian.transpose() * ((matrix_temp).inverse());
//std::cout << "\n===============================" << endl;
//std::cout << "jacobian:\n " << jacobian << std::endl;
//std::cout << "jacPseudoInv:\n " << jacPseudoInv << std::endl;
//std::cout << "\n===============================" << endl;
//computePivot();
//computerQR();
}
return jacPseudoInv;
}