MoveIt-Py unable to construct goal representation with PoseStamped

AGuthmann commented 5 months ago

Description

I am currently trying to get the MoveItPy package to work with an URSimulator. Followed this approach for the MoveIt-Py-tutorial and almost got all of the examples to work. But one -Plan and Execute Motion with PoseStamped- runs into an ominous error: [moveit.ompl_planning.model_based_planning_context]: Unable to construct goal representation without any further information.

Did anyone run into a similar issue before and could provide help with it?

Your environment

(All Packages Binary)

OS Version: WSL2 Ubuntu 22.04
UniversalRobots URSim_cb3 (Docker)
ROS Distro: Rolling : 2.8.0-2jammy.20231019.013248 amd64
ros-rolling-ros2-control 3.19.1-1jammy.20231018.232413 amd64
ros-rolling-ros2-controllers 3.16.0-1jammy.20231018.233440 amd64
ur_ros2_driver (ros-rolling-ur): 2.4.1-1jammy.20231031.014900 amd64
ros-rolling-moveit: 2.8.0-2jammy.20231019.013248 amd64
ross-rolling-moveit-py: 2.8.0-2jammy.20231019.005438 amd64

Steps to reproduce

Install ros2 Rolling and all other listed binaries
Source ros2 environment
Create own package with code example from here
Adjust config directories from moveit_tutorial to UR_Driver or current package.
Only return moveit_py_node as node to start (easier to debug when moveit move_group node runs separate)
Add launch and config files to setup.py
Launch Ursim ros2 run ur_client_library start_ursim.sh -m ur5
Launch UR_Driver & wait for it to start all controllers ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5 robot_ip:=localhost launch_rviz:=false
Launch MoveIt Move_group ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5 launch_rviz:=true
Launch MoveIt_py_node ros2 launch hello_py_ur hello_py_ur.launch.py

Expected behaviour

The robot should be able to plan a trajectory and move to the given coordinates. The coordinates should be valid since they are the same as in the C++ tutorial.

Actual behaviour

Instead of getting a planned trajectory, the robot stays in place and only displays the ERROR: Unable to construct goal representation

Backtrace or Console output

[INFO] [launch]: All log files can be found below ~/.ros/log/2024-01-31-08-59-58-102474-WKS0000644601-77628
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [hello_py_ur-1]: process started with pid [77640]
[hello_py_ur-1] [INFO] [1706687998.971443917] [moveit_cpp_initializer]: Initialize rclcpp
[hello_py_ur-1] [INFO] [1706687998.972696020] [moveit_cpp_initializer]: Initialize node parameters
[hello_py_ur-1] [INFO] [1706687998.972719620] [moveit_cpp_initializer]: Initialize node and executor
[hello_py_ur-1] [INFO] [1706687999.027788319] [moveit_cpp_initializer]: Spin separate thread
[hello_py_ur-1] [INFO] [1706687999.032909428] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00497661 seconds
[hello_py_ur-1] [INFO] [1706687999.032976828] [moveit_robot_model.robot_model]: Loading robot model 'ur'...
[hello_py_ur-1] [INFO] [1706687999.032983328] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[hello_py_ur-1] [WARN] [1706687999.049702458] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[hello_py_ur-1] [INFO] [1706687999.104146256] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[hello_py_ur-1] [INFO] [1706687999.104360556] [moveit.ros_planning_interface.moveit_cpp]: Listening to '/joint_states' for joint states
[hello_py_ur-1] [INFO] [1706687999.105521358] [moveit_ros.current_state_monitor]: Listening to joint states on topic '/joint_states'
[hello_py_ur-1] [INFO] [1706687999.106591960] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/moveit_cpp/planning_scene_monitor' for attached collision objects
[hello_py_ur-1] [INFO] [1706687999.106624860] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[hello_py_ur-1] [INFO] [1706687999.107360261] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/moveit_cpp/publish_planning_scene'
[hello_py_ur-1] [INFO] [1706687999.107390561] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[hello_py_ur-1] [INFO] [1706687999.108157063] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[hello_py_ur-1] [INFO] [1706687999.108896664] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[hello_py_ur-1] [WARN] [1706687999.109005164] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[hello_py_ur-1] [ERROR] [1706687999.109023464] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[hello_py_ur-1] [INFO] [1706687999.119580383] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[hello_py_ur-1] [INFO] [1706687999.123346290] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[hello_py_ur-1] [INFO] [1706687999.123397090] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[hello_py_ur-1] [INFO] [1706687999.123416290] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[hello_py_ur-1] [INFO] [1706687999.123419990] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[hello_py_ur-1] [INFO] [1706687999.123423590] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[hello_py_ur-1] [INFO] [1706687999.129915502] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[hello_py_ur-1] [INFO] [1706687999.146702932] [moveit.pilz_industrial_motion_planner]: Available plugins: pilz_industrial_motion_planner/PlanningContextLoaderCIRC pilz_industrial_motion_planner/PlanningContextLoaderLIN pilz_industrial_motion_planner/PlanningContextLoaderPTP 
[hello_py_ur-1] [INFO] [1706687999.146746132] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderCIRC
[hello_py_ur-1] [INFO] [1706687999.148119535] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [CIRC]
[hello_py_ur-1] [INFO] [1706687999.148171535] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderLIN
[hello_py_ur-1] [INFO] [1706687999.149006136] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [LIN]
[hello_py_ur-1] [INFO] [1706687999.149041436] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderPTP
[hello_py_ur-1] [INFO] [1706687999.149945638] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [PTP]
[hello_py_ur-1] [INFO] [1706687999.149981438] [moveit.ros_planning.planning_pipeline]: Using planning interface 'Pilz Industrial Motion Planner'
[hello_py_ur-1] [INFO] [1706687999.155972949] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[hello_py_ur-1] [INFO] [1706687999.156017649] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[hello_py_ur-1] [INFO] [1706687999.156023149] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[hello_py_ur-1] [INFO] [1706687999.156026949] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[hello_py_ur-1] [INFO] [1706687999.233728588] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller
[hello_py_ur-1] [INFO] [1706687999.238728097] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller
[hello_py_ur-1] [INFO] [1706687999.238919898] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[hello_py_ur-1] [INFO] [1706687999.238942598] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[hello_py_ur-1] [INFO] [1706687999.239621699] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[hello_py_ur-1] [INFO] [1706687999.249072116] [moveit_py.pose_goal]: MoveItPy instance created
[hello_py_ur-1] [INFO] [1706687999.249705017] [moveit_py.pose_goal]: 
[hello_py_ur-1] 
[hello_py_ur-1]  Start Plan 3:
[hello_py_ur-1] 
[hello_py_ur-1] [INFO] [1706687999.508993382] [moveit_py.pose_goal]: Planning trajectory
[hello_py_ur-1] [ERROR] [1706687999.509948084] [moveit.ompl_planning.model_based_planning_context]: Unable to construct goal representation
[hello_py_ur-1] [WARN] [1706687999.510036084] [moveit.ros_planning.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[hello_py_ur-1] [ERROR] [1706687999.510847186] [moveit_py.pose_goal]: Planning failed
[hello_py_ur-1] [INFO] [1706688002.518970382] [moveit.ros_planning_interface.moveit_cpp]: Deleting MoveItCpp

Gist with relevant files

github-actions[bot] commented 3 months ago

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

peterdavidfagan commented 2 months ago

Thank you @AGuthmann for posting this detailed issue, I am setting up a workspace for UR10s in my lab and it will involve the usage of the MoveIt python library. When I go through this I will try to replicate this error, my work will also be open sourced so if it works on my setup I can share this as a reference.

github-actions[bot] commented 3 weeks ago

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

moveit / moveit2