Ports the ROS1 Cartesian Impedance Controller [draft]

.devcontainer/docker-compose.yml

@@ -3,6 +3,7 @@ version: '2'
 services:
   ros2_control:
     build: .
+    container_name: franka_ros2
     network_mode: "host"
     privileged: true
     command: /bin/bash
@@ -22,4 +23,4 @@ services:
     ulimits:
       rtprio: 99
       rttime: -1
-      memlock: 8428281856
+      memlock: 8428281856

franka_example_controllers/CMakeLists.txt

@@ -39,6 +39,7 @@ add_library(
         src/joint_impedance_with_ik_example_controller.cpp
         src/cartesian_elbow_example_controller.cpp
         src/cartesian_orientation_example_controller.cpp
+        src/cartesian_impedance_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

@@ -71,4 +71,10 @@
             The franka model example controller evaluates and prints the dynamic model of Franka Emika Robots.
         </description>
     </class>
-</library>
+    <class name="franka_example_controllers/CartesianImpedanceExampleController"
+        type="franka_example_controllers::CartesianImpedanceExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            An experimental port of the ROS1 cartesian impedance controller
+        </description>
+    </class>
+</library>

franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp

@@ -0,0 +1,102 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <string>
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <franka_semantic_components/franka_cartesian_pose_interface.hpp>
+#include <franka_semantic_components/franka_robot_model.hpp>
+#include <franka_semantic_components/franka_robot_state.hpp>
+#include <franka_msgs/msg/franka_robot_state.hpp>
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * The Cartesian Impedance example controller
+*/
+class CartesianImpedanceExampleController : public controller_interface::ControllerInterface {
+public:
+
+    /*
+     * Interface:
+     * Takes in the equilibrium position of the spring-mass-damper system that
+     * the impedance controller implements, computes the torques and then sends
+     * the joint torques to the corresponding interface
+     *
+     * QUE: What is the Torque command interface?
+     * QUE: How do you retrieve the robot state?
+     * QUE: How do you retrieve the robot model?
+     */
+
+    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;
+
+    /* The only thing that needs to be implemented for ros2_control interface
+     */
+    controller_interface::return_type update(
+        const rclcpp::Time& time,
+        const rclcpp::Duration& period) override;
+
+    /* The following is all ROS2's lifecycle related stuff
+     */
+    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;
+
+    /* What Do I need for this controller
+     *
+    */
+
+
+private:
+    void UpdateJointStates();
+    Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+        const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+        const Eigen::Matrix<double, 7, 1>& tau_J_d);
+    std::string arm_id_;
+
+    // Interface for Panda model for dynamics and kinematics
+    std::unique_ptr<franka_semantic_components::FrankaRobotModel> franka_robot_model_;
+
+    // Interface for Desired equilibrium pose of the eef
+    std::unique_ptr<franka_semantic_components::FrankaCartesianPoseInterface> equilibrium_pose_d_;
+
+    // Interface for Robot state
+    std::unique_ptr<franka_semantic_components::FrankaRobotState> franka_robot_state_;
+
+    // message object that takes in robot state
+    // DEBG: delete after testing
+    // std::unique_ptr<franka_msgs::msg::FrankaRobotState> robot_state_;
+    franka_msgs::msg::FrankaRobotState robot_state_, init_robot_state_;
+
+    const std::string k_robot_state_interface_name{"robot_state"};
+    const std::string k_robot_model_interface_name{"robot_model"};
+
+    const double delta_tau_max_{1.0};
+    double nullspace_stiffness_{20.0};
+    double nullspace_stiffness_target_{20.0};
+    int num_joints{7};
+    bool k_elbow_activated{true};
+    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_;
+    Eigen::Vector3d position_d_target_;
+    Eigen::Quaterniond orientation_d_target_;
+
+};
+} // namespace franka_example_controllers

franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp

@@ -0,0 +1,30 @@
+// Author: Enrico Corvaglia
+// https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_controllers/include/utils/pseudo_inversion.h
+// File provided under public domain
+// pseudo_inverse() computes the pseudo inverse of matrix M_ using SVD decomposition (can choose
+// between damped and not)
+// returns the pseudo inverted matrix M_pinv_
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+#include <Eigen/SVD>
+
+namespace franka_example_controllers {
+
+inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, 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_);
+
+  M_pinv_ = Eigen::MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose());
+}
+
+}  // namespace franka_example_controllers