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; }