0
$\begingroup$

I recently performed camera and robot-robot calibration and, I wanted to validate the results with the touch test method. So I retrieved a particular point on the ChArUco board and wrote a node that transforms the end effector equipped with a pointer to the "marker_frame" on the board using the computeCartesianPath function. The issue is the pointer does not exactly reach the "marker_frame". I am unable to identify the cause of this issue. Does anyone have any ideas? Thanks!

Setup: Two ABB IRB120 industrial arms set on a table (one arm's end effector equipped with camera and other with pointer) ROS Noetic Ubuntu 18.04

This is the node:

int main(int argc, char** argv)
{
    ros::init(argc, argv, "abb_irb120_cartesian_planning");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    std::string group_name;
    std::cout << "Enter the group name (valid choices are: mp, mp_m, dual_arm): ";
    std::cin >> group_name;

    if (group_name != "mp" && group_name != "mp_m" && group_name != "dual_arm") {
        std::cout << "Invalid group name entered. Exiting..." << std::endl;
        return -1;
    }

    moveit::planning_interface::MoveGroupInterface group(group_name);
    group.setStartStateToCurrentState();

    robot_state::RobotStatePtr current_state = group.getCurrentState();
    ros::Duration(5.0).sleep(); // Give it some time to receive data
    if (!current_state) {
        ROS_ERROR("Failed to fetch the current robot state.");
        return -1;
    }

    ros::Publisher pub = nh.advertise<control_msgs::FollowJointTrajectoryActionGoal>("/" + group_name + 
                                                                                    "/joint_trajectory_action/goal", 1);
    ros::Publisher display_pub = nh.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 10);

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);
    ros::Duration(1.0).sleep(); 

    std::vector<geometry_msgs::Pose> waypoints;
    waypoints.push_back(group.getCurrentPose().pose);

    std::string frame_name;

    std::cout << "Enter the name of the frame you want to transform to: ";
    std::cin >> frame_name;

    try
    {
        geometry_msgs::TransformStamped transform_stamped = tfBuffer.lookupTransform(group.getPlanningFrame(), frame_name, 
                                                                                        ros::Time(0), ros::Duration(1.0));

        geometry_msgs::PoseStamped marker_pose_stamped;
        marker_pose_stamped.header.frame_id = group.getPlanningFrame();
        
        marker_pose_stamped.pose.position.x = transform_stamped.transform.translation.x;
        marker_pose_stamped.pose.position.y = transform_stamped.transform.translation.y;
        marker_pose_stamped.pose.position.z = transform_stamped.transform.translation.z;

        marker_pose_stamped.pose.orientation.x = transform_stamped.transform.rotation.x;
        marker_pose_stamped.pose.orientation.y = transform_stamped.transform.rotation.y;
        marker_pose_stamped.pose.orientation.z = transform_stamped.transform.rotation.z;
        marker_pose_stamped.pose.orientation.w = transform_stamped.transform.rotation.w;

        ROS_INFO_STREAM("Original Marker Pose in Planning Frame: Position x: " << marker_pose_stamped.pose.position.x 
                        << ", y: " << marker_pose_stamped.pose.position.y << ", z: " << marker_pose_stamped.pose.position.z);
        ROS_INFO_STREAM("Original Marker Pose in Planning Frame: Orientation x: " << marker_pose_stamped.pose.orientation.x 
                        << ", y: " << marker_pose_stamped.pose.orientation.y << ", z: " << marker_pose_stamped.pose.orientation.z 
                        << ", w: " << marker_pose_stamped.pose.orientation.w);

        geometry_msgs::PoseStamped transformed_pose;
        tf2::doTransform(marker_pose_stamped, transformed_pose, transform_stamped);

        waypoints.push_back(transformed_pose.pose);

        ROS_INFO_STREAM("Transformed Pose to Planning Frame: Position x: " << transformed_pose.pose.position.x << ", y: " 
                                    << transformed_pose.pose.position.y << ", z: " << transformed_pose.pose.position.z);
        ROS_INFO_STREAM("Transformed Pose to Planning Frame: Orientation x: " << transformed_pose.pose.orientation.x 
                        << ", y: " << transformed_pose.pose.orientation.y << ", z: " << transformed_pose.pose.orientation.z 
                        << ", w: " << transformed_pose.pose.orientation.w);

        moveit::planning_interface::MoveGroupInterface::Plan plan;

        double fraction = group.computeCartesianPath(waypoints, 0.01, 0.0, plan.trajectory_, true);
    
        if (fraction >= 0.9)  
        {
            control_msgs::FollowJointTrajectoryActionGoal action_goal;
            action_goal.goal.trajectory = plan.trajectory_.joint_trajectory;

            ros::Duration time_from_start(0.0);
            for (auto& point : plan.trajectory_.joint_trajectory.points) {
                time_from_start += ros::Duration(0.1);
                point.time_from_start = time_from_start;

                ROS_INFO("Timestamp for point: %f", point.time_from_start.toSec());
            }

            for (size_t i = 0; i < action_goal.goal.trajectory.points.size(); ++i) {
                if (action_goal.goal.trajectory.points[i].time_from_start.toSec() == 0) {
                    ROS_WARN("Invalid timestamp at trajectory point %zu", i);
                }
            }

            pub.publish(action_goal);

            moveit_msgs::DisplayTrajectory display_trajectory;
            display_trajectory.model_id = group_name;
            display_trajectory.trajectory.push_back(plan.trajectory_);
            display_pub.publish(display_trajectory);    
            
            std::string input;
            std::cout << "Execute plan? y or n: ";
            std::cin >> input;

            if (input == "y")
            {
                // Using 'group.move()' to execute previously computed plan
                moveit::core::MoveItErrorCode success_code = group.move();
                bool success = (success_code == moveit::planning_interface::MoveItErrorCode::SUCCESS);
                group.stop();
                group.clearPoseTargets();
                std::cout << "Executed? " << (success ? "Yes" : "No") << std::endl;
            }
            else
            {
            std::cout << "Trajectory execution on robot aborted." << std::endl;
            }
        }
        else
        {
            ROS_WARN("Could not compute a valid Cartesian path. Fraction returned: %f", fraction);
        }
    }
    catch (tf2::TransformException &ex)
    {
        ROS_WARN("Could not transform to target pose: %s", ex.what());
    }

    ros::shutdown();

    return 0;
}

enter image description here

Transform before executing the motion:
rosrun tf tf_echo pointer_m marker_frame
At time 1698729554.314
- Translation: [-0.150, 0.075, -0.466]


Transform after executing the motion:
rosrun tf tf_echo pointer_m marker_frame
At time 1698728441.460
 - Translation: [0.006, -0.082, -0.099]

I expected the translation to be zero after the transformation, I'm not sure why it's not reaching the frame precisely.

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.