Merge pull request frankaemika#45 in SRR/franka_ros2 from feat/cartes…

…ian-velocity-command-interface to humble

* commit 'dc27872de32c3e51f7a5bcdcef66bfb300d78d7d':
  bump version
  remove joint trajectory controller
  feat: add elbow nullspace controller
  feat: use elbow activate flag variable
  feat: devcontainer with realtime support
  feat: cartesian velocity interface

.devcontainer/Dockerfile

@@ -43,11 +43,8 @@ RUN apt-get update && \
     ros-humble-joint-trajectory-controller \
     libpoco-dev \
     libeigen3-dev \
-    ament-cmake-clang-tidy \
     && rm -rf /var/lib/apt/lists/*
 
-ENV XDG_RUNTIME_DIR=/run/user/"${USER_UID}"
-
 RUN mkdir ~/source_code
 RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \
     && cd libfranka \
@@ -59,8 +56,14 @@ RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \
     && make franka -j$(nproc) \
     && cpack -G DEB \
     && sudo dpkg -i libfranka*.deb
-
+
+RUN addgroup realtime
+RUN usermod -a -G realtime $USERNAME
+
+ENV XDG_RUNTIME_DIR=/run/user/"${USER_UID}"
+
 # set the default user to the newly created user
 USER $USERNAME
 
-RUN echo 'source "/opt/ros/$ROS_DISTRO/setup.bash"' >>~/.bashrc
+RUN echo "source /ros_entrypoint.sh" >>~/.bashrc
+RUN echo "export ROS_DOMAIN_ID=100" >>~/.bashrc

.devcontainer/docker-compose.yml

@@ -12,7 +12,14 @@ services:
       - ../:/workspaces/src
       - /tmp/.X11-unix:/tmp/.X11-unix
       - $XAUTHORITY:$XAUTHORITY
+      - ./limits.conf:/etc/security/limits.conf
     environment:
       QT_X11_NO_MITSHM: 1
       DISPLAY: $DISPLAY
     user: $UID:$UID
+    cap_add:
+      - SYS_NICE
+    ulimits:
+      rtprio: 99
+      rttime: -1
+      memlock: 8428281856

.devcontainer/limits.conf

@@ -0,0 +1,6 @@
+@realtime soft rtprio 99
+@realtime soft priority 99
+@realtime soft memlock 102400
+@realtime hard rtprio 99
+@realtime hard priority 99
+@realtime hard memlock 102400

CHANGELOG.md

@@ -1,5 +1,14 @@
 # Changelog
 
+## 0.1.6 - 2023-11-03
+
+Requires libfranka >= 0.12.1, required ROS 2 Humble
+
+* franka\_hardware: support for cartesian velocity command interface
+* franka\_semantic\_component: implemented cartesian velocity interface
+* franka\_example\_controllers: implement cartesian velocity example controller
+* franka\_example\_controllers: implement elbow example controller
+
 ## 0.1.5 - 2023-10-13
 
 Requires libfranka >= 0.12.1, required ROS 2 Humble

franka_bringup/config/controllers.yaml

@@ -11,6 +11,12 @@ controller_manager:
     joint_velocity_example_controller:
       type: franka_example_controllers/JointVelocityExampleController
 
+    cartesian_velocity_example_controller:
+      type: franka_example_controllers/CartesianVelocityExampleController
+
+    elbow_example_controller:
+      type: franka_example_controllers/ElbowExampleController
+
     move_to_start_example_controller:
       type: franka_example_controllers/MoveToStartExampleController
 

franka_bringup/launch/cartesian_velocity_example_controller.launch.py

@@ -0,0 +1,77 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    load_gripper_parameter_name = 'load_gripper'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_rviz_parameter_name = 'use_rviz'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+    use_rviz = LaunchConfiguration(use_rviz_parameter_name)
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            robot_ip_parameter_name,
+            description='Hostname or IP address of the robot.'),
+        DeclareLaunchArgument(
+            use_rviz_parameter_name,
+            default_value='false',
+            description='Visualize the robot in Rviz'),
+        DeclareLaunchArgument(
+            use_fake_hardware_parameter_name,
+            default_value='false',
+            description='Use fake hardware'),
+        DeclareLaunchArgument(
+            fake_sensor_commands_parameter_name,
+            default_value='false',
+            description="Fake sensor commands. Only valid when '{}' is true".format(
+                use_fake_hardware_parameter_name)),
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
+                        'without an end-effector.'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution(
+                [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]),
+            launch_arguments={robot_ip_parameter_name: robot_ip,
+                              load_gripper_parameter_name: load_gripper,
+                              use_fake_hardware_parameter_name: use_fake_hardware,
+                              fake_sensor_commands_parameter_name: fake_sensor_commands,
+                              use_rviz_parameter_name: use_rviz
+                              }.items(),
+        ),
+
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['cartesian_velocity_example_controller'],
+            output='screen',
+        ),
+    ])

franka_bringup/launch/elbow_example_controller.launch.py

@@ -0,0 +1,77 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    load_gripper_parameter_name = 'load_gripper'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_rviz_parameter_name = 'use_rviz'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+    use_rviz = LaunchConfiguration(use_rviz_parameter_name)
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            robot_ip_parameter_name,
+            description='Hostname or IP address of the robot.'),
+        DeclareLaunchArgument(
+            use_rviz_parameter_name,
+            default_value='false',
+            description='Visualize the robot in Rviz'),
+        DeclareLaunchArgument(
+            use_fake_hardware_parameter_name,
+            default_value='false',
+            description='Use fake hardware'),
+        DeclareLaunchArgument(
+            fake_sensor_commands_parameter_name,
+            default_value='false',
+            description="Fake sensor commands. Only valid when '{}' is true".format(
+                use_fake_hardware_parameter_name)),
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
+                        'without an end-effector.'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution(
+                [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]),
+            launch_arguments={robot_ip_parameter_name: robot_ip,
+                              load_gripper_parameter_name: load_gripper,
+                              use_fake_hardware_parameter_name: use_fake_hardware,
+                              fake_sensor_commands_parameter_name: fake_sensor_commands,
+                              use_rviz_parameter_name: use_rviz
+                              }.items(),
+        ),
+
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['elbow_example_controller'],
+            output='screen',
+        ),
+    ])

franka_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_bringup</name>
-  <version>0.1.5</version>
+  <version>0.1.6</version>
   <description>Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control</description>
   <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_description/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_description</name>
-  <version>0.1.5</version>
+  <version>0.1.6</version>
   <description>franka_description contains URDF files and meshes of Franka Emika robots</description>
   <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

franka_example_controllers/CMakeLists.txt

@@ -31,6 +31,8 @@ add_library(
         src/gravity_compensation_example_controller.cpp
         src/joint_impedance_example_controller.cpp
         src/joint_velocity_example_controller.cpp
+        src/cartesian_velocity_example_controller.cpp
+        src/elbow_example_controller.cpp
         src/model_example_controller.cpp
         src/move_to_start_example_controller.cpp
         src/motion_generator.cpp)

franka_example_controllers/franka_example_controllers.xml

@@ -17,6 +17,18 @@
             The joint velocity example controller moves joint 4 and 5 in a periodic movement.
         </description>
     </class>
+    <class name="franka_example_controllers/CartesianVelocityExampleController"
+           type="franka_example_controllers::CartesianVelocityExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The cartesian velocity example controller commands linear velocity with x and z components.
+        </description>
+    </class>
+    <class name="franka_example_controllers/ElbowExampleController"
+           type="franka_example_controllers::ElbowExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The elbow example controller commands elbow position with zero cartesian velocity. Nullspace of the robot is actuated.
+        </description>
+    </class>
     <class name="franka_example_controllers/MoveToStartExampleController"
            type="franka_example_controllers::MoveToStartExampleController" base_class_type="controller_interface::ControllerInterface">
         <description>

franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.hpp

@@ -0,0 +1,56 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <string>
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <franka_semantic_components/franka_cartesian_velocity_interface.hpp>
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * The cartesian velocity example controller
+ */
+class CartesianVelocityExampleController : public controller_interface::ControllerInterface {
+ public:
+  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
+      const override;
+  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
+      const override;
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period) override;
+  CallbackReturn on_init() override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+ private:
+  std::unique_ptr<franka_semantic_components::FrankaCartesianVelocityInterface>
+      franka_cartesian_velocity_;
+
+  const double k_time_max_{4.0};
+  const double k_v_max_{0.05};
+  const double k_angle_{M_PI / 4.0};
+  const bool k_elbow_activated_{false};
+
+  rclcpp::Duration elapsed_time_ = rclcpp::Duration(0, 0);
+};
+
+}  // namespace franka_example_controllers

franka_example_controllers/include/franka_example_controllers/elbow_example_controller.hpp

@@ -0,0 +1,56 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <string>
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <franka_semantic_components/franka_cartesian_velocity_interface.hpp>
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * The elbow example controller
+ */
+class ElbowExampleController : public controller_interface::ControllerInterface {
+ public:
+  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
+      const override;
+  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
+      const override;
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period) override;
+  CallbackReturn on_init() override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+ private:
+  std::unique_ptr<franka_semantic_components::FrankaCartesianVelocityInterface>
+      franka_cartesian_velocity_;
+
+  const bool k_elbow_activated_{true};
+  std::vector<double> initial_cartesian_velocity_and_elbow;
+  bool first_pass_{true};
+  std::array<double, 2> initial_elbow_configuration_{0.0, 0.0};
+  double elapsed_time_{0.0};
+  const double traj_frequency_{0.001};
+};
+
+}  // namespace franka_example_controllers