Frank ROS 2 support for FER and libfranka v0.9.2

[MadsRossen]

Is there any plan for support of the Franka Emika Robot for ROS 2 humble,and it's compatible libfranka v0.9.2 ?

[Tejas-Shastha]

I would also be interested in knowing this, as we have a few FER1s that we are very interested in actively using with ROS2 going forward. As of now the compatibility table shows that the FER1 does not receive the 5.2.0 firmware update -> which means it does not support libfranka 0.11.0 -> which means we cannot use the humble branch of this driver.

[drewskoots]

This https://github.com/frankaemika/franka_ros2/pull/10 (now closed) pull request contains a franka_ros2 version compatible with Panda. It's effectively just a port of the older foxy franka_ros2, so it may not enable all the functionality of the official humble port.

[Tejas-Shastha]

This #10 (now closed) pull request contains a franka_ros2 version compatible with Panda. It's effectively just a port of the older foxy franka_ros2, so it may not enable all the functionality of the official humble port.

@drewskoots Thanks for the tip! This helps the interim to have even a slightly outdated code base to work with existing hardware.

However, it would be very helpful for us to know what the long term support plan for the Panda (FER1) is w.r.t ROS2, even beyond humble.

[andrejpan]

This could be a bit difficult in the current situation. https://www.linkedin.com/posts/frankaemika_franka-emika-news-activity-7102968220122304513-AfYd?utm_source=share&utm_medium=member_desktop

[MadsRossen]

This #10 (now closed) pull request contains a franka_ros2 version compatible with Panda. It's effectively just a port of the older foxy franka_ros2, so it may not enable all the functionality of the official humble port.

@drewskoots Thanks for the tip! This helps the interim to have even a slightly outdated code base to work with existing hardware.

However, it would be very helpful for us to know what the long term support plan for the Panda (FER1) is w.r.t ROS2, even beyond humble.

Thanks for the Tip! I tried to run the different examples from the documentation using pull request #10 , the only one that does not work seems to be the following command:

ros2 launch franka_moveit_config moveit.launch.py robot_ip:=<fci-ip>

When the robot is moved to another position using the MotionPlanning, the robot does not hold it's position. I think it is a problem with the JointTrajectoryController.

[Tejas-Shastha]

This #10 (now closed) pull request contains a franka_ros2 version compatible with Panda. It's effectively just a port of the older foxy franka_ros2, so it may not enable all the functionality of the official humble port.

@drewskoots Thanks for the tip! This helps the interim to have even a slightly outdated code base to work with existing hardware. However, it would be very helpful for us to know what the long term support plan for the Panda (FER1) is w.r.t ROS2, even beyond humble.

Thanks for the Tip! I tried to run the different examples from the documentation using pull request #10 , the only one that does not work seems to be the following command:

ros2 launch franka_moveit_config moveit.launch.py robot_ip:=<fci-ip>

When the robot is moved to another position using the MotionPlanning, the robot does not hold it's position. I think it is a problem with the JointTrajectoryController.

Try combining libfranka 0.9.2 with this ros2 driver's version v0.1.0, this works for me on humble, to even run moveit with the robot.

[Tejas-Shastha]

@BarisYazici Would you be at liberty to comment about the long term support of the FER1 for ROS2 Humble/Iron/J ..?

[tenfoldpaper]

I've just finished developing the first version of panda_ros2, which is a port of franka_ros into ROS2 for the FER robots. It's been tested with libfranka 0.9.2 and Panda ver. 4.2.2. Most features for one arm scenario are working, though more thorough testing is necessary. Still, I think it's an improvement over what we have for the Panda robots in ROS2 at the moment. I would appreciate it if the community could try it out and let me know about bugs or if anything is missing!

[BarisYazici]

I've just finished developing the first version of panda_ros2, which is a port of franka_ros into ROS2 for the FER robots. It's been tested with libfranka 0.9.2 and Panda ver. 4.2.2. Most features for one arm scenario are working, though more thorough testing is necessary. Still, I think it's an improvement over what we have for the Panda robots in ROS2 at the moment. I would appreciate it if the community could try it out and let me know about bugs or if anything is missing!

Looks really good! Good job! The feature that causes incompatibility is the ActiveControl that introduced with https://github.com/frankaemika/libfranka/releases/tag/0.11.0 . ActiveControl ensures that libfranka control loop and ROS 2 hardware interface run in the same thread. You can potentially port the ActiveControl feature in Libfranka fr3-develop branch to FER branch, and you would be able to use the franka_ros2 with libfranka for FER that has external control feature.

[tenfoldpaper]

I've just finished developing the first version of panda_ros2, which is a port of franka_ros into ROS2 for the FER robots. It's been tested with libfranka 0.9.2 and Panda ver. 4.2.2. Most features for one arm scenario are working, though more thorough testing is necessary. Still, I think it's an improvement over what we have for the Panda robots in ROS2 at the moment. I would appreciate it if the community could try it out and let me know about bugs or if anything is missing!

Looks really good! Good job! The feature that causes incompatibility is the ActiveControl that introduced with https://github.com/frankaemika/libfranka/releases/tag/0.11.0 . ActiveControl ensures that libfranka control loop and ROS 2 hardware interface run in the same thread. You can potentially port the ActiveControl feature in Libfranka fr3-develop branch to FER branch, and you would be able to use the franka_ros2 with libfranka for FER that has external control feature.

Thanks! I will take a look at it, after implementing the multi-arm feature.

[yilmazabdurrah]

I've just finished developing the first version of panda_ros2, which is a port of franka_ros into ROS2 for the FER robots. It's been tested with libfranka 0.9.2 and Panda ver. 4.2.2. Most features for one arm scenario are working, though more thorough testing is necessary. Still, I think it's an improvement over what we have for the Panda robots in ROS2 at the moment. I would appreciate it if the community could try it out and let me know about bugs or if anything is missing!

Please note that it is also working with libfranka 0.8.0 and Panda FCI 4.0.1.

[tingelst]

Looks really good! Good job! The feature that causes incompatibility is the ActiveControl that introduced with https://github.com/frankaemika/libfranka/releases/tag/0.11.0 . ActiveControl ensures that libfranka control loop and ROS 2 hardware interface run in the same thread. You can potentially port the ActiveControl feature in Libfranka fr3-develop branch to FER branch, and you would be able to use the franka_ros2 with libfranka for FER that has external control feature.

For those interested, I have now been able to run the humble branch on an FER (v4.2.2) by modifying libfranka to employ v0.9.2 libfranka-common. You can find the updates at https://github.com/tingelst/libfranka, see the active-control-backport branch.

The effort and velocity controllers work, but the position controllers triggers a joint_motion_generator_acceleration_discontinuity.

[BarisYazici]

@tingelst Good one. FR3 has a better communication architecture that could be the reason you are receiving joint_motion_generator_acceleration_discontinuity errors.

The effort and velocity controllers work, but the position controllers triggers a joint_motion_generator_acceleration_discontinuity.

You could try to activate rate limiter for the joint position controller rate_limiter

[peterdavidfagan]

@tingelst should one be building from your latest branch? I am attempting to upgrade our libfranka version based on your fork but I am encountering build errors.

-- Configuring done
-- Generating done
-- Build files have been written to: /home/peter/Code/libfranka/build
[  1%] Building CXX object CMakeFiles/franka.dir/src/active_control.cpp.o
In file included from /home/peter/Code/libfranka/src/active_control.cpp:9:
/home/peter/Code/libfranka/src/robot_impl.h: In member function ‘std::enable_if_t<(! std::is_base_of<research_interface::robot::GetterSetterCommandBase<T, T::kCommand>, T>::value)> franka::Robot::Impl::handleCommandResponse(const typename T::Response&) const [with T = research_interface::robot::Move; std::enable_if_t<(! std::is_base_of<research_interface::robot::GetterSetterCommandBase<T, T::kCommand>, T>::value)> = void; typename T::Response = research_interface::robot::ResponseBase<research_interface::robot::Move>]’:
/home/peter/Code/libfranka/src/robot_impl.h:256:51: error: ‘kPreemptedDueToActivatedSafetyFunctions’ is not a member of ‘research_interface::robot::Move::Status’
  256 |     case research_interface::robot::Move::Status::kPreemptedDueToActivatedSafetyFunctions:
      |                                                   ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
gmake[2]: *** [CMakeFiles/franka.dir/build.make:76: CMakeFiles/franka.dir/src/active_control.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:160: CMakeFiles/franka.dir/all] Error 2
gmake: *** [Makefile:156: all] Error 2

I see the fr3-develop branch on common has this defined here. Are you building with @BarisYazici latest changes? If so I'll look to do the same, also happy to open a pr to update the submodule reference on your fork.

Ok yes looks like this was the issue, will open a pr. Thanks @tingelst for backporting the code, this is super helpful.

Update: changing to fr3-develop only fixed the build error but seems to introduce server compatibility issues.

[peterdavidfagan]

Ok pr is opened here in case others are also building on @tingelst's work. Thanks again @tingelst for backporting and @BarisYazici for supporting.

[peterdavidfagan]

I am in the process of testing the backport on my own repository. It seems when I build libfranka from source where I set the robot and gripper version to be compatible with my control server and robot, everything builds but I get the following output when running ros2 launch franka_bringup franka.launch.py robot_ip:=192.168.106.99 use_rviz:=true load_gripper:=false using the humble branch on the official franka_ros2 repository.

[INFO] [launch]: All log files can be found below /home/robot/.ros/log/2024-04-02-10-12-59-523989-r2d2-48611
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [48614]
[INFO] [joint_state_publisher-2]: process started with pid [48616]
[INFO] [ros2_control_node-3]: process started with pid [48618]
[INFO] [spawner-4]: process started with pid [48620]
[INFO] [spawner-5]: process started with pid [48622]
[INFO] [rviz2-6]: process started with pid [48624]
[robot_state_publisher-1] [INFO] [1712049179.798216960] [robot_state_publisher]: got segment panda_link0
[robot_state_publisher-1] [INFO] [1712049179.798274499] [robot_state_publisher]: got segment panda_link1
[robot_state_publisher-1] [INFO] [1712049179.798279552] [robot_state_publisher]: got segment panda_link2
[robot_state_publisher-1] [INFO] [1712049179.798282690] [robot_state_publisher]: got segment panda_link3
[robot_state_publisher-1] [INFO] [1712049179.798286068] [robot_state_publisher]: got segment panda_link4
[robot_state_publisher-1] [INFO] [1712049179.798289056] [robot_state_publisher]: got segment panda_link5
[robot_state_publisher-1] [INFO] [1712049179.798291685] [robot_state_publisher]: got segment panda_link6
[robot_state_publisher-1] [INFO] [1712049179.798294266] [robot_state_publisher]: got segment panda_link7
[robot_state_publisher-1] [INFO] [1712049179.798296810] [robot_state_publisher]: got segment panda_link8
[ros2_control_node-3] [WARN] [1712049179.803412685] [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] [1712049179.803569966] [resource_manager]: Loading hardware 'FrankaHardwareInterface' 
[ros2_control_node-3] [INFO] [1712049179.806129705] [resource_manager]: Initialize hardware 'FrankaHardwareInterface' 
[ros2_control_node-3] [INFO] [1712049179.806160578] [FrankaHardwareInterface]: Connecting to robot at "192.168.106.99" ...
[joint_state_publisher-2] [INFO] [1712049180.096138470] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...
[rviz2-6] [INFO] [1712049181.010636266] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-6] [INFO] [1712049181.010739051] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-6] [INFO] [1712049181.022895012] [rviz2]: Stereo is NOT SUPPORTED
[spawner-4] [INFO] [1712049182.077244600] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-5] [INFO] [1712049182.089841105] [spawner_franka_robot_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-4] [INFO] [1712049184.083718853] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-5] [INFO] [1712049184.096185431] [spawner_franka_robot_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-4] [INFO] [1712049186.090269266] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-5] [INFO] [1712049186.102365654] [spawner_franka_robot_state_broadcaster]: Waiting for '/controller_manager' services to be available
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[rviz2-6] [INFO] [1712049187.575781400] [rclcpp]: signal_handler(signum=2)
[robot_state_publisher-1] [INFO] [1712049187.575798469] [rclcpp]: signal_handler(signum=2)
[ros2_control_node-3] [INFO] [1712049187.575840505] [rclcpp]: signal_handler(signum=2)
[spawner-5] Traceback (most recent call last):
[spawner-5]   File "/opt/ros/humble/lib/controller_manager/spawner", line 33, in <module>
[spawner-5]     sys.exit(load_entry_point('controller-manager==2.40.0', 'console_scripts', 'spawner')())
[spawner-5]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 222, in main
[spawner-5]     if not wait_for_controller_manager(
[spawner-5]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 117, in wait_for_controller_manager
[spawner-5]     return wait_for_value_or(
[spawner-5]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 66, in wait_for_value_or
[spawner-5]     time.sleep(0.2)
[spawner-5] KeyboardInterrupt
[spawner-4] Traceback (most recent call last):
[spawner-4]   File "/opt/ros/humble/lib/controller_manager/spawner", line 33, in <module>
[spawner-4]     sys.exit(load_entry_point('controller-manager==2.40.0', 'console_scripts', 'spawner')())
[spawner-4]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 222, in main
[spawner-4]     if not wait_for_controller_manager(
[spawner-4]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 117, in wait_for_controller_manager
[spawner-4]     return wait_for_value_or(
[spawner-4]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 66, in wait_for_value_or
[spawner-4]     time.sleep(0.2)
[spawner-4] KeyboardInterrupt
[ERROR] [spawner-5]: process has died [pid 48622, exit code -2, cmd '/opt/ros/humble/lib/controller_manager/spawner franka_robot_state_broadcaster --ros-args'].
[ERROR] [spawner-4]: process has died [pid 48620, exit code -2, cmd '/opt/ros/humble/lib/controller_manager/spawner joint_state_broadcaster --ros-args'].
[INFO] [robot_state_publisher-1]: process has finished cleanly [pid 48614]
[INFO] [joint_state_publisher-2]: process has finished cleanly [pid 48616]
[INFO] [rviz2-6]: process has finished cleanly [pid 48624]
[ERROR] [ros2_control_node-3]: process[ros2_control_node-3] failed to terminate '5' seconds after receiving 'SIGINT', escalating to 'SIGTERM'
[INFO] [ros2_control_node-3]: sending signal 'SIGTERM' to process[ros2_control_node-3]
[ros2_control_node-3] [INFO] [1712049192.580984710] [rclcpp]: signal_handler(signum=15)
[ERROR] [ros2_control_node-3]: process[ros2_control_node-3] failed to terminate '10.0' seconds after receiving 'SIGTERM', escalating to 'SIGKILL'
[INFO] [ros2_control_node-3]: sending signal 'SIGKILL' to process[ros2_control_node-3]
[ERROR] [ros2_control_node-3]: process has died [pid 48618, exit code -9, cmd '/opt/ros/humble/lib/controller_manager/ros2_control_node --ros-args --params-file /tmp/launch_params_8wet03uw --params-file /home/robot/Code/test_franka/install/franka_bringup/share/franka_bringup/config/controllers.yaml -r joint_states:=franka/joint_states'].
[INFO] [launch]: process[ros2_control_node-3] was required: shutting down launched system

I am still debugging why this is occurring but @BarisYazici I believe this is due to incompatibility between the system version (4.2.2) and the active_control libfranka backport. I was hoping the active control backport for libfranka would work out of the box with the humble branch of franka_ros2. Is there something else I should check? I am building directly on @tingelst's changes, I didn't plan to go into detail on developing libfranka code but I am willing to if it is necessary to get the backport working, any guidance would be appreciated.

My system version is: 4.2.2 Libfranka: https://github.com/peterdavidfagan/libfranka/tree/235f9a18f9117f21c78b5ad31741b94e756e6eaf Libfranka Common: https://github.com/peterdavidfagan/libfranka-common/tree/952bcbffd6146b418676b2993776f668068be1d6

I manually regressed the robot version to 5 in libfranka-common. I am going to clean the commit history on my fork so it is more readable, any pointers are appreciated.

Note: Testing tagged versions of libfranka (0.9.0, 0.9.2) results in errors as expected:

[ros2_control_node-3] Stack trace (most recent call last):
[ros2_control_node-3] #0    Object "", at 0x7822aad6d010, in 
[ros2_control_node-3] Segmentation fault (Invalid permissions for mapped object [0x7822aad6d010])

I had the following Docker container working with a custom fork of franka_ros2 but as I plan to develop controllers etc. I wanted to ensure my software is up-to-date with franka_ros2 so I could align with development happening across the FER ecosystem. I am happy to contribute prs to help support research on FER.

[tingelst]

I checked on our Franka again today with the libfranka you can find here and installed by:

sudo dpkg -i libfranka-0.9.3-x86_64.deb

For franka_ros2 to compile you have to change the required libfranka version in franka_hardware, franka_gripper, and franka_semantic_components to 0.9.3 as can be seen in my fork on the fer branch: https://github.com/tingelst/franka_ros2/

I did not encounter any problems running:

ros2 launch franka_bringup franka.launch.py robot_ip:=192.168.106.99 use_rviz:=true load_gripper:=false

@peterdavidfagan: Feel free to reach out for a meeting if you encounter any problems getting it to work.

[peterdavidfagan]

Thanks @tingelst, this is much appreciated I will test the above on my repo. I'm also going to start investing more time in understanding the libfranka codebase so would be open to help maintain future backports based on changes made upstream (if/when possible).

[peterdavidfagan]

Latest version posted by @tingelst looks to work, thank you so much @tingelst. I'm going to update my repo to point towards your fork. @BarisYazici is it feasible for the open-source dev community to contribute to the official repos a backported version? I am happy to work on this based on @tingelst's work.

[peterdavidfagan]

Thanks for swift responses, I will add to my task list the item of opening a pr to the official repos for this backport. I'll be sure to tag you both in the pr and attribute commits appropriately in the pr.

Note: it may be next week before I can get started on this as I need to finish application level code for my repo for an internal deadline.

[BarisYazici]

I cannot guarantee a backport. But feel free to start a PR.

[Zarnack]

@tingelst Good one. FR3 has a better communication architecture that could be the reason you are receiving joint_motion_generator_acceleration_discontinuity errors.

The effort and velocity controllers work, but the position controllers triggers a joint_motion_generator_acceleration_discontinuity.

You could try to activate rate limiter for the joint position controller rate_limiter

Hi, I just tried the backport from @tingelst and the backport seems to be working successfully. Unfortunately I am only able to use the effort command-interface. Using the velocity interface I instantly get an "joint_motion_generator_acceleration_discontinuity" error at startup. Using the position interface I get the same error + "joint_motion_generator_velocity_discontinuity" as soon as I want to execute a movement.

Activating the rate limiter and the low pass filter does not change this behavior.

Am I am missing something? Why is this error even occurring when using the rate limiter?

I am using the FER without the franka gripper and disabled it in the webinterface.

[peterdavidfagan]

Hi @Zarnack,

I can also look to test the above on my setup at some point. So far I have only been using the effort interface.

[Zarnack]

Hi @peterdavidfagan, have you maybe had some time testing the above on your setup?

[peterdavidfagan]

Hi @Zarnack, I am still using the effort interface in my setup and its working for my use cases without issues. I plan to test new controllers soon, but again these all use the effort interface. I am keen to test the other interfaces but haven't done so yet.

[Zarnack]

Ok. Thanks for the quick response!