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

…ian-pose-command-interface to humble

* commit 'f669c362cb21657e1834272cbbfd267ad4d3ca18':
  bump version
  feat: support cartesian pose command interface

CHANGELOG.md

@@ -1,5 +1,16 @@
 # Changelog
 
+## 0.1.9 - 2023-12-04
+
+Requires libfranka >= 0.13.0, required ROS 2 Humble
+
+* franka\_hardware: add state interfaces for initial position, cartesian pose and elbow.
+* franka\_hardware: support cartesian pose interface.
+* franka\_semantic\_component: support cartesian pose interface.
+* franka\_example\_controllers: add cartesian pose example controller
+* franka\_example\_controllers: add cartesian elbow controller
+* franka\_example\_controllers: add cartesian orientation controller
+
 ## 0.1.8 - 2023-11-16
 
 Requires libfranka >= 0.13.0, required ROS 2 Humble

franka_bringup/config/controllers.yaml

@@ -16,6 +16,15 @@ controller_manager:
 
     cartesian_velocity_example_controller:
       type: franka_example_controllers/CartesianVelocityExampleController
+
+    cartesian_pose_example_controller:
+      type: franka_example_controllers/CartesianPoseExampleController
+
+    cartesian_pose_elbow_example_controller:
+      type: franka_example_controllers/CartesianElbowExampleController
+
+    cartesian_orientation_example_controller:
+      type: franka_example_controllers/CartesianOrientationExampleController
 
     elbow_example_controller:
       type: franka_example_controllers/ElbowExampleController

franka_bringup/launch/cartesian_elbow_example_controller.launch.py

@@ -0,0 +1,77 @@
+#  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.
+
+
+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_pose_elbow_example_controller'],
+            output='screen',
+        ),
+    ])

franka_bringup/launch/cartesian_orientation_example_controller.launch.py

@@ -0,0 +1,77 @@
+#  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.
+
+
+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_orientation_example_controller'],
+            output='screen',
+        ),
+    ])

franka_bringup/launch/cartesian_pose_example_controller.launch.py

@@ -0,0 +1,77 @@
+#  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.
+
+
+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_pose_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.8</version>
+  <version>0.1.9</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.8</version>
+  <version>0.1.9</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

@@ -33,6 +33,9 @@ add_library(
         src/joint_velocity_example_controller.cpp
         src/joint_position_example_controller.cpp
         src/cartesian_velocity_example_controller.cpp
+        src/cartesian_pose_example_controller.cpp
+        src/cartesian_elbow_example_controller.cpp
+        src/cartesian_orientation_example_controller.cpp
         src/elbow_example_controller.cpp
         src/model_example_controller.cpp
         src/move_to_start_example_controller.cpp

franka_example_controllers/franka_example_controllers.xml

@@ -29,6 +29,24 @@
             The cartesian velocity example controller commands linear velocity with x and z components.
         </description>
     </class>
+    <class name="franka_example_controllers/CartesianPoseExampleController"
+           type="franka_example_controllers::CartesianPoseExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The cartesian pose example controller commands to translation on x and z components.
+        </description>
+    </class>
+    <class name="franka_example_controllers/CartesianElbowExampleController"
+           type="franka_example_controllers::CartesianElbowExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The cartesian pose example controller commands the elbow with the same initial pose configuration.
+        </description>
+    </class>
+    <class name="franka_example_controllers/CartesianOrientationExampleController"
+           type="franka_example_controllers::CartesianOrientationExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The cartesian orientation example controller commands to rotation around x axis.
+        </description>
+    </class>
     <class name="franka_example_controllers/ElbowExampleController"
            type="franka_example_controllers::ElbowExampleController" base_class_type="controller_interface::ControllerInterface">
         <description>

franka_example_controllers/include/franka_example_controllers/cartesian_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_pose_interface.hpp>
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * The elbow example controller through cartesian pose command interface
+ */
+class CartesianElbowExampleController : 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::FrankaCartesianPoseInterface> franka_cartesian_pose_;
+  const bool k_elbow_activated_{true};
+  std::vector<double> initial_cartesian_pose_and_elbow;
+  bool initialization_flag_{true};
+  std::array<double, 2> initial_elbow_configuration_{0.0, 0.0};
+  std::array<double, 16> initial_pose_configuration_{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  double elapsed_time_{0.0};
+  const double traj_frequency_{0.001};
+};
+
+}  // namespace franka_example_controllers