Merge pull request #52 in SRR/franka_ros2 from feat/joint-impedance-w…

…ith-bio-ik to humble * commit '30347acfe6122b8dd6ffd55e0a316cde2e0e73a8': bump version 0.1.11 chore: switch to orocos-lma-ik feat: joint impedance controller with moveit ik service
frankaemika · Dec 20, 2023 · acf27d1 · acf27d1
2 parents 958eeb7 + 30347ac
commit acf27d1
Show file tree

Hide file tree

Showing 27 changed files with 686 additions and 34 deletions.
diff --git a/.clang-tidy b/.clang-tidy
@@ -1,5 +1,4 @@
-Checks: 'cppcoreguidelines-*,
-        -cppcoreguidelines-pro-type-vararg,
+Checks: '-cppcoreguidelines-pro-type-vararg,
         -cppcoreguidelines-pro-bounds-array-to-pointer-decay,
         clang-diagnostic-*,
         clang-analyzer-*,
@@ -15,9 +14,7 @@ Checks: 'cppcoreguidelines-*,
         -clang-diagnostic-reinterpret-base-class,
         -clang-diagnostic-return-type,
         -clang-diagnostic-switch,
-        -bugprone-lambda-function-name,
-        -cppcoreguidelines-avoid-magic-numbers,
-        -readability-magic-numbers'
+        -bugprone-lambda-function-name'
 HeaderFilterRegex: '^franka_.*'
 CheckOptions:
 # Classes, structs, ...

diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile
@@ -52,7 +52,7 @@ RUN apt-get update && \
 RUN mkdir ~/source_code
 RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \
     && cd libfranka \
-    && git switch fr3-develop \
+    && git switch 0.13.2 \
     && git submodule init \
     && git submodule update \
     && mkdir build && cd build \

diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,5 +1,12 @@
 # Changelog
 
+## 0.1.11 - 2023-12-20
+
+Requires libfranka >= 0.13.2, requires ROS 2 Humble
+
+* franka\_example\_controllers: Add a joint impedance example using OrocosKDL(LMA-ik) through MoveIt service.
+* franka\_hardware: Register initial joint positions and cartesian pose state interface without having running command interfaces.
+
 ## 0.1.10 - 2023-12-04
 
 Requires libfranka >= 0.13.0, required ROS 2 Humble

diff --git a/Dockerfile b/Dockerfile
@@ -40,6 +40,7 @@ RUN apt-get update -y && apt-get install -y --allow-unauthenticated \
     ros-humble-controller-interface \
     ros-humble-ros2-control-test-assets \
     ros-humble-controller-manager \
+    ros-humble-moveit \
     && rm -rf /var/lib/apt/lists/*
 
 RUN python3 -m pip install -U \
@@ -59,7 +60,7 @@ RUN python3 -m pip install -U \
 RUN mkdir ~/source_code    
 RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \
     && cd libfranka \
-    && git checkout 0.13.0 \
+    && git checkout 0.13.2 \
     && git submodule init \
     && git submodule update \
     && mkdir build && cd build \

diff --git a/NOTICE b/NOTICE
@@ -1,6 +1,6 @@
 franka_ros2
 
-Copyright 2021 Franka Emika GmbH
+Copyright 2023 Franka Robotics GmbH
 
 Licensed under the Apache License, Version 2.0 (the "License");
 you may not use this file except in compliance with the License.

diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml
@@ -23,6 +23,9 @@ controller_manager:
     cartesian_pose_elbow_example_controller:
       type: franka_example_controllers/CartesianElbowExampleController
 
+    joint_impedance_with_ik_example_controller:
+      type: franka_example_controllers/JointImpedanceWithIKExampleController
+
     cartesian_orientation_example_controller:
       type: franka_example_controllers/CartesianOrientationExampleController
 
@@ -44,6 +47,26 @@ controller_manager:
     franka_robot_state_broadcaster:
       type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster
 
+joint_impedance_with_ik_example_controller:
+  ros__parameters:
+    arm_id: panda
+    k_gains:
+      - 600.0
+      - 600.0
+      - 600.0
+      - 600.0
+      - 250.0
+      - 150.0
+      - 50.0
+    d_gains:
+      - 30.0
+      - 30.0
+      - 30.0
+      - 30.0
+      - 10.0
+      - 10.0
+      - 5.
+
 franka_robot_state_broadcaster:
   ros__parameters:
     arm_id: panda

diff --git a/franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py b/franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py
@@ -0,0 +1,96 @@
+#  Copyright (c) 2023 Franka Robotics 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(),
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [
+                    PathJoinSubstitution(
+                        [
+                            FindPackageShare('franka_moveit_config'),
+                            'launch',
+                            'move_group.launch.py',
+                        ]
+                    )
+                ]
+            ),
+            launch_arguments={
+                robot_ip: robot_ip,
+                load_gripper_parameter_name: load_gripper,
+                use_fake_hardware_parameter_name: 'true',
+                fake_sensor_commands_parameter_name: fake_sensor_commands,
+                use_rviz_parameter_name: use_rviz,
+            }.items(),
+        ),
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['joint_impedance_with_ik_example_controller'],
+            output='screen',
+        ),
+    ])
diff --git a/franka_bringup/package.xml b/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.9</version>
+  <version>0.1.11</version>
   <description>Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

diff --git a/franka_description/package.xml b/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.9</version>
+  <version>0.1.11</version>
   <description>franka_description contains URDF files and meshes of Franka Emika robots</description>
   <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt
@@ -24,6 +24,8 @@ find_package(franka_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(franka_semantic_components REQUIRED)
 find_package(generate_parameter_library)
+find_package(moveit_core REQUIRED)
+find_package(moveit_msgs REQUIRED)
 
 add_library(
         ${PROJECT_NAME}
@@ -34,6 +36,7 @@ add_library(
         src/joint_position_example_controller.cpp
         src/cartesian_velocity_example_controller.cpp
         src/cartesian_pose_example_controller.cpp
+        src/joint_impedance_with_ik_example_controller.cpp
         src/cartesian_elbow_example_controller.cpp
         src/cartesian_orientation_example_controller.cpp
         src/elbow_example_controller.cpp
@@ -54,6 +57,8 @@ ament_target_dependencies(
         rclcpp
         rclcpp_lifecycle
         franka_semantic_components
+        moveit_core
+        moveit_msgs
 )
 
 generate_parameter_library(franka_example_controllers_parameters src/model_example_controller_parameters.yaml)
@@ -141,5 +146,6 @@ ament_export_dependencies(
         rclcpp
         rclcpp_lifecycle
         hardware_interface
+        moveit_core
 )
 ament_package()
diff --git a/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml
@@ -34,6 +34,12 @@
         <description>
             The cartesian pose example controller commands to translation on x and z components.
         </description>
+    </class>
+        <class name="franka_example_controllers/JointImpedanceWithIKExampleController"
+           type="franka_example_controllers::JointImpedanceWithIKExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The joint impedance using inverse kinematics from moveit-OrocosKDL(LMA-ik) and commands torques based on joint impedance control law.
+        </description>
     </class>
     <class name="franka_example_controllers/CartesianElbowExampleController"
            type="franka_example_controllers::CartesianElbowExampleController" base_class_type="controller_interface::ControllerInterface">

diff --git a/...a_example_controllers/include/franka_example_controllers/default_robot_behavior_utils.hpp b/...a_example_controllers/include/franka_example_controllers/default_robot_behavior_utils.hpp
@@ -22,14 +22,22 @@ inline franka_msgs::srv::SetFullCollisionBehavior::Request::SharedPtr
 getDefaultCollisionBehaviorRequest() {
   auto request = std::make_shared<franka_msgs::srv::SetFullCollisionBehavior::Request>();
 
-  request->lower_torque_thresholds_nominal = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0};
-  request->upper_torque_thresholds_nominal = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0};
-  request->lower_torque_thresholds_acceleration = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0};
-  request->upper_torque_thresholds_acceleration = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0};
-  request->lower_force_thresholds_nominal = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0};
-  request->upper_force_thresholds_nominal = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0};
-  request->lower_force_thresholds_acceleration = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0};
-  request->upper_force_thresholds_acceleration = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0};
+  request->lower_torque_thresholds_nominal = {
+      25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0};  // NOLINT(cppcoreguidelines-avoid-magic-numbers)
+  request->upper_torque_thresholds_nominal = {
+      35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0};  // NOLINT(cppcoreguidelines-avoid-magic-numbers)
+  request->lower_torque_thresholds_acceleration = {
+      25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0};  // NOLINT(cppcoreguidelines-avoid-magic-numbers)
+  request->upper_torque_thresholds_acceleration = {
+      35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0};  // NOLINT(cppcoreguidelines-avoid-magic-numbers)
+  request->lower_force_thresholds_nominal = {
+      30.0, 30.0, 30.0, 25.0, 25.0, 25.0};  // NOLINT(cppcoreguidelines-avoid-magic-numbers)
+  request->upper_force_thresholds_nominal = {
+      40.0, 40.0, 40.0, 35.0, 35.0, 35.0};  // NOLINT(cppcoreguidelines-avoid-magic-numbers)
+  request->lower_force_thresholds_acceleration = {
+      30.0, 30.0, 30.0, 25.0, 25.0, 25.0};  // NOLINT(cppcoreguidelines-avoid-magic-numbers)
+  request->upper_force_thresholds_acceleration = {
+      40.0, 40.0, 40.0, 35.0, 35.0, 35.0};  // NOLINT(cppcoreguidelines-avoid-magic-numbers)
 
   return request;
 }