Joint state to cartesian position

[Chia-han Yang]

I want to be able to evaluate the end effector positions after some motion planning activities. I think this is something fundamental but I wasn't able to find such function available in ROS and MoveIt. Anybody can point me where I can convert a given joint state to Cartesian position?

Note: I do see examples like PR2 forward kinematics (http://wiki.ros.org/pr2_kinematics/Tutorials/Tutorial%203) or inside MoveIt's source code (https://github.com/ros-planning/moveit_ros/blob/hydro-devel/move_group/src/default_capabilities/kinematics_service_capability.cpp#L183). But they are just about query the current joint state from the robot simulator rather than providing a location function that allows me to input a joint state and work out the cartesian position of a given link.

Or I have to write one myself (following the source code found above) if this doesn't exist yet in MoveIt/ROS?

[12bee...@seecs.edu.pk]

I'm not exactly sure what you mean by a "location function" but if you just want to know the pose of any link based on a given set of joint values (e.g. in an array or vector), then provided you have the robot_state object for your robot, you just have to do this:

std::vector<double> joint_values;
//initialize joint values
robot_state->setJointGroupPositions("joint_group_name", joint_values);
const Eigen::Affine3d& link_pose = robot_state->getGlobalLinkTransform("link_name");
Eigen::Vector3d cartesian_position = link_pose.translation();
Eigen::Matrix3d link_orientation = link_pose.orientation();

Actually there are a number of ways you can assign joint values to a robot state (e.g. setVariablePosition(), setVariablePositions()). See the robot_state class reference for more details.

[Chia-han Yang]

Thanks. I guess I can always make a copy of the robot_state so I can use your method without actually changing existing joint state.
I found another solution where people were saying GetPositionFK service is actually always running when MoveIt is on. I can just make a request to obtain the end effector position.

        ros::ServiceClient fk_client = n.serviceClient<moveit_msgs::GetPositionFK>("compute_fk");
       
        moveit_msgs::GetPositionFK::Request fkrequest;
        moveit_msgs::GetPositionFK::Response fk_response;
        fkrequest.header.frame_id = "link0";
        fkrequest.fk_link_names.resize(1);
        fkrequest.fk_link_names[0] = "link7";
        fkrequest.robot_state.joint_state.position = goal.trajectory.points[i].positions;
        fkrequest.robot_state.joint_state.name = goal.trajectory.joint_names;

        if(fk_client.call(fkrequest, fk_response))
        {
            if(fk_response.error_code.val == fk_response.error_code.SUCCESS)
            {
                for(unsigned int i=0; i < fk_response.pose_stamped.size(); i ++)
                {
                    ROS_INFO_STREAM("Link    : " << fk_response.fk_link_names[i].c_str());
                    ROS_INFO_STREAM("Position: " <<
                        fk_response.pose_stamped[i].pose.position.x << "," <<
                        fk_response.pose_stamped[i].pose.position.y << "," <<
                        fk_response.pose_stamped[i].pose.position.z);
                    ROS_INFO("Orientation: %f %f %f %f",
                        fk_response.pose_stamped[i].pose.orientation.x,
                        fk_response.pose_stamped[i].pose.orientation.y,
                        fk_response.pose_stamped[i].pose.orientation.z,
                        fk_response.pose_stamped[i].pose.orientation.w);
                }
            }
            else
            {
                ROS_ERROR("Forward kinematics failed");
            }
        }
        else
        {
            ROS_ERROR("Forward kinematics service call failed");
        }

But your method would probably work better in performance than issuing a client/service request.

[Chia-han Yang]

Just tested and it worked pretty well. Just that I have "link_pose.rotation();" instead (probably because of the version I am using).