Sending positions to the Hybrid Planner fails, but sending Joint positions works

[skaeringur97]

Description

I followed the hybrid planning example and modified it to work with the UR10e robot. It works using the same method as the example uses, but if I try sending positions it always fails in the planning stage. Sending positions works for me on the Panda robot, as well as the Kinova Kortex, but not with the UR10e. I have verified that the position I am sending is valid, by making a simple MoveIt node that just uses MoveGroupInterface to go to a position. This plans and executes without problems.

Your environment

• ROS 2: [Humble] source build
• OS Version: Ubuntu 22.04
• UR10e Binary build (tried source build also)
• Moveit2 Source build

Steps to reproduce

To reproduce this issue, duplicate the hybrid planning example and replace the test/hybrid_planning_demo_node.cpp with this, the test/config/global_planner.yaml with this and the test/config/local_planner.yaml with this. The launch file also needs to be changed, so test/launch/hybrid_planning_common.py changes to this

If you want to see it working with the joint positions, replace these lines:

auto const target_pose = []{
        geometry_msgs::msg::PoseStamped msg;
        msg.header.frame_id = "base_link";
        msg.pose.position.x = -0.226126;
        msg.pose.position.y = 0.279057;
        msg.pose.position.z = 1.43127;
        msg.pose.orientation.x = -0.707108;
        msg.pose.orientation.y = 0.000570927;
        msg.pose.orientation.z = 0.000571454;
        msg.pose.orientation.w = 0.707105;
        return msg;
    }();

    std::vector<double> tolerance_pose(3, 0.01);
    std::vector<double> tolerance_angle(3, 0.01);

    moveit_msgs::msg::Constraints pose_goal =
    kinematic_constraints::constructGoalConstraints("tool0", target_pose, tolerance_pose, tolerance_angle);

    goal_motion_request.group_name = planning_group;
    goal_motion_request.goal_constraints.push_back(pose_goal);

with these:

moveit::core::RobotState goal_state(robot_model);
    std::vector<double> joint_values = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
    goal_state.setJointGroupPositions(joint_model_group, joint_values);

    goal_motion_request.goal_constraints.resize(1);
    goal_motion_request.goal_constraints[0] =
        kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);

in test/hybrid_planning_demo_node.cpp, and the hybrid planner will succeed.

Launching the code

ros2 launch ur_robot_driver ur10e.launch.py robot_ip:=aaa.bbb.cc.ddd use_fake_hardware:=true initial_joint_controller:=joint_trajectory_controller
ros2 launch ur_moveit_config ur_moveit.launch.py use_fake_hardware:=true ur_type:=ur10e use_sim_time:=true
ros2 launch moveit_hybrid_planning hybrid_planning_demo.launch.py

Reason for using use_sim_time:=true

Expected behaviour

A UR10e robot moves to a certain position

Actual behaviour

Planning fails

Backtrace or Console output

Console output

[skaeringur97]

This issue was caused by kinematics.yaml not loading correctly. I used this function to load it:

def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)
    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available    
        return None

While if I use kinematics_yaml = PathJoinSubstitution( [FindPackageShare("ur_moveit_config"), "config", "kinematics.yaml"] ) instead, it works!

[fmauch]

@skaeringur97 which file do you refer to? I assume something outside of this repository? Or did you have an old version of

https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/6dc5d4d478ed4f273c1394e55875900e8d1eeaba/ur_moveit_config/launch/ur_moveit.launch.py#L145-L147?

[skaeringur97]

@fmauch I was loading the same kinematics.yaml file as you showed, but using load_yaml("moveit_config_package", "config/kinematics.yaml" instead of the correct way you showed.