Moveit path is not executed even though solution found (too many states) in Webots
I integrated moveit in a webots environment with a UR. For that I installed the ros2 UR drivers. Everything starts without an error, and I tested it by manually dragging the end effector of the UR to random valid positions, and for some of them it works.
However, for some others, either it does find a solution and it doesn't move, either it cannot find a trajectory to the goal, either the solution is so computationally expensive that is not executed.
I tried the same position with the real robot (or using the usefakehardware of the UR drivers), and the robot finds a solution and executes, for this goal. So there must be something wrong with the connection with Webots.
I use ROS2 Foxy and Webots2022 with the ROS2 UR drivers (foxy). Any idea where I may be wrong? Or where I should search for a solution? Or someone that had similar case (integrating moveit in UR in webots) that may help?
The terminal looks like this (for each waypoint, it finds 150+ states which may be computationally expensive. is this normal?):
[move_group-1] [INFO] [1662472286.382194673] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-1] [INFO] [1662472286.382257541] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[rviz2-2] [INFO] [1662472286.682515286] [move_group_interface]: Plan and Execute request accepted
[move_group-1] [INFO] [1662472293.788990135] [moveit_ros.current_state_monitor]: Didn't received robot state (joint angles) with recent timestamp within 1.000000 seconds.
[move_group-1] Check clock synchronization if your are running ROS across multiple machines!
[move_group-1] [WARN] [1662472293.789042770] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to fetch current robot state.
[move_group-1] [INFO] [1662472293.789123110] [moveit_move_group_default_capabilities.move_action_capability]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[move_group-1] [INFO] [1662472293.789279686] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1
[move_group-1] [INFO] [1662472293.789291751] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'move_group'
[move_group-1] [INFO] [1662472293.793567373] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-1] [INFO] [1662472293.795332526] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662472293.795377402] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662472293.795417088] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662472293.795721994] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662472298.794576553] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 196 states (100 start + 96 goal)
[move_group-1] [INFO] [1662472298.794629630] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/src/ProblemDefinition.cpp:437 - ProblemDefinition: Adding approximate solution from planner ur_manipulator/ur_manipulator
[move_group-1] [INFO] [1662472298.802329254] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 189 states (105 start + 84 goal)
[move_group-1] [INFO] [1662472298.802362356] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/src/ProblemDefinition.cpp:437 - ProblemDefinition: Adding approximate solution from planner ur_manipulator/ur_manipulator
[move_group-1] [INFO] [1662472298.811363599] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 170 states (101 start + 69 goal)
[move_group-1] [INFO] [1662472298.811409872] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/src/ProblemDefinition.cpp:437 - ProblemDefinition: Adding approximate solution from planner ur_manipulator/ur_manipulator
[move_group-1] [INFO] [1662472298.813679027] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 187 states (96 start + 91 goal)
[move_group-1] [INFO] [1662472298.813710534] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/src/ProblemDefinition.cpp:437 - ProblemDefinition: Adding approximate solution from planner ur_manipulator/ur_manipulator
[move_group-1] [INFO] [1662472298.813928206] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/src/ProblemDefinition.cpp:437 - ProblemDefinition: Adding approximate solution from planner PathHybridization
[move_group-1] [INFO] [1662472298.813965329] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/tools/multiplan/src/ParallelPlan.cpp:135 - ParallelPlan::solve(): Solution found by one or more threads in 5.020122 seconds
[move_group-1] [WARN] [1662472298.814005298] [moveit.ompl_planning.model_based_planning_context]: Computed solution is approximate
[move_group-1] [INFO] [1662472298.814019232] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[move_group-1] [INFO] [1662472298.814076225] [moveit_move_group_default_capabilities.move_action_capability]: No motion plan found. No execution attempted.
[rviz2-2] [INFO] [1662472299.098724661] [move_group_interface]: Plan and Execute request aborted
[rviz2-2] [ERROR] [1662472299.198888073] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached
Below I also post the terminal output of a moving command that was actually executed, only for comparison (which is strange since the output indicates that it would not be executed):
[move_group-1] [INFO] [1662473359.885897712] [moveit_move_group_default_capabilities.move_action_capability]: Received
request
[move_group-1] [INFO] [1662473359.886080805] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[rviz2-2] [INFO] [1662473360.186360385] [move_group_interface]: Plan and Execute request accepted
[move_group-1] [INFO] [1662473370.190854749] [moveit_ros.current_state_monitor]: Didn't received robot state (joint angles) with recent timestamp within 1.000000 seconds.
[move_group-1] Check clock synchronization if your are running ROS across multiple machines!
[move_group-1] [WARN] [1662473370.190900284] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to fetch current robot state.
[move_group-1] [INFO] [1662473370.191064040] [moveit_move_group_default_capabilities.move_action_capability]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[move_group-1] [INFO] [1662473370.191201575] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1
[move_group-1] [INFO] [1662473370.191212283] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'move_group'
[move_group-1] [INFO] [1662473370.196334385] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-1] [INFO] [1662473370.198317843] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.198363645] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.198400580] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.198432200] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.265451419] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 4 states (2 start + 2 goal)
[move_group-1] [INFO] [1662473370.267571502] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 4 states (2 start + 2 goal)
[move_group-1] [INFO] [1662473370.277358788] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 4 states (2 start + 2 goal)
[move_group-1] [INFO] [1662473370.327611256] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 4 states (2 start + 2 goal)
[move_group-1] [INFO] [1662473370.327856688] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/tools/multiplan/src/ParallelPlan.cpp:135 - ParallelPlan::solve(): Solution found by one or more threads in 0.131146 seconds
[move_group-1] [INFO] [1662473370.328139992] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.328174957] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.328206656] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.328228035] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.374162402] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 4 states (2 start + 2 goal)
[move_group-1] [INFO] [1662473370.386446835] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 4 states (2 start + 2 goal)
[move_group-1] [INFO] [1662473370.391317403] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 5 states (3 start + 2 goal)
[move_group-1] [INFO] [1662473370.405680316] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 4 states (2 start + 2 goal)
[move_group-1] [INFO] [1662473370.405916727] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/tools/multiplan/src/ParallelPlan.cpp:135 - ParallelPlan::solve(): Solution found by one or more threads in 0.077925 seconds
[move_group-1] [INFO] [1662473370.406170912] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.406211945] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-1] [INFO] [1662473370.457288716] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 4 states (2 start + 2 goal)
[move_group-1] [INFO] [1662473370.489098404] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 5 states (3 start + 2 goal)
[move_group-1] [INFO] [1662473370.489278770] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/tools/multiplan/src/ParallelPlan.cpp:135 - ParallelPlan::solve(): Solution found by one or more threads in 0.083257 seconds
[move_group-1] [INFO] [1662473370.775374151] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/src/SimpleSetup.cpp:179 - SimpleSetup: Path simplification took 0.285941 seconds and changed from 3 to 2 states
[move_group-1] [ERROR] [1662473370.916567070] [moveit_ros.planning_scene_monitor.trajectory_monitor]: The sampling frequency for trajectory states should be positive
[move_group-1] [INFO] [1662473370.916616747] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-1] [INFO] [1662473370.916695955] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-1] [INFO] [1662473370.916758979] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to joint_trajectory_controller
[driver-4] [INFO] [1662473370.917300773] [joint_trajectory_controller]: Received new action goal
[driver-4] [INFO] [1662473370.917390816] [joint_trajectory_controller]: Accepted new action goal
[move_group-1] [INFO] [1662473370.917638429] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: joint_trajectory_controller started execution
[move_group-1] [INFO] [1662473370.917656441] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[move_group-1] [WARN] [1662473382.911419328] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: waitForExecution timed out
[move_group-1] [WARN] [1662473382.911488374] [moveit_ros.trajectory_execution_manager]: Controller handle joint_trajectory_controller reports status RUNNING
[move_group-1] [INFO] [1662473382.911501997] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status RUNNING ...
[move_group-1] [INFO] [1662473382.916974272] [moveit_move_group_default_capabilities.move_action_capability]: Solution found but controller failed during execution
[rviz2-2] [INFO] [1662473383.217288563] [move_group_interface]: Plan and Execute request aborted
[rviz2-2] [ERROR] [1662473383.317434128] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached
[driver-4] [INFO] [1662473466.533250225] [joint_trajectory_controller]: Goal reached, success!
[move_group-1] [INFO] [1662473466.533495683] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller joint_trajectory_controller successfully finished
Asked by AlexandrosNic on 2022-09-06 09:06:49 UTC
Comments
This may be related to the MoveIt configuration in: https://github.com/cyberbotics/webots_ros2/tree/master/webots_ros2_universal_robot/resource
There was no MoveIt2 UR5 configuration at the time of the package creation, so the MoveIt configuration is generated using MoveIt Setup Assistant from the old MoveIt.
Asked by lukicdarkoo on 2022-09-07 05:12:25 UTC
Thank you for your reply Darko (and btw I consulted your project for creating the vacuum gripper). I used the official moveit configuration of the UR robots: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/main/ur_moveit_config so I would assume that this shouldn't be the case. Also I have to add that oddly, the robot doesn't have the same behavior for the real robot (or the use)fake)hardware option of the UR driver), i.e. when it finds a solution, it executes it. So there must be a problem with how this is integrated in webots?!
Asked by AlexandrosNic on 2022-09-07 05:36:41 UTC
Webots simply exposes a joint trajectory interface through ros2_control. The interface is used for control (not for planning) so I don't see how it could affect the planning. Maybe I am missing something, but I don't see a way for Webots to affect the planning.
Asked by lukicdarkoo on 2022-09-07 09:09:29 UTC
You are right. The problem lies somewhere else, and specifically in the approximate solution of moveit, which I was not able yet to find a solution to it
Asked by AlexandrosNic on 2022-09-21 10:41:23 UTC