Moveit path is not executed even though solution found (too many states) in Webots

asked 2022-09-06 09:06:49 -0600

AlexandrosNic gravatar image

updated 2022-09-06 09:13:45 -0600

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 use_fake_hardware 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 ...
(more)
edit retag flag offensive close merge delete

Comments

This may be related to the MoveIt configuration in: https://github.com/cyberbotics/webots...

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.

lukicdarkoo gravatar image lukicdarkoo  ( 2022-09-07 05:12:25 -0600 )edit

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/Un... 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?!

AlexandrosNic gravatar image AlexandrosNic  ( 2022-09-07 05:36:41 -0600 )edit

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.

lukicdarkoo gravatar image lukicdarkoo  ( 2022-09-07 09:09:29 -0600 )edit

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

AlexandrosNic gravatar image AlexandrosNic  ( 2022-09-21 10:41:23 -0600 )edit