frankaemika / franka_ros2

ROS 2 integration for Franka research robots
https://frankaemika.github.io/docs/franka_ros2.html
Apache License 2.0
103 stars 67 forks source link

Issue when implement Joint Impedance With IK Example #72

Closed SKYLEO98 closed 2 weeks ago

SKYLEO98 commented 3 weeks ago

Hello,

I tried to imply the Joint Impedance With IK Example for real Franka Research 3 robot but I am encountering this issue [ros2_control_node-3] [INFO] [1729703193.749591890] [joint_impedance_with_ik_example_controller]: service not available, waiting again...

The terminal is shown as below:

ros2 launch franka_bringup joint_impedance_with_ik_example_controller.launch.py arm_id:=fr3 robot_ip:=172.16.0.10 [INFO] [launch]: All log files can be found below /home/rmilnuc3/.ros/log/2024-10-23-19-06-25-547791-rmilnuc3-ZBOX-277537 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [joint_state_publisher-1]: process started with pid [277540] [INFO] [robot_state_publisher-2]: process started with pid [277542] [INFO] [ros2_control_node-3]: process started with pid [277544] [INFO] [spawner-4]: process started with pid [277546] [INFO] [spawner-5]: process started with pid [277548] [INFO] [franka_gripper_node-6]: process started with pid [277550] [INFO] [move_group-7]: process started with pid [277552] [INFO] [spawner-8]: process started with pid [277554] [franka_gripper_node-6] [INFO] [1729703186.778723846] [fr3_gripper]: Trying to establish a connection with the gripper [franka_gripper_node-6] [INFO] [1729703186.779958138] [fr3_gripper]: Connected to gripper [ros2_control_node-3] [WARN] [1729703186.829791221] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead. [ros2_control_node-3] [INFO] [1729703186.830633427] [resource_manager]: Loading hardware 'FrankaHardwareInterface' [ros2_control_node-3] [INFO] [1729703186.841814632] [resource_manager]: Initialize hardware 'FrankaHardwareInterface' [ros2_control_node-3] [INFO] [1729703186.841937473] [FrankaHardwareInterface]: Connecting to robot at "172.16.0.10" ... [move_group-7] [WARN] [1729703186.849633789] [move_group.move_group]: Falling back to using the the move_group node namespace (deprecated behavior). [ros2_control_node-3] [INFO] [1729703186.853352545] [FrankaHardwareInterface]: Successfully connected to robot [ros2_control_node-3] [INFO] [1729703186.867697952] [service_server]: Service started [robot_state_publisher-2] [INFO] [1729703186.888796528] [robot_state_publisher]: got segment base [robot_state_publisher-2] [INFO] [1729703186.889040531] [robot_state_publisher]: got segment fr3_hand [robot_state_publisher-2] [INFO] [1729703186.889060441] [robot_state_publisher]: got segment fr3_hand_tcp [robot_state_publisher-2] [INFO] [1729703186.889072187] [robot_state_publisher]: got segment fr3_leftfinger [robot_state_publisher-2] [INFO] [1729703186.889082846] [robot_state_publisher]: got segment fr3_link0 [robot_state_publisher-2] [INFO] [1729703186.889092928] [robot_state_publisher]: got segment fr3_link1 [robot_state_publisher-2] [INFO] [1729703186.889101952] [robot_state_publisher]: got segment fr3_link2 [robot_state_publisher-2] [INFO] [1729703186.889111403] [robot_state_publisher]: got segment fr3_link3 [robot_state_publisher-2] [INFO] [1729703186.889120358] [robot_state_publisher]: got segment fr3_link4 [robot_state_publisher-2] [INFO] [1729703186.889129916] [robot_state_publisher]: got segment fr3_link5 [robot_state_publisher-2] [INFO] [1729703186.889138716] [robot_state_publisher]: got segment fr3_link6 [robot_state_publisher-2] [INFO] [1729703186.889147483] [robot_state_publisher]: got segment fr3_link7 [robot_state_publisher-2] [INFO] [1729703186.889156228] [robot_state_publisher]: got segment fr3_link8 [robot_state_publisher-2] [INFO] [1729703186.889164983] [robot_state_publisher]: got segment fr3_rightfinger [move_group-7] [INFO] [1729703186.942539513] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.092479 seconds [move_group-7] [INFO] [1729703186.942619183] [moveit_robot_model.robot_model]: Loading robot model 'fr3'... [move_group-7] [WARN] [1729703186.942645040] [moveit_robot_model.robot_model]: Skipping virtual joint 'virtual_joint' because its child frame 'fr3_link0' does not match the URDF frame 'base' [move_group-7] [INFO] [1729703186.942654672] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint [ros2_control_node-3] [INFO] [1729703186.988204678] [action_server]: Action server started [ros2_control_node-3] [INFO] [1729703186.988371241] [resource_manager]: Successful initialization of hardware 'FrankaHardwareInterface' [ros2_control_node-3] [INFO] [1729703186.988814385] [resource_manager]: 'configure' hardware 'FrankaHardwareInterface'

[ros2_control_node-3] [INFO] [1729703186.988920544] [resource_manager]: 'activate' hardware 'FrankaHardwareInterface'

[ros2_control_node-3] [INFO] [1729703187.001965629] [controller_manager]: update rate is 1000 Hz [ros2_control_node-3] [INFO] [1729703187.002426737] [controller_manager]: Successful set up FIFO RT scheduling policy with priority 50. [move_group-7] [INFO] [1729703187.043487992] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states [move_group-7] [INFO] [1729703187.048476905] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states' [move_group-7] [INFO] [1729703187.061708278] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects [move_group-7] [INFO] [1729703187.064460416] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' [move_group-7] [INFO] [1729703187.064744108] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor [move_group-7] [INFO] [1729703187.065404911] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene' [move_group-7] [INFO] [1729703187.065436720] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. [move_group-7] [INFO] [1729703187.065942003] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object' [move_group-7] [INFO] [1729703187.066431144] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry [move_group-7] [WARN] [1729703187.068743947] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead [move_group-7] [ERROR] [1729703187.068784371] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates [move_group-7] [INFO] [1729703187.070936819] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'move_group' [move_group-7] [INFO] [1729703187.081552666] [moveit.ros_planning.planning_pipeline]: Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using 'chomp_interface/CHOMPPlanner' for now. [move_group-7] [INFO] [1729703187.089521176] [moveit.ros_planning.planning_pipeline]: Using planning interface 'CHOMP' [move_group-7] [FATAL] [1729703187.102907787] [moveit_ros.trajectory_execution_manager]: Parameter '~moveit_controller_manager' not specified. This is needed to identify the plugin to use for interacting with controllers. No paths can be executed. [move_group-7] [ERROR] [1729703187.102955054] [moveit_ros.trajectory_execution_manager]: Failed to reload controllers: controller_manager_ does not exist. [ERROR] [move_group-7]: process has died [pid 277552, exit code -11, cmd '/home/rmilnuc3/eurobin/ros2_ws/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_to1j5mmm --params-file /tmp/launch_params_2gs6ic5n --params-file /tmp/launch_params_01r_5p0g']. [joint_state_publisher-1] [INFO] [1729703187.693501487] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic... [ros2_control_node-3] [INFO] [1729703188.620083074] [controller_manager]: Loading controller 'joint_impedance_with_ik_example_controller' [ros2_control_node-3] [INFO] [1729703188.634552732] [controller_manager]: Loading controller 'joint_state_broadcaster' [spawner-8] [INFO] [1729703188.659548478] [spawner_joint_impedance_with_ik_example_controller]: Loaded joint_impedance_with_ik_example_controller [spawner-4] [INFO] [1729703188.677313104] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster [ros2_control_node-3] [INFO] [1729703188.678881783] [controller_manager]: Configuring controller 'joint_state_broadcaster' [ros2_control_node-3] [INFO] [1729703188.679114092] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published [spawner-4] [INFO] [1729703188.690481608] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster [ros2_control_node-3] [INFO] [1729703188.743596333] [controller_manager]: Configuring controller 'joint_impedance_with_ik_example_controller' [INFO] [spawner-4]: process has finished cleanly [pid 277546] [ros2_control_node-3] [INFO] [1729703189.748352760] [joint_impedance_with_ik_example_controller]: service not available, waiting again... [ros2_control_node-3] [INFO] [1729703190.748706983] [joint_impedance_with_ik_example_controller]: service not available, waiting again... [ros2_control_node-3] [INFO] [1729703191.749077884] [joint_impedance_with_ik_example_controller]: service not available, waiting again... [ros2_control_node-3] [INFO] [1729703192.749334135] [joint_impedance_with_ik_example_controller]: service not available, waiting again... [ros2_control_node-3] [INFO] [1729703193.749591890] [joint_impedance_with_ik_example_controller]: service not available, waiting again...

The issue seems to happen from moveit2. Do you have any solutions for this issue?

AndreasKuhner commented 2 weeks ago

Hi @SKYLEO98 , did you build the whole franka_ros2 workspace and source it? Especially the joint_trajectory_controller? It looks like that the trajectory executor from MoveIt 2 is crashing.

(It can be that you downloaded the joint_trajectory_controller from the ros2_control .deb which still has (or had) a bug, so you might need to uninstall that one.

SKYLEO98 commented 2 weeks ago

Hello @AndreasKuhner ,

I solved that issue by reinstalling the moveit2 binary. sudo apt install ros-humble-moveit

However, I am encountering another issue with the same mentioned example as

[ros2_control_node-3] [INFO] [1729871841.264508227] [joint_impedance_with_ik_example_controller]: Inverse kinematics solution failed.

The terminal output is shown as follows: ' $ ros2 launch franka_bringup joint_impedance_with_ik_example_controller.launch.py arm_id:=fr3 robot_ip:=172.16.0.11 [INFO] [launch]: All log files can be found below /home/jellyfish/.ros/log/2024-10-25-18-03-25-185587-jellyfish-164015 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [joint_state_publisher-1]: process started with pid [164018] [INFO] [robot_state_publisher-2]: process started with pid [164020] [INFO] [ros2_control_node-3]: process started with pid [164022] [INFO] [spawner-4]: process started with pid [164024] [INFO] [spawner-5]: process started with pid [164026] [INFO] [franka_gripper_node-6]: process started with pid [164028] [INFO] [move_group-7]: process started with pid [164030] [INFO] [spawner-8]: process started with pid [164032] [move_group-7] /opt/ros/humble/lib/moveit_ros_move_group/move_group: error while loading shared libraries: libsrdfdom.so.2.0.5: cannot open shared object file: No such file or directory [ERROR] [move_group-7]: process has died [pid 164030, exit code 127, cmd '/opt/ros/humble/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_huvwy5vl --params-file /tmp/launch_params_rh5iqtzg --params-file /tmp/launch_params_jy2uo296']. [ros2_control_node-3] [WARN] [1729872205.752716350] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead. [ros2_control_node-3] [INFO] [1729872205.753110411] [resource_manager]: Loading hardware 'FrankaHardwareInterface' [ros2_control_node-3] [INFO] [1729872205.758312875] [resource_manager]: Initialize hardware 'FrankaHardwareInterface' [ros2_control_node-3] [INFO] [1729872205.758393695] [FrankaHardwareInterface]: Connecting to robot at "172.16.0.11" ... [ros2_control_node-3] [INFO] [1729872205.764551392] [FrankaHardwareInterface]: Successfully connected to robot [franka_gripper_node-6] [INFO] [1729872205.766299294] [fr3_gripper]: Trying to establish a connection with the gripper [robot_state_publisher-2] [INFO] [1729872205.766387853] [robot_state_publisher]: got segment base [robot_state_publisher-2] [INFO] [1729872205.766457978] [robot_state_publisher]: got segment fr3_hand [robot_state_publisher-2] [INFO] [1729872205.766463034] [robot_state_publisher]: got segment fr3_hand_tcp [robot_state_publisher-2] [INFO] [1729872205.766465980] [robot_state_publisher]: got segment fr3_leftfinger [robot_state_publisher-2] [INFO] [1729872205.766468913] [robot_state_publisher]: got segment fr3_link0 [robot_state_publisher-2] [INFO] [1729872205.766471651] [robot_state_publisher]: got segment fr3_link1 [robot_state_publisher-2] [INFO] [1729872205.766474139] [robot_state_publisher]: got segment fr3_link2 [robot_state_publisher-2] [INFO] [1729872205.766476569] [robot_state_publisher]: got segment fr3_link3 [robot_state_publisher-2] [INFO] [1729872205.766478958] [robot_state_publisher]: got segment fr3_link4 [robot_state_publisher-2] [INFO] [1729872205.766481407] [robot_state_publisher]: got segment fr3_link5 [robot_state_publisher-2] [INFO] [1729872205.766483755] [robot_state_publisher]: got segment fr3_link6 [robot_state_publisher-2] [INFO] [1729872205.766486143] [robot_state_publisher]: got segment fr3_link7 [robot_state_publisher-2] [INFO] [1729872205.766488500] [robot_state_publisher]: got segment fr3_link8 [robot_state_publisher-2] [INFO] [1729872205.766490949] [robot_state_publisher]: got segment fr3_rightfinger [franka_gripper_node-6] [INFO] [1729872205.766927505] [fr3_gripper]: Connected to gripper [ros2_control_node-3] [INFO] [1729872205.776340020] [service_server]: Service started [ros2_control_node-3] [INFO] [1729872205.890595090] [action_server]: Action server started [ros2_control_node-3] [INFO] [1729872205.890660204] [resource_manager]: Successful initialization of hardware 'FrankaHardwareInterface' [ros2_control_node-3] [INFO] [1729872205.890809848] [resource_manager]: 'configure' hardware 'FrankaHardwareInterface'

[ros2_control_node-3] [INFO] [1729872205.890845395] [resource_manager]: 'activate' hardware 'FrankaHardwareInterface'

[ros2_control_node-3] [INFO] [1729872205.902249300] [controller_manager]: update rate is 1000 Hz [ros2_control_node-3] [WARN] [1729872205.902484817] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. [ros2_control_node-3] [INFO] [1729872205.950838878] [controller_manager]: Loading controller 'franka_robot_state_broadcaster' [ros2_control_node-3] [INFO] [1729872205.971389990] [controller_manager]: Loading controller 'joint_state_broadcaster' [spawner-5] [INFO] [1729872205.981161044] [spawner_franka_robot_state_broadcaster]: Loaded franka_robot_state_broadcaster [ros2_control_node-3] [INFO] [1729872205.995426622] [controller_manager]: Configuring controller 'franka_robot_state_broadcaster' [spawner-8] [INFO] [1729872206.003841255] [spawner_joint_impedance_with_ik_example_controller]: waiting for service /controller_manager/list_controllers to become available... [spawner-4] [INFO] [1729872206.010305355] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster [ros2_control_node-3] [INFO] [1729872206.063328878] [controller_manager]: Configuring controller 'joint_state_broadcaster' [ros2_control_node-3] [INFO] [1729872206.063384355] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published [spawner-5] [INFO] [1729872206.067963910] [spawner_franka_robot_state_broadcaster]: Configured and activated franka_robot_state_broadcaster [spawner-4] [INFO] [1729872206.070228796] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster [joint_state_publisher-1] [INFO] [1729872206.076247973] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic... [INFO] [spawner-5]: process has finished cleanly [pid 164026] [INFO] [spawner-4]: process has finished cleanly [pid 164024] [ros2_control_node-3] [INFO] [1729872206.255636218] [controller_manager]: Loading controller 'joint_impedance_with_ik_example_controller' [spawner-8] [INFO] [1729872206.274012255] [spawner_joint_impedance_with_ik_example_controller]: Loaded joint_impedance_with_ik_example_controller [ros2_control_node-3] [INFO] [1729872206.274628201] [controller_manager]: Configuring controller 'joint_impedance_with_ik_example_controller' [ros2_control_node-3] [INFO] [1729872206.279528536] [joint_impedance_with_ik_example_controller]: Default collision behavior set. [spawner-8] [INFO] [1729872206.285909202] [spawner_joint_impedance_with_ik_example_controller]: Configured and activated joint_impedance_with_ik_example_controller [ros2_control_node-3] [INFO] [1729872206.287014689] [joint_impedance_with_ik_example_controller]: Inverse kinematics solution failed. '

AndreasKuhner commented 2 weeks ago

[ros2_control_node-3] [WARN] [1729872205.902484817] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling.

This might be something you also want to look into. It is important that the program is able to set a high priority for its process to receive the necessary system resources (and also priority for execution).

Inverse kinematics solution failed

This mostly means that you want to go to a Cartesian pose which can't be reached by the robot - or which problem do you mean?

SKYLEO98 commented 2 weeks ago

Hello @AndreasKuhner ,

I figured out the issue from my real-time kernel. I deployed the example on real-time pc and it works well now. Thank you for your support.

AndreasKuhner commented 2 weeks ago

Good to hear :smile:

If everything is clear, you could close the issue. Otherwise, just go ahead and ask :rocket: