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