feat: joint impedance controller with moveit ik service

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 \

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

franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py

@@ -0,0 +1,97 @@
+#  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',
+        ),
+    ])
+

franka_example_controllers/CMakeLists.txt

@@ -24,6 +24,7 @@ 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)
 
 add_library(
         ${PROJECT_NAME}
@@ -34,6 +35,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 +56,7 @@ ament_target_dependencies(
         rclcpp
         rclcpp_lifecycle
         franka_semantic_components
+        moveit_core
 )
 
 generate_parameter_library(franka_example_controllers_parameters src/model_example_controller_parameters.yaml)
@@ -141,5 +144,6 @@ ament_export_dependencies(
         rclcpp
         rclcpp_lifecycle
         hardware_interface
+        moveit_core
 )
 ament_package()

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(bio-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">

franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp

@@ -0,0 +1,91 @@
+// 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.
+
+#pragma once
+
+#include <Eigen/Dense>
+#include <string>
+
+#include <controller_interface/controller_interface.hpp>
+#include <moveit_msgs/srv/get_position_ik.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include "franka_semantic_components/franka_cartesian_pose_interface.hpp"
+#include "franka_semantic_components/franka_robot_model.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * joint impedance example controller get desired pose and use moveit inverse kinematics
+ */
+class JointImpedanceWithIKExampleController : public controller_interface::ControllerInterface {
+ public:
+  using Vector7d = Eigen::Matrix<double, 7, 1>;
+  [[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:
+  void update_joint_states();
+  Eigen::Vector3d compute_new_position();
+  std::shared_ptr<moveit_msgs::srv::GetPositionIK::Request> create_ik_service_request(
+      Eigen::Vector3d new_position,
+      Eigen::Quaterniond new_orientation,
+      std::vector<double> joint_positions_desired,
+      std::vector<double> joint_positions_current,
+      std::vector<double> joint_efforts_current);
+
+  Vector7d compute_torque_command(Vector7d joint_positions_desired,
+                                  Vector7d joint_positions_current,
+                                  Vector7d joint_velocities_current);
+
+  bool assign_parameters();
+
+  std::unique_ptr<franka_semantic_components::FrankaCartesianPoseInterface> franka_cartesian_pose_;
+
+  Eigen::Quaterniond orientation_;
+  Eigen::Vector3d position_;
+  rclcpp::Client<moveit_msgs::srv::GetPositionIK>::SharedPtr compute_ik_client_;
+
+  double trajectory_period_{0.001};
+  const bool k_elbow_activated_{false};
+  bool initialization_flag_{true};
+
+  std::string arm_id_;
+
+  double elapsed_time_{0.0};
+  std::unique_ptr<franka_semantic_components::FrankaRobotModel> franka_robot_model_;
+
+  const std::string k_robot_state_interface_name{"robot_state"};
+  const std::string k_robot_model_interface_name{"robot_model"};
+
+  Vector7d dq_filtered_;
+  Vector7d k_gains_;
+  Vector7d d_gains_;
+  int num_joints_{7};
+
+  std::vector<double> joint_positions_desired;
+  std::vector<double> joint_positions_current{0, 0, 0, 0, 0, 0, 0};
+  std::vector<double> joint_velocities_current{0, 0, 0, 0, 0, 0, 0};
+  std::vector<double> joint_efforts_current{0, 0, 0, 0, 0, 0, 0};
+};
+}  // namespace franka_example_controllers