search

bhomaidan1990 / yumi_egm_driver

GNU Lesser General Public License v2.1
1 stars 1 forks source link

Need help on execution #1

Closed Mahdiford closed 2 years ago

Mahdiford commented 2 years ago

Would you please add some instructions about how to use this repo? There are many features, and I am confused about how to use the pick and place example or even execute the motion_api initialization.

bhomaidan1990 commented 2 years ago

I use this repo to control YuMi using EGM, along with abb_robot_driver you need to install StateMachine AddIn on YuMi using Robot Studio ( Which will forbid you from using old driver ! ). After that, you can lunch MoveIt using this launch file, and then you call the Motion-API-Node which serves as a server you can call it as done here.

Mahdiford commented 2 years ago

@bhomaidan1990 Thank you for the details. I tried the steps and in the motion_api_node I got this: "Planning Time is: 0.402 seconds! ===================== EGM Settings Updated ===================== ok: True [ INFO] [1656860131.062482250]: ABORTED: Solution found but controller failed during execution" Also, this is my rqt graph: Screenshot from 2022-07-03 07-58-55

The ex3 from bring-up-examples is called in the moveit launch file, in the instructions on abb driver this service should be called in order to activate the egm "rosservice call /yumi/egm/controller_manager/switch_controller "start_controllers: [joint_group_velocity_controller]". As you can see this service (/yumi/egm/controller_manager/switch_controller ) is not called in the rqt graph. Would you please clarify this?

bhomaidan1990 commented 2 years ago

@Mahdiford Please provide more details, is the StateMachine AddIn installed on YuMi? what are the log messages there? You can see here that I'm starting the controller. Note that you might need these modifications to the abb robot driver to interface it with MoveIt. If the issue is not solved then, please feel free to open it with further details.

Mahdiford commented 2 years ago

@bhomaidan1990 Perfect! Thanks a lot. Now I got some progress. The modifications you mentioned solved some errors when executing launch file and motion API. Now my rqt becomes like this: Screenshot from 2022-07-03 09-34-46

On the Flexpendant I got starting joint motion for both arms. Also, I installed the original EGM and state machine on the robot side without any modification. I followed ABB's documentation and ex3 example worked fine. But still when I execute the servcall I got this on the motion API side :

"===================== Grippers: Calibrated ===================== ===================== YuMi Initialized! ===================== ===================== Start Motion Initialization ===================== [ INFO] [1656866336.171996713]: Loading robot model 'yumi'... [ WARN] [1656866336.268858800]: Group state 'Calib' doesn't specify all group joints in group 'both_arms'. gripper_l_joint, gripper_r_joint are missing. [ WARN] [1656866336.268889372]: Group state 'Calib_left' doesn't specify all group joints in group 'left_arm'. gripper_l_joint is missing. [ WARN] [1656866336.268901490]: Group state 'Calib_right' doesn't specify all group joints in group 'right_arm'. gripper_r_joint is missing. [ INFO] [1656866339.439648604]: Ready to take commands for planning group left_arm. [ INFO] [1656866339.731679447]: Ready to take commands for planning group right_arm. [ INFO] [1656866340.131904451]: Ready to take commands for planning group both_arms. ===================== Motion Initialization Successful! =====================

YuMi MoveIt! motion initialized within 6.971 seconds!

Planning Time is: 0.357 seconds! ===================== EGM Settings Updated ===================== ok: True [ INFO] [1656866350.492876236]: ABORTED: Solution found but controller failed during execution

"

bhomaidan1990 commented 2 years ago

@bhomaidan1990 Perfect! Thanks a lot. Now I got some progress. The modifications you mentioned solved some errors when executing launch file and motion API. Now my rqt becomes like this: Screenshot from 2022-07-03 09-34-46

On the Flexpendant I got starting joint motion for both arms. Also, I installed the original EGM and state machine on the robot side without any modification. I followed ABB's documentation and ex3 example worked fine. But still when I execute the servcall I got this on the motion API side :

"===================== Grippers: Calibrated ===================== ===================== YuMi Initialized! ===================== ===================== Start Motion Initialization ===================== [ INFO] [1656866336.171996713]: Loading robot model 'yumi'... [ WARN] [1656866336.268858800]: Group state 'Calib' doesn't specify all group joints in group 'both_arms'. gripper_l_joint, gripper_r_joint are missing. [ WARN] [1656866336.268889372]: Group state 'Calib_left' doesn't specify all group joints in group 'left_arm'. gripper_l_joint is missing. [ WARN] [1656866336.268901490]: Group state 'Calib_right' doesn't specify all group joints in group 'right_arm'. gripper_r_joint is missing. [ INFO] [1656866339.439648604]: Ready to take commands for planning group left_arm. [ INFO] [1656866339.731679447]: Ready to take commands for planning group right_arm. [ INFO] [1656866340.131904451]: Ready to take commands for planning group both_arms. ===================== Motion Initialization Successful! =====================

YuMi MoveIt! motion initialized within 6.971 seconds!

Planning Time is: 0.357 seconds! ===================== EGM Settings Updated ===================== ok: True [ INFO] [1656866350.492876236]: ABORTED: Solution found but controller failed during execution

"

What is the output on the Launch File Terminal? There should be an error message there. Also, can you call the service to list the controllers that are stopped / running / etc ...

Mahdiford commented 2 years ago

@bhomaidan1990 I got this error in moveit_planning_execution.launch

`[ INFO] [1656868964.525422929]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner [ INFO] [1656868964.525435163]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1656868968.472338213]: Loading robot model 'yumi'... [ WARN] [1656868968.576542609]: Group state 'Calib' doesn't specify all group joints in group 'both_arms'. gripper_l_joint, gripper_r_joint are missing. [ WARN] [1656868968.576601419]: Group state 'Calib_left' doesn't specify all group joints in group 'left_arm'. gripper_l_joint is missing. [ WARN] [1656868968.576613514]: Group state 'Calib_right' doesn't specify all group joints in group 'right_arm'. gripper_r_joint is missing. [ INFO] [1656868968.886266068]: Starting planning scene monitor [ INFO] [1656868968.888018945]: Listening to '/move_group/monitored_planning_scene' [ INFO] [1656868968.944276665]: Constructing new MoveGroup connection for group 'left_arm' in namespace '' [ INFO] [1656868970.102616961]: Ready to take commands for planning group left_arm. [ WARN] [1656868974.426512785]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656868984.445281336]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656868994.464326237]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656869004.483548012]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656869014.502221717]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656869024.521207573]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656869025.777500731]: Tried to remove world object 'table', but it does not exist in this scene. [ WARN] [1656869026.109756670]: Empty quaternion found in pose message. Setting to neutral orientation. [ WARN] [1656869034.539397394]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656869044.558207167]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ INFO] [1656869051.390731214]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [ INFO] [1656869051.392463605]: Planner configuration 'both_arms[RRTConnect]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [ INFO] [1656869051.393137554]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.393208538]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.393235267]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.393257115]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.412433079]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.412666211]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.416120436]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.416985169]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.417076174]: ParallelPlan::solve(): Solution found by one or more threads in 0.024326 seconds [ INFO] [1656869051.417240037]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.417263911]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.417287524]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.417360145]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.425663661]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.426721573]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.430836691]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.431406465]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.431499085]: ParallelPlan::solve(): Solution found by one or more threads in 0.014344 seconds [ INFO] [1656869051.431596958]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.431650020]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.431698253]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.431760488]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.440213741]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.440246105]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.440367412]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.441519463]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.441602405]: ParallelPlan::solve(): Solution found by one or more threads in 0.010047 seconds [ INFO] [1656869051.441743099]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.441832467]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.441883225]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.441912867]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.450744725]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.451467687]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.455760502]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.455921279]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.456001920]: ParallelPlan::solve(): Solution found by one or more threads in 0.014329 seconds [ INFO] [1656869051.456165092]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.456226523]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.456314490]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.456383753]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.464392241]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.465487744]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.465714753]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.467410133]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.467532523]: ParallelPlan::solve(): Solution found by one or more threads in 0.011449 seconds [ INFO] [1656869051.467659883]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.467680756]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.467747322]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.467774269]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.476139284]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.476314866]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.477559791]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.478164250]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.478281616]: ParallelPlan::solve(): Solution found by one or more threads in 0.010679 seconds [ INFO] [1656869051.478375370]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.478406256]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.478432229]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.478452494]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.487266510]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.487642016]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.488398658]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.489114762]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.489246507]: ParallelPlan::solve(): Solution found by one or more threads in 0.010911 seconds [ INFO] [1656869051.489341925]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.489393478]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.489457898]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.489510615]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.497441107]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.497788993]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.498478649]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.501106594]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.501249409]: ParallelPlan::solve(): Solution found by one or more threads in 0.011947 seconds [ INFO] [1656869051.501380954]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.501403241]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.501442711]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.501497020]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.510074543]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.510193998]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.510228242]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.510276496]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.510558331]: ParallelPlan::solve(): Solution found by one or more threads in 0.009238 seconds [ INFO] [1656869051.510676191]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.510693049]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.510753084]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.510775966]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.520695313]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.521227336]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.521962388]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.525018907]: both_arms/both_arms[RRTConnect]: Created 6 states (4 start + 2 goal) [ INFO] [1656869051.525215647]: ParallelPlan::solve(): Solution found by one or more threads in 0.014594 seconds [ INFO] [1656869051.525356412]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.525415118]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.525438239]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.525458929]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.535112647]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.536125362]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.536330092]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.536757754]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.536953151]: ParallelPlan::solve(): Solution found by one or more threads in 0.011666 seconds [ INFO] [1656869051.537092899]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.537115809]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.537189970]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.537217490]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.546727142]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.546777613]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.546872012]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.547167764]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.547762579]: ParallelPlan::solve(): Solution found by one or more threads in 0.010732 seconds [ INFO] [1656869051.547910633]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.547934348]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.548010018]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.548035894]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.556749503]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.557374244]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.562857959]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.563011360]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.563348814]: ParallelPlan::solve(): Solution found by one or more threads in 0.015504 seconds [ INFO] [1656869051.563488680]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.563535957]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.563562811]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.563590348]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.571082193]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.572737808]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.572995610]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.573422113]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.573620255]: ParallelPlan::solve(): Solution found by one or more threads in 0.010194 seconds [ INFO] [1656869051.573734612]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.573759585]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.573795060]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.573820365]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.583325355]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.585207425]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.587035979]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.588214754]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.588445960]: ParallelPlan::solve(): Solution found by one or more threads in 0.014760 seconds [ INFO] [1656869051.588583180]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.588677009]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.588732038]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.588818610]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.597171065]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.597690935]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.597873203]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.598347594]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.598638008]: ParallelPlan::solve(): Solution found by one or more threads in 0.010127 seconds [ INFO] [1656869051.598783349]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.598808248]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.598870200]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.598915081]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.607411722]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.608209257]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.608949659]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.609491464]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.609775826]: ParallelPlan::solve(): Solution found by one or more threads in 0.011066 seconds [ INFO] [1656869051.609896511]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.609977095]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.610027127]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.610101193]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.618559663]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.620023763]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.620705867]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.620903365]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.621206016]: ParallelPlan::solve(): Solution found by one or more threads in 0.011365 seconds [ INFO] [1656869051.621319054]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.621349679]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.621431574]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.621453001]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.630019956]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.630491846]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.631054579]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.631971476]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.632290035]: ParallelPlan::solve(): Solution found by one or more threads in 0.011022 seconds [ INFO] [1656869051.632417897]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.632463500]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.632514043]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.632564888]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.641214658]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.642493787]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.642808947]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.642943498]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.643436623]: ParallelPlan::solve(): Solution found by one or more threads in 0.011075 seconds [ INFO] [1656869051.643594281]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.643645468]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.643710169]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.643760921]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.652070598]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.652307736]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.653407605]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.654251885]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.654614087]: ParallelPlan::solve(): Solution found by one or more threads in 0.011106 seconds [ INFO] [1656869051.654746621]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.654836613]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.654924835]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.654977543]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.663296533]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.664125534]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.664330142]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.665134263]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.665508175]: ParallelPlan::solve(): Solution found by one or more threads in 0.010833 seconds [ INFO] [1656869051.665627411]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.665656971]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.665676176]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.665742137]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.673565074]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.674041056]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.675586006]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869051.676025494]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869051.676421821]: ParallelPlan::solve(): Solution found by one or more threads in 0.010855 seconds [ INFO] [1656869051.676565511]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.676647736]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.676737693]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.676761659]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.684791005]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.684936690]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.685430011]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.685517923]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.686280098]: ParallelPlan::solve(): Solution found by one or more threads in 0.009807 seconds [ INFO] [1656869051.686373632]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.686429842]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.686449510]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.686469642]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869051.694803011]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.695602840]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.695661710]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.695719007]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869051.696631666]: ParallelPlan::solve(): Solution found by one or more threads in 0.010298 seconds [ INFO] [1656869051.723649805]: SimpleSetup: Path simplification took 0.026910 seconds and changed from 3 to 2 states [ INFO] [1656869053.690764663]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline. [ INFO] [1656869053.690894989]: Planning attempt 1 of at most 1 [ INFO] [1656869053.691914646]: Planner configuration 'both_arms[RRTConnect]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [ INFO] [1656869053.692527197]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.692553221]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.692599418]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.692681663]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.711829412]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.711889258]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.712011924]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.716606924]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.716657943]: ParallelPlan::solve(): Solution found by one or more threads in 0.024398 seconds [ INFO] [1656869053.716803911]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.716884370]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.716940360]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.717013138]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.725156939]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.725535715]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.726128866]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.726711347]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.726799653]: ParallelPlan::solve(): Solution found by one or more threads in 0.010071 seconds [ INFO] [1656869053.726926459]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.726947714]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.727010247]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.727110219]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.734802727]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.736449515]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.736545625]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.741027564]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.741119894]: ParallelPlan::solve(): Solution found by one or more threads in 0.014259 seconds [ INFO] [1656869053.741251107]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.741269385]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.741426074]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.741513898]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.749624880]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.750280402]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.750551420]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.751182081]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.751292154]: ParallelPlan::solve(): Solution found by one or more threads in 0.010115 seconds [ INFO] [1656869053.751421920]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.751504776]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.751525993]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.751606650]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.759774279]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.760352356]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.761107157]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.761235455]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.761350823]: ParallelPlan::solve(): Solution found by one or more threads in 0.009999 seconds [ INFO] [1656869053.761455116]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.761501259]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.761554151]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.761632214]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.769397981]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.769685649]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.769956866]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.770130702]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.770250662]: ParallelPlan::solve(): Solution found by one or more threads in 0.008846 seconds [ INFO] [1656869053.770349247]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.770381916]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.770425000]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.770446672]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.777946305]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.779021838]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.779578297]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.779944922]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.780045189]: ParallelPlan::solve(): Solution found by one or more threads in 0.009741 seconds [ INFO] [1656869053.780189507]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.780239873]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.780259086]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.780294880]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.789165402]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.789202812]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.789411434]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.789613861]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.789768549]: ParallelPlan::solve(): Solution found by one or more threads in 0.009664 seconds [ INFO] [1656869053.789912300]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.790007226]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.790105426]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.790187818]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.798205099]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.799257116]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.799283488]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.799511695]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.799680029]: ParallelPlan::solve(): Solution found by one or more threads in 0.009841 seconds [ INFO] [1656869053.799808903]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.799830347]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.799855615]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.799878726]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.808139590]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.809148842]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.810692944]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.812249369]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.812448498]: ParallelPlan::solve(): Solution found by one or more threads in 0.012704 seconds [ INFO] [1656869053.812569810]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.812620116]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.812691197]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.812776556]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.821075062]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.821099882]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.821667921]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.821838443]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.822013458]: ParallelPlan::solve(): Solution found by one or more threads in 0.009491 seconds [ INFO] [1656869053.822147442]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.822166553]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.822188636]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.822237612]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.830629673]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.830874612]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.832131883]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.832388047]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.832606252]: ParallelPlan::solve(): Solution found by one or more threads in 0.010533 seconds [ INFO] [1656869053.832756470]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.832785385]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.832832196]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.832914677]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.841201289]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.841775018]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.841996332]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.842408343]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.842656698]: ParallelPlan::solve(): Solution found by one or more threads in 0.009993 seconds [ INFO] [1656869053.842774771]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.842861403]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.842887277]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.842942645]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.850619436]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.852371067]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.852471763]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.852758489]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.852990109]: ParallelPlan::solve(): Solution found by one or more threads in 0.010279 seconds [ INFO] [1656869053.853135274]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.853211714]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.853240658]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.853295878]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.861687366]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.861911852]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.862151057]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.862492743]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.862730294]: ParallelPlan::solve(): Solution found by one or more threads in 0.009671 seconds [ INFO] [1656869053.862824861]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.862875252]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.862902556]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.862928268]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.871104654]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.871874463]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.877603639]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.878166435]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.878416540]: ParallelPlan::solve(): Solution found by one or more threads in 0.015632 seconds [ INFO] [1656869053.878524952]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.878582170]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.878629835]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.878654900]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.886785326]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.886976686]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.887782134]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.889112580]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.889357683]: ParallelPlan::solve(): Solution found by one or more threads in 0.010882 seconds [ INFO] [1656869053.889506708]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.889594172]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.889667307]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.889693263]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.897881621]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.898247915]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.898696981]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.899941936]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.900230099]: ParallelPlan::solve(): Solution found by one or more threads in 0.010812 seconds [ INFO] [1656869053.900346043]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.900466670]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.900520861]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.900589326]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.908435588]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.908907201]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.909071885]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.910601374]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.910901305]: ParallelPlan::solve(): Solution found by one or more threads in 0.010609 seconds [ INFO] [1656869053.911022805]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.911070539]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.911152978]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.911174239]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.920327024]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.920410617]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.921082473]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.927231823]: both_arms/both_arms[RRTConnect]: Created 6 states (4 start + 2 goal) [ INFO] [1656869053.927560064]: ParallelPlan::solve(): Solution found by one or more threads in 0.016600 seconds [ INFO] [1656869053.927689092]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.927741815]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.927792046]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.927836944]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.936231283]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.936805628]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.937058448]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.937622569]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.937969967]: ParallelPlan::solve(): Solution found by one or more threads in 0.010348 seconds [ INFO] [1656869053.938087469]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.938103765]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.938148968]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.938192285]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.946607460]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.946957681]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.947376476]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.947464811]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.948090327]: ParallelPlan::solve(): Solution found by one or more threads in 0.010056 seconds [ INFO] [1656869053.948233124]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.948254410]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.948281730]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.948336389]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.956719440]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.957165466]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.957500459]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.958036862]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.958434659]: ParallelPlan::solve(): Solution found by one or more threads in 0.010267 seconds [ INFO] [1656869053.958583696]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.958714126]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.958824506]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.958898995]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.968013194]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.968659386]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.968793421]: both_arms/both_arms[RRTConnect]: Created 5 states (3 start + 2 goal) [ INFO] [1656869053.970134611]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.970527399]: ParallelPlan::solve(): Solution found by one or more threads in 0.012018 seconds [ INFO] [1656869053.970660418]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.970686812]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.970708223]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.970754801]: both_arms/both_arms[RRTConnect]: Starting planning with 1 states already in datastructure [ INFO] [1656869053.979627675]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.980177202]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.980315871]: both_arms/both_arms[RRTConnect]: Created 4 states (2 start + 2 goal) [ INFO] [1656869053.980709514]: both_arms/both_arms[RRTConnect]: Created 5 states (2 start + 3 goal) [ INFO] [1656869053.981336691]: ParallelPlan::solve(): Solution found by one or more threads in 0.010733 seconds [ INFO] [1656869054.024417205]: SimpleSetup: Path simplification took 0.043013 seconds and changed from 3 to 2 states [ERROR] [1656869054.026548203]: Can't accept new action goals. Controller is not running. [ WARN] [1656869054.026823989]: Controller '/yumi/egm/jnt_traj_vel_controller' failed with error INVALID_GOAL: [ WARN] [1656869054.026928644]: Controller handle /yumi/egm/jnt_traj_vel_controller reports status FAILED [ INFO] [1656869054.026956209]: Completed trajectory execution with status FAILED ... [ INFO] [1656869054.036831511]: Received event 'stop'`

I tested the ex3 example after applying changes you mentioned and it is not responding as well. This one:

rosservice call /yumi/egm/controller_manager/switch_controller "start_controllers: [jnt_traj_vel_controller] stop_controllers: [''] strictness: 1 start_asap: false timeout: 0.0"

returns true but the ex3 itself has this error:

`SUMMARY PARAMETERS

NODES /yumi/ egm_controller_stopper (abb_egm_hardware_interface/egm_controller_stopper) egm_hardware_interface (abb_egm_hardware_interface/egm_hardware_interface) rws_service_provider (abb_rws_service_provider/rws_service_provider) rws_state_publisher (abb_rws_state_publisher/rws_state_publisher) /yumi/egm/ started (controller_manager/spawner) stopped (controller_manager/spawner)

auto-starting new master process[master]: started with pid [960644] ROS_MASTER_URI=http://localhost:11311

setting /run_id to 70f89cca-faf5-11ec-ace2-1f50950b9fa2 process[rosout-1]: started with pid [960669] started core service [/rosout] process[yumi/rws_state_publisher-2]: started with pid [960676] process[yumi/rws_service_provider-3]: started with pid [960677] process[yumi/egm_hardware_interface-4]: started with pid [960678] process[yumi/egm_controller_stopper-5]: started with pid [960679] process[yumi/egm/started-6]: started with pid [960685] process[yumi/egm/stopped-7]: started with pid [960691] [ WARN] [1656869262.049536640]: Configured default soft limits for 'yumi_robr_joint_1': position [-2.91147; 2.91147], k: 1 [ WARN] [1656869262.059021911]: Configured default soft limits for 'yumi_robr_joint_2': position [-2.4795; 0.751626], k: 1 [ WARN] [1656869262.068071943]: Configured default soft limits for 'yumi_robr_joint_3': position [-2.91147; 2.91147], k: 1 [ WARN] [1656869262.077290844]: Configured default soft limits for 'yumi_robr_joint_4': position [-2.13393; 1.3823], k: 1 [ WARN] [1656869262.084506541]: Configured default soft limits for 'yumi_robr_joint_5': position [-5.01084; 5.01084], k: 1 [ WARN] [1656869262.088529166]: Configured default soft limits for 'yumi_robr_joint_6': position [-1.52053; 2.38446], k: 1 [ WARN] [1656869262.091795431]: Configured default soft limits for 'yumi_robr_joint_7': position [-3.95683; 3.95683], k: 1 [ WARN] [1656869262.095574471]: Configured default soft limits for 'yumi_robl_joint_1': position [-2.91147; 2.91147], k: 1 [ WARN] [1656869262.098356753]: Configured default soft limits for 'yumi_robl_joint_2': position [-2.4795; 0.751626], k: 1 [ WARN] [1656869262.100680873]: Configured default soft limits for 'yumi_robl_joint_3': position [-2.91147; 2.91147], k: 1 [ WARN] [1656869262.102864876]: Configured default soft limits for 'yumi_robl_joint_4': position [-2.13393; 1.3823], k: 1 [ WARN] [1656869262.105024526]: Configured default soft limits for 'yumi_robl_joint_5': position [-5.01084; 5.01084], k: 1 [ WARN] [1656869262.106913729]: Configured default soft limits for 'yumi_robl_joint_6': position [-1.52053; 2.38446], k: 1 [ WARN] [1656869262.108810211]: Configured default soft limits for 'yumi_robl_joint_7': position [-3.95683; 3.95683], k: 1 [ERROR] [1656869262.351753225]: Could not find parameter robot_description on parameter server [ERROR] [1656869262.351923481]: Failed to parse URDF contained in 'robot_description' parameter [ERROR] [1656869262.352002150]: Failed to initialize the controller [ERROR] [1656869262.352081214]: Initializing controller 'jnt_traj_vel_controller' failed [ WARN] [1656869262.431142247]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ERROR] [1656869263.354019]: Failed to load jnt_traj_vel_controller [ WARN] [1656869272.460581250]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656869282.490192263]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656869292.518059420]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656869302.546081170]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) [ WARN] [1656869312.574697729]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?)`

Also, these are the topics that you asked:

/attached_collision_object /collision_object /execute_trajectory/cancel /execute_trajectory/feedback /execute_trajectory/goal /execute_trajectory/result /execute_trajectory/status /joint_states /move_group/cancel /move_group/display_contacts /move_group/display_planned_path /move_group/feedback /move_group/goal /move_group/monitored_planning_scene /move_group/plan_execution/parameter_descriptions /move_group/plan_execution/parameter_updates /move_group/planning_pipelines/ompl/ompl/parameter_descriptions /move_group/planning_pipelines/ompl/ompl/parameter_updates /move_group/planning_scene_monitor/parameter_descriptions /move_group/planning_scene_monitor/parameter_updates /move_group/result /move_group/sense_for_plan/parameter_descriptions /move_group/sense_for_plan/parameter_updates /move_group/status /move_group/trajectory_execution/parameter_descriptions /move_group/trajectory_execution/parameter_updates /pickup/cancel /pickup/feedback /pickup/goal /pickup/result /pickup/status /place/cancel /place/feedback /place/goal /place/result /place/status /planning_scene /planning_scene_world /recognized_object_array /rosout /rosout_agg /rviz/moveit/execute /rviz/moveit/move_marker/goal_gripper_l_finger_r /rviz/moveit/plan /rviz/moveit/select_planning_group /rviz/moveit/stop /rviz/moveit/update_custom_goal_state /rviz/moveit/update_custom_start_state /rviz/moveit/update_goal_state /rviz/moveit/update_start_state /rviz_user_MS_7B10_961005_4192256111181628858/motionplanning_planning_scene_monitor/parameter_descriptions /rviz_user_MS_7B10_961005_4192256111181628858/motionplanning_planning_scene_monitor/parameter_updates /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full /tf /tf_static /trajectory_execution_event /yumi/egm/egm_states /yumi/egm/jnt_traj_vel_controller/command /yumi/egm/jnt_traj_vel_controller/follow_joint_trajectory/cancel /yumi/egm/jnt_traj_vel_controller/follow_joint_trajectory/feedback /yumi/egm/jnt_traj_vel_controller/follow_joint_trajectory/goal /yumi/egm/jnt_traj_vel_controller/follow_joint_trajectory/result /yumi/egm/jnt_traj_vel_controller/follow_joint_trajectory/status /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_1/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_1/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_2/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_2/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_3/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_3/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_4/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_4/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_5/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_5/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_6/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_6/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_7/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_7/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_1/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_1/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_2/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_2/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_3/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_3/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_4/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_4/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_5/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_5/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_6/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_6/parameter_updates /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_7/parameter_descriptions /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_7/parameter_updates /yumi/egm/jnt_traj_vel_controller/state /yumi/egm/joint_states /yumi/rws/joint_states /yumi/rws/sm_addin/runtime_states /yumi/rws/system_states

and services:

/apply_planning_scene /check_state_validity /clear_octomap /compute_cartesian_path /compute_fk /compute_ik /get_planner_params /get_planning_scene /move_group/get_loggers /move_group/load_map /move_group/plan_execution/set_parameters /move_group/planning_pipelines/ompl/ompl/set_parameters /move_group/planning_scene_monitor/set_parameters /move_group/save_map /move_group/sense_for_plan/set_parameters /move_group/set_logger_level /move_group/trajectory_execution/set_parameters /move_group_commander_wrappers_1656869400517714907/get_loggers /move_group_commander_wrappers_1656869400517714907/set_logger_level /move_joints /move_pose /plan_kinematic_path /query_planner_interface /robot_state_publisher/get_loggers /robot_state_publisher/set_logger_level /rosout/get_loggers /rosout/set_logger_level /rviz_user_MS_7B10_961005_4192256111181628858/get_loggers /rviz_user_MS_7B10_961005_4192256111181628858/load_config /rviz_user_MS_7B10_961005_4192256111181628858/load_config_discarding_changes /rviz_user_MS_7B10_961005_4192256111181628858/motionplanning_planning_scene_monitor/set_parameters /rviz_user_MS_7B10_961005_4192256111181628858/reload_shaders /rviz_user_MS_7B10_961005_4192256111181628858/save_config /rviz_user_MS_7B10_961005_4192256111181628858/set_logger_level /set_planner_params /yumi/egm/controller_manager/list_controller_types /yumi/egm/controller_manager/list_controllers /yumi/egm/controller_manager/load_controller /yumi/egm/controller_manager/reload_controller_libraries /yumi/egm/controller_manager/switch_controller /yumi/egm/controller_manager/unload_controller /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_1/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_2/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_3/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_4/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_5/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_6/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robl_joint_7/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_1/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_2/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_3/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_4/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_5/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_6/set_parameters /yumi/egm/jnt_traj_vel_controller/gains/yumi_robr_joint_7/set_parameters /yumi/egm/jnt_traj_vel_controller/query_state /yumi/egm/started/get_loggers /yumi/egm/started/set_logger_level /yumi/egm/stopped/get_loggers /yumi/egm/stopped/set_logger_level /yumi/egm_controller_stopper/get_loggers /yumi/egm_controller_stopper/set_logger_level /yumi/egm_hardware_interface/get_loggers /yumi/egm_hardware_interface/set_logger_level /yumi/rws/get_file_contents /yumi/rws/get_io_signal /yumi/rws/get_rapid_bool /yumi/rws/get_rapid_dnum /yumi/rws/get_rapid_num /yumi/rws/get_rapid_string /yumi/rws/get_rapid_symbol /yumi/rws/get_robot_controller_description /yumi/rws/get_speed_ratio /yumi/rws/pp_to_main /yumi/rws/set_file_contents /yumi/rws/set_io_signal /yumi/rws/set_motors_off /yumi/rws/set_motors_on /yumi/rws/set_rapid_bool /yumi/rws/set_rapid_dnum /yumi/rws/set_rapid_num /yumi/rws/set_rapid_string /yumi/rws/set_rapid_symbol /yumi/rws/set_speed_ratio /yumi/rws/sm_addin/get_egm_settings /yumi/rws/sm_addin/run_rapid_routine /yumi/rws/sm_addin/run_sg_routine /yumi/rws/sm_addin/set_egm_settings /yumi/rws/sm_addin/set_rapid_routine /yumi/rws/sm_addin/set_sg_command /yumi/rws/sm_addin/start_egm_joint /yumi/rws/sm_addin/start_egm_pose /yumi/rws/sm_addin/start_egm_stream /yumi/rws/sm_addin/stop_egm /yumi/rws/sm_addin/stop_egm_stream /yumi/rws/start_rapid /yumi/rws/stop_rapid /yumi/rws_service_provider/get_loggers /yumi/rws_service_provider/set_logger_level /yumi/rws_state_publisher/get_loggers /yumi/rws_state_publisher/set_logger_level /yumi_joint_state_publisher/get_loggers /yumi_joint_state_publisher/set_logger_level /yumi_motion_api/get_loggers /yumi_motion_api/set_logger_level

bhomaidan1990 commented 2 years ago

INVALID_GOAL

Can you please choose a different joints values as a goal and assign it here, also you can test if the EGM started that you can move the robot using MoveIt as long as the EGM start motion signal is present for the same controller that you have activated (Left, Right, Both), MoveIt can provide valid and random goals to ensure that the controller runs.

As you changed the controllers, the old EX3 will not run as before without changes! no worries.

Mahdiford commented 2 years ago

That is a good point. I can not move the robot with Moveit as well. I got the same error [ERROR] [1656870604.849253149]: Can't accept new action goals. Controller is not running. [ WARN] [1656870604.849534870]: Controller '/yumi/egm/jnt_traj_vel_controller' failed with error INVALID_GOAL: [ WARN] [1656870604.849694207]: Controller handle /yumi/egm/jnt_traj_vel_controller reports status FAILED [ INFO] [1656870604.849720030]: Completed trajectory execution with status FAILED ... [ INFO] [1656870604.849790957]: Execution completed: FAILED [ INFO] [1656870604.881901820]: ABORTED: Solution found but controller failed during execution [ WARN] [1656870608.283528832]: Timed out while waiting for EGM feedback (did the EGM session end on the robot controller?) I have also tried to change the assigned values as well. But the same error occurs.

bhomaidan1990 commented 2 years ago

@Mahdiford Have you configured your Communication Configuration as mentioned here? if yes, then I'm not sure why your controller is not running, I need a link to your repo where you put all the code that you are using, and the settings/setup that you are using, so I can debug it tomorrow with the Robot.

Mahdiford commented 2 years ago

@bhomaidan1990 I appreciated your help on this issue. I had set up the ips and ports. I will add them to a private repo and share them with you. Later on, I will share the results here for future users.

bhomaidan1990 commented 2 years ago

@Mahdiford I have updated your repository with the working code in my case.

Mahdiford commented 2 years ago

@bhomaidan1990 Thank you very much for the help. I have the chance to check the updates on the YuMi today. Two points: 1: with the current commit on your shared repo, I could run the YuMi using Moveit. I should use both arms instead of one arm. I believe it is because of the definition in the yaml file. I wasn't successful with the servcall. 2: With the new repo that you provided, I could get results out of servcall. but the Moveit is not working anymore (the position is updated accurately when the robot moves.) I appreciated your help in getting the YuMi updated. With a little bit of modification, I believe this is the best replacement for KTH's outdated repo.