Port cartesian_impedance_controller to franka_ros2 example controllers

franka_bringup/config/controllers.yaml

@@ -47,6 +47,10 @@ controller_manager:
     franka_robot_state_broadcaster:
       type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster
 
+    cartesian_impedance_example_controller:
+      type: franka_example_controllers::CartesianImpedanceExampleController
+
+
 joint_impedance_with_ik_example_controller:
   ros__parameters:
     k_gains:

franka_bringup/launch/cartesian_impedance_example_controller.launch.py

@@ -0,0 +1,69 @@
+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'
+    arm_id_parameter_name = 'arm_id'
+    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)
+    arm_id = LaunchConfiguration(arm_id_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(
+            arm_id_parameter_name,
+            default_value='fr3',
+            description='ID of the type of arm used. Supported values: fer, fr3, fp3'),
+        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,
+                              arm_id_parameter_name: arm_id,
+                              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_impedance_example_controller'],
+            output='screen',
+        ),
+    ])

franka_example_controllers/CMakeLists.txt

@@ -36,6 +36,7 @@ add_library(
         src/cartesian_velocity_example_controller.cpp
         src/cartesian_pose_example_controller.cpp
         src/joint_impedance_with_ik_example_controller.cpp
+        src/cartesian_impedance_example_controller.cpp
         src/cartesian_elbow_example_controller.cpp
         src/cartesian_orientation_example_controller.cpp
         src/elbow_example_controller.cpp

franka_example_controllers/franka_example_controllers.xml

@@ -71,4 +71,10 @@
             The franka model example controller evaluates and prints the dynamic model ofFranka Robotics Robots.
         </description>
     </class>
+    <class name="franka_example_controllers/CartesianImpedanceExampleController" 
+           type="franka_example_controllers::CartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+        <description>
+        A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively.
+        </description>
+  </class>
 </library>

franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp

@@ -0,0 +1,91 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <fstream>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <ostream>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <rclcpp/time.hpp>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include <franka/robot_state.h>
+#include <controller_interface/controller_interface.hpp>
+#include <franka_msgs/msg/franka_robot_state.hpp>
+#include "franka_semantic_components/franka_robot_model.hpp"
+#include "franka_semantic_components/franka_robot_state.hpp"
+
+#include "franka_example_controllers/visibility_control.h"
+
+namespace franka_example_controllers {
+class CartesianImpedanceExampleController : public controller_interface::ControllerInterface {
+ public:
+  using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+  // Config methods
+
+  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
+      const override;
+  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
+      const override;
+
+  // ROS2 lifecycle related methods
+  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;
+
+  // Main real-time method
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period) override;
+
+ private:
+  std::unique_ptr<franka_semantic_components::FrankaRobotModel> franka_robot_model_;
+  std::unique_ptr<franka_semantic_components::FrankaRobotState> franka_robot_state_;
+
+  bool k_elbow_activated{true};
+
+  franka_msgs::msg::FrankaRobotState robot_state_;
+  franka_msgs::msg::FrankaRobotState init_robot_state_;
+
+  const std::string k_robot_state_interface_name{"robot_state"};
+  const std::string k_robot_model_interface_name{"robot_model"};
+
+  std::string arm_id_;
+  int num_joints{7};
+
+  // Saturation
+  Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+      const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+      const Eigen::Matrix<double, 7, 1>& tau_J_d);
+
+  // Classic cartesian controller
+  double filter_params_{0.005};
+  double nullspace_stiffness_{20.0};
+  double nullspace_stiffness_target_{20.0};
+  const double delta_tau_max_{1.0};
+
+  Eigen::Matrix<double, 6, 6> cartesian_stiffness_;
+  Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_;
+  Eigen::Matrix<double, 6, 6> cartesian_damping_;
+  Eigen::Matrix<double, 6, 6> cartesian_damping_target_;
+  Eigen::Matrix<double, 7, 1> q_d_nullspace_;
+  Eigen::Vector3d position_d_;
+  Eigen::Quaterniond orientation_d_;
+  std::mutex position_and_orientation_d_target_mutex_;
+  Eigen::Vector3d position_d_target_;
+  Eigen::Quaterniond orientation_d_target_;
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_equilibrium_pose_;
+
+  void equilibriumPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+};
+
+}  // namespace franka_example_controllers

franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <Eigen/Dense>
+
+namespace franka_example_controllers {
+
+inline Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& M_, bool damped = true) {
+  double lambda_ = damped ? 0.2 : 0.0;
+
+  Eigen::JacobiSVD<Eigen::MatrixXd> svd(M_, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType sing_vals_ = svd.singularValues();
+  Eigen::MatrixXd S_ = M_;  // copying the dimensions of M_, its content is not needed.
+  S_.setZero();
+
+  for (int i = 0; i < sing_vals_.size(); i++)
+    S_(i, i) = (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_);
+
+  return svd.matrixV() * S_.transpose() * svd.matrixU().transpose();
+}
+
+}  // namespace franka_example_controllers