Closed YuLiHN closed 1 year ago
marcbone
commented
1 year ago This is expected behavior. Maybe you can elaborate a bit, what are you trying to achieve, so we can think about changing this in the future. I guess you want to be able to guide the robot and only want to control the robot when you are executing a trajectory in MoveIt2. Is that right?
YuLiHN
commented
1 year ago This is expected behavior. Maybe you can elaborate a bit, what are you trying to achieve, so we can think about changing this in the future. I guess you want to be able to guide the robot and only want to control the robot when you are executing a trajectory in MoveIt2. Is that right?
Thanks for the fast answer. And yes, when we are planing&executing some trajectories in Moveit2, there could be some failure cases. We want to guide the robot back to the start position manually by bringing to robot to activated state, and then continue to use Moveit2 node. Otherwise it takes a lot of time to restart the Moveit2.
marcbone
commented
1 year ago So the problem is, that when you launch the MoveIt, it will start a controller that keeps the robot in its current position. When you then press the enabling device, the controller dies. You can try to disable the controller using the ros2 control cli, then guide the robot and then enable the controller again. But I am not sure if that works. What would definitely work is to move the robot back using MoveIt
YuLiHN
commented
1 year ago So the problem is, that when you launch the MoveIt, it will start a controller that keeps the robot in its current position. When you then press the enabling device, the controller dies. You can try to disable the controller using the ros2 control cli, then guide the robot and then enable the controller again. But I am not sure if that works. What would definitely work is to move the robot back using MoveIt
Thanks for your advice, we will add a function that moves the robot back using Moveit2. Closing the issue
marcbone
commented
1 year ago There is also the predefined ready pose that you can use directly as goal pose in Rviz
While the terminal is running
ros2 launch franka_moveit_config moveit.launch.py robot_ip:=my_robot_ipand I press external activation device which makes the display light turn from blue to white, the moveit node crushes because of the exceptionwhat(): libfranka: Move command aborted: User Stop pressed!. But in ros1 it is not the case. In ros1 the moveit node is still running after I bring the external activation device to the activated state (white light).my terminal is like this:
[INFO] [launch]: All log files can be found below /home/rpl/.ros/log/2023-02-03-11-10-03-834375-robotics-261820 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [rviz2-1]: process started with pid [261824] [INFO] [robot_state_publisher-2]: process started with pid [261826] [INFO] [move_group-3]: process started with pid [261828] [INFO] [ros2_control_node-4]: process started with pid [261830] [INFO] [joint_state_publisher-5]: process started with pid [261832] [INFO] [franka_gripper_node-6]: process started with pid [261834] [INFO] [ros2 run controller_manager spawner.py panda_arm_controller-7]: process started with pid [261836] [INFO] [ros2 run controller_manager spawner.py joint_state_broadcaster-8]: process started with pid [261838] [robot_state_publisher-2] Parsing robot urdf xml string. [franka_gripper_node-6] [INFO] [1675419004.204783183] [panda_gripper]: Trying to establish a connection with the gripper [franka_gripper_node-6] [INFO] [1675419004.205655200] [panda_gripper]: Connected to gripper [robot_state_publisher-2] Link panda_link0 had 1 children [robot_state_publisher-2] Link panda_link1 had 1 children [robot_state_publisher-2] Link panda_link2 had 1 children [robot_state_publisher-2] Link panda_link3 had 1 children [robot_state_publisher-2] Link panda_link4 had 1 children [robot_state_publisher-2] Link panda_link5 had 1 children [robot_state_publisher-2] Link panda_link6 had 1 children [robot_state_publisher-2] Link panda_link7 had 1 children [robot_state_publisher-2] Link panda_link8 had 1 children [robot_state_publisher-2] Link panda_hand had 3 children [robot_state_publisher-2] Link panda_leftfinger had 0 children [robot_state_publisher-2] Link panda_rightfinger had 0 children [robot_state_publisher-2] Link panda_hand_tcp had 0 children [robot_state_publisher-2] [INFO] [1675419004.206866968] [robot_state_publisher]: got segment panda_hand [robot_state_publisher-2] [INFO] [1675419004.206939973] [robot_state_publisher]: got segment panda_hand_tcp [robot_state_publisher-2] [INFO] [1675419004.206952427] [robot_state_publisher]: got segment panda_leftfinger [robot_state_publisher-2] [INFO] [1675419004.206962090] [robot_state_publisher]: got segment panda_link0 [robot_state_publisher-2] [INFO] [1675419004.206971538] [robot_state_publisher]: got segment panda_link1 [robot_state_publisher-2] [INFO] [1675419004.206980825] [robot_state_publisher]: got segment panda_link2 [robot_state_publisher-2] [INFO] [1675419004.206990021] [robot_state_publisher]: got segment panda_link3 [robot_state_publisher-2] [INFO] [1675419004.206999166] [robot_state_publisher]: got segment panda_link4 [robot_state_publisher-2] [INFO] [1675419004.207008518] [robot_state_publisher]: got segment panda_link5 [robot_state_publisher-2] [INFO] [1675419004.207017768] [robot_state_publisher]: got segment panda_link6 [robot_state_publisher-2] [INFO] [1675419004.207026967] [robot_state_publisher]: got segment panda_link7 [robot_state_publisher-2] [INFO] [1675419004.207036385] [robot_state_publisher]: got segment panda_link8 [robot_state_publisher-2] [INFO] [1675419004.207045645] [robot_state_publisher]: got segment panda_rightfinger [robot_state_publisher-2] [INFO] [1675419004.207055537] [robot_state_publisher]: got segment panda_table_link [ros2_control_node-4] [INFO] [1675419004.219853161] [FrankaHardwareInterface]: Connecting to robot at "172.16.1.22" ... [ros2_control_node-4] [INFO] [1675419004.221130550] [FrankaHardwareInterface]: Successfully connected to robot
[ros2_control_node-4] [INFO] [1675419004.231040806] [controller_manager]: update rate is 1000 Hz [move_group-3] [WARN] [1675419004.259931590] [move_group.move_group]: MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default planning pipeline configuration [move_group-3] [WARN] [1675419004.259978652] [move_group.move_group]: Falling back to using the the move_group node namespace (deprecated behavior). [move_group-3] Parsing robot urdf xml string. [move_group-3] [INFO] [1675419004.264849176] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00253798 seconds [move_group-3] [INFO] [1675419004.264884871] [moveit_robot_model.robot_model]: Loading robot model 'panda'... [move_group-3] [WARN] [1675419004.264991459] [moveit_robot_model.robot_model]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [move_group-3] [WARN] [1675419004.265001741] [moveit_robot_model.robot_model]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [move_group-3] Link panda_link0 had 1 children [move_group-3] Link panda_link1 had 1 children [move_group-3] Link panda_link2 had 1 children [move_group-3] Link panda_link3 had 1 children [move_group-3] Link panda_link4 had 1 children [move_group-3] Link panda_link5 had 1 children [move_group-3] Link panda_link6 had 1 children [move_group-3] Link panda_link7 had 1 children [move_group-3] Link panda_link8 had 1 children [move_group-3] Link panda_hand had 3 children [move_group-3] Link panda_leftfinger had 0 children [move_group-3] Link panda_rightfinger had 0 children [move_group-3] Link panda_hand_tcp had 0 children [move_group-3] [INFO] [1675419004.279157109] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' [move_group-3] [INFO] [1675419004.279686837] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states [move_group-3] [INFO] [1675419004.280174600] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states' [move_group-3] [INFO] [1675419004.280810808] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects [move_group-3] [INFO] [1675419004.280828779] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor [move_group-3] [INFO] [1675419004.281094433] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene' [move_group-3] [INFO] [1675419004.281107126] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. [move_group-3] [INFO] [1675419004.281435202] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object' [move_group-3] [INFO] [1675419004.281718383] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry [move_group-3] [WARN] [1675419004.281751017] [moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead [move_group-3] [ERROR] [1675419004.281779418] [moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates [move_group-3] [INFO] [1675419004.282650961] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'move_group' [move_group-3] [INFO] [1675419004.302923011] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL' [move_group-3] [INFO] [1675419004.306300556] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.path_tolerance' was not set. Using default value: 0.100000 [move_group-3] [INFO] [1675419004.306323127] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.resample_dt' was not set. Using default value: 0.100000 [move_group-3] [INFO] [1675419004.306330188] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.min_angle_change' was not set. Using default value: 0.001000 [move_group-3] [INFO] [1675419004.306369961] [moveit_ros.fix_workspace_bounds]: Param 'move_group.default_workspace_bounds' was not set. Using default value: 10.000000 [move_group-3] [INFO] [1675419004.306397597] [moveit_ros.fix_start_state_bounds]: Param 'move_group.start_state_max_bounds_error' was set to 0.050000 [move_group-3] [INFO] [1675419004.306406704] [moveit_ros.fix_start_state_bounds]: Param 'move_group.start_state_max_dt' was not set. Using default value: 0.500000 [move_group-3] [INFO] [1675419004.306427389] [moveit_ros.fix_start_state_collision]: Param 'move_group.start_state_max_dt' was not set. Using default value: 0.500000 [move_group-3] [INFO] [1675419004.306435406] [moveit_ros.fix_start_state_collision]: Param 'move_group.jiggle_fraction' was not set. Using default value: 0.020000 [move_group-3] [INFO] [1675419004.306450990] [moveit_ros.fix_start_state_collision]: Param 'move_group.max_sampling_attempts' was not set. Using default value: 100 [move_group-3] [INFO] [1675419004.306472482] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization' [move_group-3] [INFO] [1675419004.306519999] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links' [move_group-3] [INFO] [1675419004.306528189] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds' [move_group-3] [INFO] [1675419004.306534163] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds' [move_group-3] [INFO] [1675419004.306539943] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision' [move_group-3] [INFO] [1675419004.306545828] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints' [ros2 run controller_manager spawner.py panda_arm_controller-7] [INFO] [1675419004.627826860] [spawner_panda_arm_controller]: Waiting for /controller_manager services [ros2 run controller_manager spawner.py joint_state_broadcaster-8] [INFO] [1675419004.658067986] [spawner_joint_state_broadcaster]: Waiting for /controller_manager services [joint_state_publisher-5] [INFO] [1675419004.661600739] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic... [ros2_control_node-4] [INFO] [1675419004.833877638] [controller_manager]: Loading controller 'panda_arm_controller' [ros2 run controller_manager spawner.py panda_arm_controller-7] [INFO] [1675419004.843504311] [spawner_panda_arm_controller]: Loaded panda_arm_controller [ros2_control_node-4] [INFO] [1675419004.850177252] [controller_manager]: Configuring controller 'panda_arm_controller' [ros2_control_node-4] [INFO] [1675419004.850761103] [panda_arm_controller]: Command interfaces are [effort] and and state interfaces are [position velocity]. [ros2_control_node-4] [INFO] [1675419004.851711442] [panda_arm_controller]: Controller state will be published at 50.00 Hz. [ros2_control_node-4] [INFO] [1675419004.852404412] [panda_arm_controller]: Action status changes will be monitored at 20.00 Hz. [move_group-3] [INFO] [1675419004.854752552] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for panda_arm_controller [move_group-3] [INFO] [1675419004.854835725] [moveit.plugins.moveit_simple_controller_manager]: Max effort set to 0.0 [move_group-3] [INFO] [1675419004.857165807] [moveit.plugins.moveit_simple_controller_manager]: Added GripperCommand controller for panda_gripper [move_group-3] [INFO] [1675419004.857294080] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-3] [INFO] [1675419004.857672915] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers [move_group-3] [INFO] [1675419004.857688853] [move_group.move_group]: MoveGroup debug mode is ON [ros2 run controller_manager spawner.py panda_arm_controller-7] [INFO] [1675419004.859272546] [spawner_panda_arm_controller]: Configured and started panda_arm_controller [ros2_control_node-4] [INFO] [1675419004.866736039] [controller_manager]: Loading controller 'joint_state_broadcaster' [ros2 run controller_manager spawner.py joint_state_broadcaster-8] [INFO] [1675419004.876576208] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster [move_group-3] [INFO] [1675419004.879070080] [move_group.move_group]: [move_group-3] [move_group-3] **** [move_group-3] MoveGroup using: [move_group-3] - ApplyPlanningSceneService [move_group-3] - ClearOctomapService [move_group-3] - CartesianPathService [move_group-3] - ExecuteTrajectoryAction [move_group-3] - GetPlanningSceneService [move_group-3] - KinematicsService [move_group-3] - MoveAction [move_group-3] - MotionPlanService [move_group-3] - QueryPlannersService [move_group-3] * - StateValidationService [move_group-3] **** [move_group-3] [move_group-3] [INFO] [1675419004.879115799] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner [move_group-3] [INFO] [1675419004.879131668] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete [move_group-3] Loading 'move_group/ApplyPlanningSceneService'... [move_group-3] Loading 'move_group/ClearOctomapService'... [move_group-3] Loading 'move_group/MoveGroupCartesianPathService'... [move_group-3] Loading 'move_group/MoveGroupExecuteTrajectoryAction'... [move_group-3] Loading 'move_group/MoveGroupGetPlanningSceneService'... [move_group-3] Loading 'move_group/MoveGroupKinematicsService'... [move_group-3] Loading 'move_group/MoveGroupMoveAction'... [move_group-3] Loading 'move_group/MoveGroupPlanService'... [move_group-3] Loading 'move_group/MoveGroupQueryPlannersService'... [move_group-3] Loading 'move_group/MoveGroupStateValidationService'... [move_group-3] [move_group-3] You can start planning now! [move_group-3] [rviz2-1] [INFO] [1675419004.899197301] [rviz2]: Stereo is NOT SUPPORTED [rviz2-1] [INFO] [1675419004.899367139] [rviz2]: OpenGl version: 4.6 (GLSL 4.6) [INFO] [ros2 run controller_manager spawner.py panda_arm_controller-7]: process has finished cleanly [pid 261836] [rviz2-1] [INFO] [1675419004.925408516] [rviz2]: Stereo is NOT SUPPORTED [ros2_control_node-4] [INFO] [1675419004.958353948] [controller_manager]: Configuring controller 'joint_state_broadcaster' [ros2 run controller_manager spawner.py joint_state_broadcaster-8] [INFO] [1675419004.963613340] [spawner_joint_state_broadcaster]: Configured and started joint_state_broadcaster [INFO] [ros2 run controller_manager spawner.py joint_state_broadcaster-8]: process has finished cleanly [pid 261838] [rviz2-1] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open. [rviz2-1] at line 253 in /opt/ros/foxy/include/class_loader/class_loader_core.hpp [rviz2-1] Parsing robot urdf xml string. [rviz2-1] [INFO] [1675419005.047024419] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00134235 seconds [rviz2-1] [INFO] [1675419005.047112127] [moveit_robot_model.robot_model]: Loading robot model 'panda'... [rviz2-1] [WARN] [1675419005.047299808] [moveit_robot_model.robot_model]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [rviz2-1] [WARN] [1675419005.047313272] [moveit_robot_model.robot_model]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [rviz2-1] [ERROR] [1675419008.726199542] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available [rviz2-1] Parsing robot urdf xml string. [rviz2-1] [INFO] [1675419008.829768378] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00140166 seconds [rviz2-1] [INFO] [1675419008.829819146] [moveit_robot_model.robot_model]: Loading robot model 'panda'... [rviz2-1] [WARN] [1675419008.829913203] [moveit_robot_model.robot_model]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [rviz2-1] [WARN] [1675419008.829927946] [moveit_robot_model.robot_model]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [rviz2-1] Link panda_link0 had 1 children [rviz2-1] Link panda_link1 had 1 children [rviz2-1] Link panda_link2 had 1 children [rviz2-1] Link panda_link3 had 1 children [rviz2-1] Link panda_link4 had 1 children [rviz2-1] Link panda_link5 had 1 children [rviz2-1] Link panda_link6 had 1 children [rviz2-1] Link panda_link7 had 1 children [rviz2-1] Link panda_link8 had 1 children [rviz2-1] Link panda_hand had 3 children [rviz2-1] Link panda_leftfinger had 0 children [rviz2-1] Link panda_rightfinger had 0 children [rviz2-1] Link panda_hand_tcp had 0 children [rviz2-1] [INFO] [1675419008.875369626] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor [rviz2-1] [INFO] [1675419008.876056027] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene' [ros2_control_node-4] terminate called after throwing an instance of 'franka::ControlException' [ros2_control_node-4] what(): libfranka: Move command aborted: User Stop pressed! [ERROR] [ros2_control_node-4]: process has died [pid 261830, exit code -6, cmd '/opt/ros/foxy/lib/controller_manager/ros2_control_node --ros-args --params-file /tmp/launch_params_u3tunc1p --params-file /home/rpl/franka_ros2_ws/install/franka_moveit_config/share/franka_moveit_config/config/panda_ros_controllers.yaml -r joint_states:=franka/joint_states']. [INFO] [launch]: process[ros2_control_node-4] was required: shutting down launched system [INFO] [franka_gripper_node-6]: sending signal 'SIGINT' to process[franka_gripper_node-6] [INFO] [joint_state_publisher-5]: sending signal 'SIGINT' to process[joint_state_publisher-5] [INFO] [move_group-3]: sending signal 'SIGINT' to process[move_group-3] [INFO] [robot_state_publisher-2]: sending signal 'SIGINT' to process[robot_state_publisher-2] [INFO] [rviz2-1]: sending signal 'SIGINT' to process[rviz2-1]
[move_group-3] [INFO] [1675419011.090332322] [moveit.ros_planning_interface.moveit_cpp]: Deleting MoveItCpp [move_group-3] [INFO] [1675419011.091618592] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopped publishing maintained planning scene. [move_group-3] [INFO] [1675419011.092063162] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopping world geometry monitor [move_group-3] [INFO] [1675419011.092272626] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopping planning scene monitor [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 261826] [move_group-3] Warning: class_loader.ClassLoader: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded. [move_group-3] at line 127 in /tmp/binarydeb/ros-foxy-class-loader-2.0.3/src/class_loader.cpp [INFO] [move_group-3]: process has finished cleanly [pid 261828] [move_group-3] [INFO] [joint_state_publisher-5]: process has finished cleanly [pid 261832] [rviz2-1] [INFO] [1675419011.132836383] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopping planning scene monitor [INFO] [franka_gripper_node-6]: process has finished cleanly [pid 261834] [INFO] [rviz2-1]: process has finished cleanly [pid 261824]