setPreGraspPose fails when planning to a previously used state

[Giovadess]

Description

Hello, I am currently trying to execute a pick and place task using the mobile manipulator stretch. The code (https://github.com/PickNikRobotics/stretch_moveit_plugins/blob/main/pick_place_task/src/pick_place_task.cpp) is from their own repo but the issue seems to be with the task constructor.

When I plan the grasping, the planning fails at "setPreGraspPose" with the following error :

The weird thing that makes me think of a bug is that if I take out all the stages after the "Open Hand" one in line 48, the planning actually happens and I am able to see the gripper opening from the starting stage in Rviz.

From this I understand that the state is correctly loaded from the srdf and is read from Moveit, but I am not sure how the setGraspPose is unable to find it. Is this a known issue? Or is there a work around to it?

Your environment

• ROS Distro: [Galactic]
• OS Version: e.g. Ubuntu 20.04
• Source

Steps to reproduce

Download the stretch_repo from https://github.com/hello-robot/stretch_ros and run the Demo. Launch the task from the pick_place_task repo. (pick_place_demo.launch.py)

Expected behaviour

The robot should be able to plan from the grasp pose and move the all mobile base to the object as in this video. https://www.youtube.com/watch?v=Tm93GFlT234&t=19s

Actual behaviour

Unable to plan.

Backtrace or Console output

[INFO] [launch]: All log files can be found below /home/gdessy/.ros/log/2023-08-29-10-00-36-885648-iitavr005dw002u-9454 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [pick_place_task_demo-1]: process started with pid [9458] [pick_place_task_demo-1] [INFO] [1693296037.437636821] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00523423 seconds [pick_place_task_demo-1] [INFO] [1693296037.437696365] [moveit_robot_model.robot_model]: Loading robot model 'stretch_description'... [pick_place_task_demo-1] [WARN] [1693296037.635075018] [moveit_robot_model.robot_model]: Group state 'lift_down' doesn't specify all group joints in group 'stretch_arm'. joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw are missing. [pick_place_task_demo-1] [WARN] [1693296037.635122251] [moveit_robot_model.robot_model]: Group state 'lift_up' doesn't specify all group joints in group 'stretch_arm'. joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw are missing. [pick_place_task_demo-1] [WARN] [1693296037.635132882] [moveit_robot_model.robot_model]: Group state 'arm_in' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_wrist_yaw are missing. [pick_place_task_demo-1] [WARN] [1693296037.635140912] [moveit_robot_model.robot_model]: Group state 'arm_out' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_wrist_yaw are missing. [pick_place_task_demo-1] [WARN] [1693296037.635147653] [moveit_robot_model.robot_model]: Group state 'wrist_out' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0 are missing. [pick_place_task_demo-1] [WARN] [1693296037.635154183] [moveit_robot_model.robot_model]: Group state 'wrist_in' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0 are missing. [pick_place_task_demo-1] [WARN] [1693296037.641067973] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml! [pick_place_task_demo-1] [INFO] [1693296038.187428851] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states' [pick_place_task_demo-1] [INFO] [1693296038.189872131] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects [pick_place_task_demo-1] [INFO] [1693296038.201615515] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'planning_scene' [pick_place_task_demo-1] [INFO] [1693296038.702139414] [pick_place_task]: Initializing task pipeline [pick_place_task_demo-1] [INFO] [1693296038.715022908] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00379643 seconds [pick_place_task_demo-1] [INFO] [1693296038.715046589] [moveit_robot_model.robot_model]: Loading robot model 'stretch_description'... [pick_place_task_demo-1] [WARN] [1693296038.911782884] [moveit_robot_model.robot_model]: Group state 'lift_down' doesn't specify all group joints in group 'stretch_arm'. joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw are missing. [pick_place_task_demo-1] [WARN] [1693296038.911823027] [moveit_robot_model.robot_model]: Group state 'lift_up' doesn't specify all group joints in group 'stretch_arm'. joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw are missing. [pick_place_task_demo-1] [WARN] [1693296038.911832517] [moveit_robot_model.robot_model]: Group state 'arm_in' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_wrist_yaw are missing. [pick_place_task_demo-1] [WARN] [1693296038.911841028] [moveit_robot_model.robot_model]: Group state 'arm_out' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_wrist_yaw are missing. [pick_place_task_demo-1] [WARN] [1693296038.911847768] [moveit_robot_model.robot_model]: Group state 'wrist_out' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0 are missing. [pick_place_task_demo-1] [WARN] [1693296038.911854429] [moveit_robot_model.robot_model]: Group state 'wrist_in' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0 are missing. [pick_place_task_demo-1] [WARN] [1693296038.913962975] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml! [pick_place_task_demo-1] [INFO] [1693296038.915966974] [pick_place_task]: Start searching for task solutions [pick_place_task_demo-1] [WARN] [1693296038.916199980] [pick_place_demo]: Failed to find 'ompl.planning_plugin'. Attempting to load pipeline from old parameter structure. Please update your MoveIt config. [pick_place_task_demo-1] [INFO] [1693296038.935555984] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL' [pick_place_task_demo-1] [INFO] [1693296038.938711852] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.path_tolerance' was not set. Using default value: 0.100000 [pick_place_task_demo-1] [INFO] [1693296038.938761196] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.resample_dt' was not set. Using default value: 0.100000 [pick_place_task_demo-1] [INFO] [1693296038.938767726] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.min_angle_change' was not set. Using default value: 0.001000 [pick_place_task_demo-1] [INFO] [1693296038.938784737] [moveit_ros.fix_workspace_bounds]: Param 'move_group.default_workspace_bounds' was not set. Using default value: 10.000000 [pick_place_task_demo-1] [INFO] [1693296038.938802589] [moveit_ros.fix_start_state_bounds]: Param 'move_group.start_state_max_bounds_error' was set to 0.100000 [pick_place_task_demo-1] [INFO] [1693296038.938808249] [moveit_ros.fix_start_state_bounds]: Param 'move_group.start_state_max_dt' was not set. Using default value: 0.500000 [pick_place_task_demo-1] [INFO] [1693296038.938820990] [moveit_ros.fix_start_state_collision]: Param 'move_group.start_state_max_dt' was not set. Using default value: 0.500000 [pick_place_task_demo-1] [INFO] [1693296038.938827090] [moveit_ros.fix_start_state_collision]: Param 'move_group.jiggle_fraction' was not set. Using default value: 0.020000 [pick_place_task_demo-1] [INFO] [1693296038.938839191] [moveit_ros.fix_start_state_collision]: Param 'move_group.max_sampling_attempts' was not set. Using default value: 100 [pick_place_task_demo-1] [INFO] [1693296038.938852152] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization' [pick_place_task_demo-1] [INFO] [1693296038.938858793] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds' [pick_place_task_demo-1] [INFO] [1693296038.938863153] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds' [pick_place_task_demo-1] [INFO] [1693296038.938867033] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision' [pick_place_task_demo-1] [INFO] [1693296038.938870813] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints' [pick_place_task_demo-1] [ERROR] [1693296038.941335835] [moveit_exceptions.exceptions]: unknown state 'open' [pick_place_task_demo-1] Exception thrown. [pick_place_task_demo-1] [ERROR] [1693296038.942966038] [pick_place_task]: Initialization failed: Error initializing stage: [pick_place_task_demo-1] generate grasp pose: invalid pregrasp: unknown state 'open' [pick_place_task_demo-1] [pick_place_task_demo-1] [ERROR] [1693296038.942988269] [pick_place_demo]: Failed to plan