diff --git a/.gitignore b/.gitignore
index b931091cf188bfe8175a6e2e0f7992f453d9ecad..914e43bf52d4a975464da295a54cab9d51b3f971 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,7 +1,11 @@
.vscode/
+build/
+assets/data/*.asv
+fr3_impedance_controller_real/build/
fr3_impedance_controller_sim/build/
fr3_ros1_mujoco_interface/build/
fr3_ps3_joy_publisher/build/
assets/data/matlab/*.asv
assets/data/*.csv
+
# assets/data/*.bag
diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9d4025de473be6d24009f9b1855d44952d2e959c
--- /dev/null
+++ b/common/CMakeLists.txt
@@ -0,0 +1,85 @@
+# @Kevin 01.2025
+# This builds common as a shared lib so it can be built as Nix package and
+# linked to the other binaries.
+
+################################################################################
+## Set minimum required version of cmake, project name and compile options
+################################################################################
+cmake_minimum_required(VERSION 3.10)
+project(common)
+
+# Set C++ standard
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED True)
+
+################################################################################
+## Find required CMake packages : pinocchio, casadi, catkin, ROS-related ...
+################################################################################
+find_package(pinocchio REQUIRED)
+find_package(casadi REQUIRED)
+find_package(Eigen3 REQUIRED NO_MODULE)
+
+################################################################################
+## Find source and header files to create executable binary file
+################################################################################
+file(GLOB_RECURSE SOURCES
+ controller/src/*.cpp
+ math/src/*.cpp
+ motion/src/*.cpp
+ robot/src/*.cpp
+ utilities/src/*.cpp
+)
+
+include_directories(
+ controller/include
+ math/include
+ motion/include
+ robot/include
+ utilities/include
+)
+
+################################################################################
+## Define executable, Add source files & Link libraries
+################################################################################
+add_library(common SHARED ${SOURCES})
+
+target_link_libraries(common
+ Eigen3::Eigen
+ pinocchio::pinocchio
+ casadi
+)
+
+target_include_directories(common
+ PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/controller/include>
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/math/include>
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/motion/include>
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/robot/include>
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/utilities/include>
+ )
+
+install(TARGETS common
+ EXPORT commonTargets
+ LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_LIBDIR})
+
+install(EXPORT commonTargets
+ FILE commonConfig.cmake
+ NAMESPACE common::
+ DESTINATION lib/cmake/common)
+
+install(DIRECTORY controller/include/
+ DESTINATION include)
+
+install(DIRECTORY math/include/
+ DESTINATION include)
+
+install(DIRECTORY motion/include/
+ DESTINATION include)
+
+install(DIRECTORY robot/include/
+ DESTINATION include)
+
+install(DIRECTORY utilities/include/
+ DESTINATION include)
+
diff --git a/common/default.nix b/common/default.nix
new file mode 100644
index 0000000000000000000000000000000000000000..3c6701c4d3386c613b65e6ea14cae6c5dc0a1690
--- /dev/null
+++ b/common/default.nix
@@ -0,0 +1,31 @@
+# This defines a function which returns a packge, intended to be evaluated by callPackage
+{ lib
+, stdenv
+, cmake
+, eigen
+, pinocchio
+, casadi
+}:
+
+stdenv.mkDerivation {
+ pname = "common";
+ version = "0.0.0";
+ src = ./.;
+
+ # These are the build-time dependencies
+ nativeBuildInputs = [
+ cmake
+ ];
+
+ # These dependencies are propagated, any package (or shell) which
+ # includes these will also include these deps
+ propagatedBuildInputs = [
+ eigen
+ casadi
+ pinocchio
+ ];
+
+ meta = {
+ description = "Common files for the observer and impedance controller";
+ };
+}
diff --git a/flake.nix b/flake.nix
index 54a0d8e6973d5e99e7c383414cea99c5956a8f6e..4ffcdd98fd4196dda84210f113b41c6ebd240e58 100644
--- a/flake.nix
+++ b/flake.nix
@@ -4,7 +4,6 @@
inputs = {
nixpkgs_2411.url = "github:nixos/nixpkgs/nixos-24.11";
nixpkgs.follows = "noetic/nixpkgs";
- #pincas.url = "git+https://gitlab.cc-asp.fraunhofer.de/ipk_aut/bestpractices/nix.git?dir=pincas";
nix-ros-overlay.follows = "noetic/nix-ros-overlay";
noetic.url = "git+https://gitlab.cc-asp.fraunhofer.de/ipk_aut/bestpractices/nix.git?dir=ros1/noetic";
nixgl.url = "github:nix-community/nixGL";
@@ -16,7 +15,6 @@
overlays = [
inputs.nix-ros-overlay.overlays.default
- #inputs.pincas.overlays.default
inputs.nixgl.overlay
];
@@ -30,8 +28,6 @@
rospkgs = pkgs.rosPackages.noetic;
- fr3-custom-msgs = rospkgs.callPackage ./fr3_custom_msgs {};
-
rosPackages = with rospkgs; [
ros-core
catkin # for builds, probably not yet needed
@@ -58,16 +54,24 @@
urdf
];
+
+ common = pkgs_2411.callPackage ./common { };
+ fr3-impedance-controller = rospkgs.callPackage ./fr3_impedance_controller_real {
+ inherit common;
+ };
+ fr3-custom-msgs = rospkgs.callPackage ./fr3_custom_msgs {};
+
+ projectPackages = [
+ common
+ fr3-impedance-controller
+ fr3-custom-msgs
+ ];
+
simPackages = [
pkgs.mujoco
pkgs.glfw
pkgs.nixgl.nixGLIntel
];
-
- customPackages = [
- fr3-custom-msgs
- ];
-
in
{
devShells.${system} = {
@@ -75,7 +79,7 @@
default = pkgs.mkShell {
packages = [
(rospkgs.buildEnv {
- paths = rosPackages ++ [
+ paths = rosPackages ++ projectPackages ++ [
#pkgs.python3Packages.pincas
pkgs_2411.casadi
pkgs_2411.pinocchio
@@ -87,7 +91,7 @@
sim = pkgs.mkShell {
packages = [
(rospkgs.buildEnv {
- paths = rosPackages ++ simPackages ++ customPackages ++ [
+ paths = rosPackages ++ simPackages ++ projectPackages ++ [
#pkgs.python3Packages.pincas
pkgs_2411.casadi
pkgs_2411.pinocchio
diff --git a/fr3_impedance_controller_real/CMakeLists.txt b/fr3_impedance_controller_real/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..34e464adad3724270962fbf824cb520014d94e51
--- /dev/null
+++ b/fr3_impedance_controller_real/CMakeLists.txt
@@ -0,0 +1,144 @@
+cmake_minimum_required(VERSION 3.4)
+project(fr3_impedance_controller_real)
+
+set(CMAKE_BUILD_TYPE Release)
+# set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(catkin REQUIRED COMPONENTS
+ controller_interface
+ dynamic_reconfigure
+ eigen_conversions
+ franka_hw
+ franka_gripper
+ franka_example_controllers
+ geometry_msgs
+ hardware_interface
+ joint_limits_interface
+ tf
+ tf_conversions
+ message_generation
+ pluginlib
+ realtime_tools
+ roscpp
+ rospy
+ urdf
+ visualization_msgs
+ sensor_msgs
+)
+
+find_package(Eigen3 REQUIRED)
+find_package(Franka 0.9.0 QUIET)
+if(NOT Franka_FOUND)
+ find_package(Franka 0.8.0 REQUIRED)
+endif()
+
+#add_message_files(FILES
+# JointTorqueComparison.msg
+#)
+
+generate_messages()
+
+generate_dynamic_reconfigure_options(
+ cfg/compliance_param.cfg
+)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES franka_example_controllers
+ CATKIN_DEPENDS
+ controller_interface
+ dynamic_reconfigure
+ eigen_conversions
+ franka_hw
+ franka_gripper
+ franka_example_controllers
+ geometry_msgs
+ hardware_interface
+ joint_limits_interface
+ tf
+ tf_conversions
+ message_runtime
+ pluginlib
+ realtime_tools
+ roscpp
+ urdf
+ visualization_msgs
+ sensor_msgs
+ DEPENDS Franka
+)
+
+add_library(${PROJECT_NAME}
+ src/fr3_impedance_controller_real.cpp
+)
+
+add_dependencies(${PROJECT_NAME}
+ ${${PROJECT_NAME}_EXPORTED_TARGETS}
+ ${catkin_EXPORTED_TARGETS}
+ ${PROJECT_NAME}_generate_messages_cpp
+ ${PROJECT_NAME}_gencpp
+ ${PROJECT_NAME}_gencfg
+)
+
+target_link_libraries(${PROJECT_NAME} PUBLIC
+ ${Franka_LIBRARIES}
+ ${catkin_LIBRARIES}
+)
+
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
+ ${Franka_INCLUDE_DIRS}
+ ${EIGEN3_INCLUDE_DIRS}
+ ${catkin_INCLUDE_DIRS}
+)
+target_include_directories(${PROJECT_NAME} PUBLIC
+ include
+ ${CMAKE_CURRENT_SOURCE_DIR}/../common/utilities/include
+ ${CMAKE_CURRENT_SOURCE_DIR}/../common/math/include
+)
+
+## Installation
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+install(DIRECTORY launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+install(DIRECTORY config
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+install(FILES fr3_impedance_controller_real_plugin.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+## Tools
+include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
+ RESULT_VARIABLE CLANG_TOOLS
+)
+if(CLANG_TOOLS)
+ file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
+ file(GLOB_RECURSE HEADERS
+ ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/../common/utilities/include/*.hpp
+ ${CMAKE_CURRENT_SOURCE_DIR}/../common/math/include/*.hpp
+ )
+ add_format_target(franka_example_controllers FILES ${SOURCES} ${HEADERS})
+ add_tidy_target(franka_example_controllers
+ FILES ${SOURCES}
+ DEPENDS franka_example_controllers
+ )
+endif()
+
+include(${CMAKE_CURRENT_LIST_DIR}/../cmake/PepTools.cmake OPTIONAL
+ RESULT_VARIABLE PEP_TOOLS
+)
+if(PEP_TOOLS)
+ file(GLOB_RECURSE PYSOURCES ${CMAKE_CURRENT_SOURCE_DIR}/scripts/*.py)
+ add_pyformat_target(franka_control FILES ${PYSOURCES})
+endif()
diff --git a/README.md b/fr3_impedance_controller_real/README.md
similarity index 100%
rename from README.md
rename to fr3_impedance_controller_real/README.md
diff --git a/fr3_impedance_controller_real/cfg/compliance_param.cfg b/fr3_impedance_controller_real/cfg/compliance_param.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..6d5bd8402f3736325f9001e9e00ffaad7b0679df
--- /dev/null
+++ b/fr3_impedance_controller_real/cfg/compliance_param.cfg
@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("translational_stiffness_x", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("translational_stiffness_y", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("translational_stiffness_z", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 10, 0, 30)
+gen.add("nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0.5, 0, 100)
+
+exit(gen.generate(PACKAGE, "dynamic_compliance", "compliance_param"))
+
diff --git a/fr3_impedance_controller_real/config/franka_example_controllers.yaml b/fr3_impedance_controller_real/config/franka_example_controllers.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4cb8fa5665b64f40f8a8628dcae2b52fe9b3b169
--- /dev/null
+++ b/fr3_impedance_controller_real/config/franka_example_controllers.yaml
@@ -0,0 +1,179 @@
+joint_velocity_example_controller:
+ type: franka_example_controllers/JointVelocityExampleController
+ arm_id: $(arg arm_id)
+ joint_names:
+ - $(arg arm_id)_joint1
+ - $(arg arm_id)_joint2
+ - $(arg arm_id)_joint3
+ - $(arg arm_id)_joint4
+ - $(arg arm_id)_joint5
+ - $(arg arm_id)_joint6
+ - $(arg arm_id)_joint7
+
+joint_position_example_controller:
+ type: franka_example_controllers/JointPositionExampleController
+ joint_names:
+ - $(arg arm_id)_joint1
+ - $(arg arm_id)_joint2
+ - $(arg arm_id)_joint3
+ - $(arg arm_id)_joint4
+ - $(arg arm_id)_joint5
+ - $(arg arm_id)_joint6
+ - $(arg arm_id)_joint7
+
+cartesian_sinpose_controller:
+ type: franka_example_controllers/CartesianSinPoseExampleController
+ arm_id: $(arg arm_id)
+
+cartesian_pose_example_controller:
+ type: franka_example_controllers/CartesianPoseExampleController
+ arm_id: $(arg arm_id)
+
+elbow_example_controller:
+ type: franka_example_controllers/ElbowExampleController
+ arm_id: $(arg arm_id)
+
+cartesian_velocity_example_controller:
+ type: franka_example_controllers/CartesianVelocityExampleController
+ arm_id: $(arg arm_id)
+
+model_example_controller:
+ type: franka_example_controllers/ModelExampleController
+ arm_id: $(arg arm_id)
+
+force_example_controller:
+ type: franka_example_controllers/ForceExampleController
+ arm_id: $(arg arm_id)
+ joint_names:
+ - $(arg arm_id)_joint1
+ - $(arg arm_id)_joint2
+ - $(arg arm_id)_joint3
+ - $(arg arm_id)_joint4
+ - $(arg arm_id)_joint5
+ - $(arg arm_id)_joint6
+ - $(arg arm_id)_joint7
+
+joint_impedance_example_controller:
+ type: franka_example_controllers/JointImpedanceExampleController
+ arm_id: $(arg arm_id)
+ joint_names:
+ - $(arg arm_id)_joint1
+ - $(arg arm_id)_joint2
+ - $(arg arm_id)_joint3
+ - $(arg arm_id)_joint4
+ - $(arg arm_id)_joint5
+ - $(arg arm_id)_joint6
+ - $(arg arm_id)_joint7
+ k_gains:
+ - 600.0
+ - 600.0
+ - 600.0
+ - 600.0
+ - 250.0
+ - 150.0
+ - 50.0
+ d_gains:
+ - 50.0
+ - 50.0
+ - 50.0
+ - 20.0
+ - 20.0
+ - 20.0
+ - 10.0
+ radius: 0.1
+ acceleration_time: 2.0
+ vel_max: 0.15
+ publish_rate: 10.0
+ coriolis_factor: 1.0
+
+cartesian_impedance_example_controller:
+ type: franka_example_controllers/CartesianImpedanceExampleController
+ arm_id: $(arg arm_id)
+ joint_names:
+ - $(arg arm_id)_joint1
+ - $(arg arm_id)_joint2
+ - $(arg arm_id)_joint3
+ - $(arg arm_id)_joint4
+ - $(arg arm_id)_joint5
+ - $(arg arm_id)_joint6
+ - $(arg arm_id)_joint7
+
+dual_arm_cartesian_impedance_example_controller:
+ type: franka_example_controllers/DualArmCartesianImpedanceExampleController
+ right:
+ arm_id: panda_1
+ joint_names:
+ - panda_1_joint1
+ - panda_1_joint2
+ - panda_1_joint3
+ - panda_1_joint4
+ - panda_1_joint5
+ - panda_1_joint6
+ - panda_1_joint7
+ left:
+ arm_id: panda_2
+ joint_names:
+ - panda_2_joint1
+ - panda_2_joint2
+ - panda_2_joint3
+ - panda_2_joint4
+ - panda_2_joint5
+ - panda_2_joint6
+ - panda_2_joint7
+
+teleop_joint_pd_example_controller:
+ type: franka_example_controllers/TeleopJointPDExampleController
+ leader:
+ arm_id: leader
+ joint_names:
+ - leader_joint1
+ - leader_joint2
+ - leader_joint3
+ - leader_joint4
+ - leader_joint5
+ - leader_joint6
+ - leader_joint7
+ # WARNING: Varying the following parameters may result in instability!
+ d_gains_lower: [1.0, 1.0, 1.0, 1.0, 0.33, 0.33, 0.17]
+ d_gains_upper: [35.0, 35.0, 35.0, 35.0, 10.0, 10.0, 8.0]
+ dq_max_lower: [1.3, 1.3, 1.3, 1.3, 1.4, 1.4, 1.4] # [rad/s]
+ dq_max_upper: [1.9, 1.9, 1.9, 1.9, 2.2, 2.2, 2.2] # [rad/s]
+ contact_force_threshold: 4.0 # [N]
+
+ follower:
+ arm_id: follower
+ joint_names:
+ - follower_joint1
+ - follower_joint2
+ - follower_joint3
+ - follower_joint4
+ - follower_joint5
+ - follower_joint6
+ - follower_joint7
+ # WARNING: Varying the following parameters may result in instability!
+ p_gains: [900.0, 900.0, 900.0, 900.0, 375.0, 225.0, 100.0]
+ d_gains: [45.0, 45.0, 45.0, 45.0, 15.0, 15.0, 10.0]
+ drift_comp_gains: [4.3, 4.3, 4.3, 4.3, 4.3, 4.3, 4.3]
+ dq_max_lower: [0.8, 0.8, 0.8, 0.8, 2.5, 2.5, 2.5] # [rad/s]
+ dq_max_upper: [2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5] # [rad/s]
+ ddq_max_lower: [0.8, 0.8, 0.8, 0.8, 10.0, 10.0, 10.0] # [rad/s^2]
+ ddq_max_upper: [6.0, 6.0, 6.0, 6.0, 15.0, 20.0, 20.0] # [rad/s^2]
+ contact_force_threshold: 5.0 # [N]
+
+spacenav_teleop_twist_controller:
+ type: franka_example_controllers/SpacenavTeleopTwistController
+ arm_id: $(arg arm_id)
+ linear_gain: $(arg linear_gain)
+ angular_gain: $(arg angular_gain)
+
+cartesian_pose_controller:
+ type: franka_example_controllers/CartesianPoseController
+ arm_id: $(arg arm_id)
+
+cartesian_traj_controller:
+ type: franka_example_controllers/CartesianTrajController
+ arm_id: $(arg arm_id)
+
+cartesian_traj_twist_correct_controller:
+ type: franka_example_controllers/CartesianTrajTwistCorrectController
+ arm_id: $(arg arm_id)
diff --git a/fr3_impedance_controller_real/default.nix b/fr3_impedance_controller_real/default.nix
new file mode 100644
index 0000000000000000000000000000000000000000..8c7ab949d7951526a050262d6bb70bba529b0e1d
--- /dev/null
+++ b/fr3_impedance_controller_real/default.nix
@@ -0,0 +1,72 @@
+# This defines a function which returns a packge, intended to be evaluated by callPackage
+{ lib
+, buildRosPackage
+, catkin
+, controller-interface
+, dynamic-reconfigure
+, pluginlib
+, realtime-tools
+, eigen
+, eigen-conversions
+, franka-control
+, franka-description
+, franka-gripper
+, franka-hw
+, franka-example-controllers
+, libfranka
+, hardware-interface
+, joint-limits-interface
+, message-generation
+, message-runtime
+, geometry-msgs
+, visualization-msgs
+, roscpp
+, rospy
+, tf
+, tf-conversions
+, urdf
+
+, common # shared library from the project
+}:
+
+buildRosPackage {
+ pname = "fr3-impedance-controller";
+ version = "0.0.0";
+ src = ./.;
+
+ buildType = "catkin";
+
+ buildInputs = [ catkin eigen message-generation ];
+ propagatedBuildInputs = [
+ controller-interface
+ dynamic-reconfigure
+ eigen-conversions
+ franka-control
+ franka-description
+ franka-gripper
+ franka-hw
+ libfranka
+ franka-example-controllers
+ geometry-msgs
+ hardware-interface
+ joint-limits-interface
+
+ message-runtime
+ pluginlib
+ realtime-tools
+ roscpp rospy
+ tf
+ tf-conversions
+ urdf
+ visualization-msgs
+ common
+ ];
+
+ nativeBuildInputs = [ catkin ];
+
+
+ meta = {
+ description = "Impedance controller to run on the Franka Emika FR3";
+ };
+}
+
diff --git a/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml b/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1bec1f5d6871804f5fca75b9f2cf7b1d414278dd
--- /dev/null
+++ b/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml
@@ -0,0 +1,84 @@
+<library path="lib/libfranka_example_controllers">
+ <class name="franka_example_controllers/JointVelocityExampleController" type="franka_example_controllers::JointVelocityExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that executes a short motion based on joint velocities to demonstrate correct usage
+ </description>
+ </class>
+ <class name="franka_example_controllers/JointPositionExampleController" type="franka_example_controllers::JointPositionExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that executes a short motion based on joint positions to demonstrate correct usage
+ </description>
+ </class>
+ <class name="franka_example_controllers/CartesianPoseExampleController" type="franka_example_controllers::CartesianPoseExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that executes a short motion based on cartesian poses to demonstrate correct usage
+ </description>
+ </class>
+ <!-- ADD here -->
+ <class name="franka_example_controllers/CartesianSinPoseExampleController" type="franka_example_controllers::CartesianSinPoseExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that executes a short motion based on cartesian poses to demonstrate correct usage
+ </description>
+ </class>
+ <!-- -->
+ <class name="franka_example_controllers/CartesianVelocityExampleController" type="franka_example_controllers::CartesianVelocityExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that executes a short motion based on cartesian velocities to demonstrate correct usage
+ </description>
+ </class>
+ <class name="franka_example_controllers/ElbowExampleController" type="franka_example_controllers::ElbowExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that executes a short motion based on cartesian poses and elbow to demonstrate correct usage
+ </description>
+ </class>
+ <class name="franka_example_controllers/ModelExampleController" type="franka_example_controllers::ModelExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that evaluates and prints the dynamic model of Franka
+ </description>
+ </class>
+ <class name="franka_example_controllers/JointImpedanceExampleController" type="franka_example_controllers::JointImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that tracks a cartesian path with a joint impedance controller that compensates coriolis torques. The torque commands are compared to measured torques in Console outputs.
+ </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>
+ <class name="franka_example_controllers/ForceExampleController" type="franka_example_controllers::ForceExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A PI controller that applies a force corresponding to a user-provided desired mass in the z axis. The desired mass value can be modified online with dynamic reconfigure.
+ </description>
+ </class>
+ <class name="franka_example_controllers/DualArmCartesianImpedanceExampleController" type="franka_example_controllers::DualArmCartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that renders Cartesian impedance based tracking of separate target poses for 2 panda arms.
+ </description>
+ </class>
+ <class name="franka_example_controllers/TeleopJointPDExampleController" type="franka_example_controllers::TeleopJointPDExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A PD controller that tracks position and velocity of the leader arm and applies it to the follower arm. It also applies force-feedback to the leader arm.
+ </description>
+ </class>
+ <class name="franka_example_controllers/SpacenavTeleopTwistController" type="franka_example_controllers::SpacenavTeleopTwistController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A Controller that takes Twist Input from the 6DoF spacenav mouse and sends these to the robot.
+ </description>
+ </class>
+ <class name="franka_example_controllers/CartesianPoseController" type="franka_example_controllers::CartesianPoseController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A Controller that takes single cartesian pose (currently only position, no orientation).
+ </description>
+ </class>
+ <class name="franka_example_controllers/CartesianTrajController" type="franka_example_controllers::CartesianTrajController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A Controller that takes PoseArray (pos traj).
+ </description>
+ </class>
+ <class name="franka_example_controllers/CartesianTrajTwistCorrectController" type="franka_example_controllers::CartesianTrajTwistCorrectController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A Controller that takes PoseArray (pos traj) and can correct the unsmoothness of the traj with the spacenav twist commands.
+ </description>
+ </class>
+</library>
diff --git a/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp b/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..bfe738d9b80449658c32b9823d5dfe913f019099
--- /dev/null
+++ b/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
@@ -0,0 +1,116 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+#include <Eigen/Dense>
+
+#include <geometry_msgs/Twist.h> // for spacenave teleop
+#include <sensor_msgs/Joy.h> // for joystick command
+#include "eigen_types.hpp" // for custom Eigen types
+#include "orientation_utilities.hpp" // for orientation computation
+
+#include <franka_example_controllers/compliance_paramConfig.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+
+namespace franka_example_controllers
+{
+class CartesianImpedanceExampleController :
+ public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface,
+ hardware_interface::EffortJointInterface,
+ franka_hw::FrankaStateInterface>
+{
+public:
+ bool init(hardware_interface::RobotHW * robot_hw, ros::NodeHandle & node_handle) override;
+ void starting(const ros::Time &) override;
+ // void stopping(const ros::Time&) override;
+ void update(const ros::Time &, const ros::Duration & period) override;
+
+private:
+ // 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); // NOLINT (readability-identifier-naming)
+
+ std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
+ std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+ std::vector<hardware_interface::JointHandle> joint_handles_;
+
+ double filter_params_{ 0.005 };
+ double nullspace_stiffness_{ 20.0 };
+ double nullspace_stiffness_target_{ 20.0 };
+ const double delta_tau_max_{ 1.0 };
+
+ const std::vector<double> fric_phi1{
+ 0.54615, 0.87224, 0.64068, 1.2794, 0.83904, 0.30301, 0.56489
+ };
+ const std::vector<double> fric_phi2{ 5.1181, 9.0657, 10.136, 5.590, 8.3469, 17.133, 10.336 };
+ // const std::vector<double> fric_phi3{0.039533, 0.025882, -0.04607, 0.036194, 0.026226,
+ // -0.021047, 0.0035526};
+ const std::vector<double> fric_phi3{
+ 0., 0., 0., 0., 0., 0., 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_;
+
+ // Dynamic reconfigure
+ std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
+ dynamic_server_compliance_param_;
+ ros::NodeHandle dynamic_reconfigure_compliance_param_node_;
+ void complianceParamCallback(franka_example_controllers::compliance_paramConfig & config,
+ uint32_t level);
+
+ // Equilibrium pose subscriber
+ ros::Subscriber sub_equilibrium_pose_;
+ void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr & msg);
+
+ /* Target EE Pose command */
+ // bool b_is_marker_mode_ = false;
+ // ros::Subscriber sub_interactive_marker_;
+ // std::mutex target_ee_pose_mutex_;
+ // Vec3<double> ee_pos_world_des_target_;
+ // Quat<double> ee_quat_world_des_target_;
+
+ /* spacenav target twist subscriber */
+ ros::Subscriber sub_target_twist_;
+ void subscribe_target_twist(const geometry_msgs::TwistConstPtr & msg);
+ // Vec6<double> ee_vel_world_target_;
+ Vec6<double> ee_vel_scale_;
+ Vec6<double> ee_vel_world_des_, ee_vel_world_des_prev_;
+ Vec6<double> ee_acc_world_des_, ee_acc_world_des_prev_;
+ Vec3<double> ee_pos_world_des_;
+ Quat<double> ee_quat_world_des_;
+ Vec3<double> ee_rpy_world_des_, ee_rpy_world_des_prev_;
+ Vec3<double> ee_rpy_world_mes_, ee_rpy_world_mes_prev_;
+
+ Vec6<double> fr3_vel_limit_;
+ Vec6<double> fr3_acc_limit_;
+ Vec6<double> fr3_jrk_limit_;
+
+ ros::Subscriber sub_joy_;
+ void subscribe_joy_command(const sensor_msgs::JoyConstPtr & msg);
+ std::array<int, 2> btn_cmd_;
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch b/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..682f04cfcd180840857522b592d624f7b1c2eae4
--- /dev/null
+++ b/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch
@@ -0,0 +1,20 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
+ <arg name="arm_id" default="$(arg robot)" />
+
+ <arg name="linear_gain" default="1.0" />
+ <arg name="angular_gain" default="1.0" />
+
+ <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/>
+ <include file="$(find spacenav_node)/launch/classic.launch" pass_all_args="true"/>
+
+ <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" subst_value="true" />
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_impedance_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/>
+ <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen">
+ <param name="link_name" value="$(arg arm_id)_link0" />
+ <remap from="equilibrium_pose" to="/cartesian_impedance_example_controller/equilibrium_pose" />
+ </node>
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
+</launch>
diff --git a/fr3_impedance_controller_real/launch/rviz/franka_description_with_marker.rviz b/fr3_impedance_controller_real/launch/rviz/franka_description_with_marker.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..931fd1cb7a451ef3d2e437be10e3d56bdfcb3055
--- /dev/null
+++ b/fr3_impedance_controller_real/launch/rviz/franka_description_with_marker.rviz
@@ -0,0 +1,279 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /Marker1
+ - /Marker1/Status1
+ - /Pose1
+ Splitter Ratio: 0.5
+ Tree Height: 862
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/InteractiveMarkers
+ Enable Transparency: true
+ Enabled: false
+ Name: InteractiveMarkers
+ Show Axes: false
+ Show Descriptions: true
+ Show Visual Aids: false
+ Update Topic: /equilibrium_pose_marker/update
+ Value: false
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /mpc_trajectory
+ Name: Marker
+ Namespaces:
+ "": true
+ Queue Size: 100
+ Value: true
+ - Alpha: 1
+ Axes Length: 0.10000000149011612
+ Axes Radius: 0.05000000074505806
+ Class: rviz/Pose
+ Color: 255; 25; 0
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: Pose
+ Queue Size: 10
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Axes
+ Topic: /cartesian_impedance_example_controller/equilibrium_pose
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: panda_link0
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 1.8979061841964722
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.17539797723293304
+ Target Frame: <Fixed Frame>
+ Yaw: 0.0903986394405365
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1163
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a000003e9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003e9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003e9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000042fc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb000003e900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1920
+ X: 2560
+ Y: 0
diff --git a/fr3_impedance_controller_real/package.xml b/fr3_impedance_controller_real/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..297f2003c5617cd900dc32956aec47fee2b122c8
--- /dev/null
+++ b/fr3_impedance_controller_real/package.xml
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>fr3_impedance_controller_real</name>
+ <version>0.0.1</version>
+ <description>Impedance controller for the Franka Emika FR3</description>
+ <maintainer email="todo@todo.com"></maintainer>
+ <license>Apache 2.0</license>
+
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>message_generation</build_depend>
+ <build_depend>eigen</build_depend>
+
+ <build_export_depend>message_runtime</build_export_depend>
+
+ <depend>controller_interface</depend>
+ <depend>dynamic_reconfigure</depend>
+ <depend>eigen_conversions</depend>
+ <depend>franka_hw</depend>
+ <depend>franka_gripper</depend>
+ <depend>franka_example_controllers</depend>
+ <depend>geometry_msgs</depend>
+ <depend>hardware_interface</depend>
+ <depend>joint_limits_interface</depend>
+ <depend>tf</depend>
+ <depend>tf_conversions</depend>
+ <depend>libfranka</depend>
+ <depend>pluginlib</depend>
+ <depend>realtime_tools</depend>
+ <depend>roscpp</depend>
+ <depend>urdf</depend>
+ <depend>sensor_msgs</depend>
+ <depend>visualization_msgs</depend>
+
+ <exec_depend>franka_control</exec_depend>
+ <exec_depend>franka_description</exec_depend>
+ <exec_depend>message_runtime</exec_depend>
+ <exec_depend>rospy</exec_depend>
+
+ <export>
+ <controller_interface plugin="${prefix}/fr3_impedance_controller_real_plugin.xml" />
+ </export>
+</package>
diff --git a/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp b/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e7798abde6b77b30ad58cf259a7af0f5293b60d6
--- /dev/null
+++ b/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp
@@ -0,0 +1,363 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/cartesian_impedance_example_controller.h>
+
+#include <cmath>
+#include <iomanip>
+#include <memory>
+
+#include <controller_interface/controller_base.h>
+#include <franka/robot_state.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <franka_example_controllers/pseudo_inversion.h>
+
+namespace franka_example_controllers
+{
+bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW * robot_hw,
+ ros::NodeHandle & node_handle)
+{
+ std::vector<double> cartesian_stiffness_vector;
+ std::vector<double> cartesian_damping_vector;
+
+ /* Subscriber declaration */
+ sub_equilibrium_pose_ = node_handle.subscribe(
+ "equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
+ ros::TransportHints().reliable().tcpNoDelay());
+
+ sub_target_twist_ = node_handle.subscribe<geometry_msgs::Twist>(
+ "/spacenav/twist", 1, &CartesianImpedanceExampleController::subscribe_target_twist, this);
+
+ sub_joy_ = node_handle.subscribe<sensor_msgs::Joy>(
+ "/spacenave/joy", 1, &CartesianImpedanceExampleController::subscribe_joy_command, this);
+
+ /* Initialize limits */
+ fr3_vel_limit_ << 3.0, 3.0, 3.0, 2.5, 2.5, 2.5; // [m/s, rad/s]
+ fr3_acc_limit_ << 9.0, 9.0, 9.0, 17.0, 17.0, 17.0; // [m/s^2, rad/s^2]
+ fr3_jrk_limit_ << 4500.0, 4500.0, 4500.0, 8500.0, 8500.0, 8500.0; // [m/s^3, rad/s^3]
+
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id))
+ {
+ ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
+ return false;
+ }
+ std::vector<std::string> joint_names;
+ if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7)
+ {
+ ROS_ERROR(
+ "CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
+ "aborting controller init!");
+ return false;
+ }
+
+ auto * model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
+ if (model_interface == nullptr)
+ {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Error getting model interface from hardware");
+ return false;
+ }
+ try
+ {
+ model_handle_ =
+ std::make_unique<franka_hw::FrankaModelHandle>(model_interface->getHandle(arm_id + "_model"));
+ }
+ catch (hardware_interface::HardwareInterfaceException & ex)
+ {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Exception getting model handle from interface: "
+ << ex.what());
+ return false;
+ }
+
+ auto * state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr)
+ {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Error getting state interface from hardware");
+ return false;
+ }
+ try
+ {
+ state_handle_ =
+ std::make_unique<franka_hw::FrankaStateHandle>(state_interface->getHandle(arm_id + "_robot"));
+ }
+ catch (hardware_interface::HardwareInterfaceException & ex)
+ {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Exception getting state handle from interface: "
+ << ex.what());
+ return false;
+ }
+
+ auto * effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
+ if (effort_joint_interface == nullptr)
+ {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Error getting effort joint interface from hardware");
+ return false;
+ }
+ for (size_t i = 0; i < 7; ++i)
+ {
+ try
+ {
+ joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
+ }
+ catch (const hardware_interface::HardwareInterfaceException & ex)
+ {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what());
+ return false;
+ }
+ }
+
+ dynamic_reconfigure_compliance_param_node_ =
+ ros::NodeHandle(node_handle.getNamespace() + "/dynamic_reconfigure_compliance_param_node");
+
+ dynamic_server_compliance_param_ = std::make_unique<
+ dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>(
+
+ dynamic_reconfigure_compliance_param_node_);
+ dynamic_server_compliance_param_->setCallback(
+ boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
+
+ /* Initialize states */
+ ee_vel_scale_ << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
+ ee_vel_world_des_.setZero();
+ ee_vel_world_des_prev_.setZero();
+ ee_acc_world_des_.setZero();
+ ee_acc_world_des_prev_.setZero();
+ ee_pos_world_des_.setZero();
+ ee_quat_world_des_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+ ee_rpy_world_des_.setZero();
+ ee_rpy_world_des_prev_.setZero();
+ ee_rpy_world_mes_.setZero();
+ ee_rpy_world_mes_prev_.setZero();
+
+ position_d_.setZero();
+ orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+ position_d_target_.setZero();
+ orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+
+ cartesian_stiffness_.setZero();
+ cartesian_damping_.setZero();
+
+ return true;
+}
+
+void CartesianImpedanceExampleController::starting(const ros::Time & /*time*/)
+{
+ // compute initial velocity with jacobian and set x_attractor and q_d_nullspace
+ // to initial configuration
+ franka::RobotState initial_state = state_handle_->getRobotState();
+ // get jacobian
+ std::array<double, 42> jacobian_array =
+ model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+ // convert to eigen
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> q_initial(initial_state.q.data());
+ Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
+
+ // set equilibrium point to current state
+ position_d_ = initial_transform.translation();
+ orientation_d_ = Eigen::Quaterniond(initial_transform.rotation());
+ position_d_target_ = initial_transform.translation();
+ orientation_d_target_ = Eigen::Quaterniond(initial_transform.rotation());
+
+ ee_pos_world_des_ = initial_transform.translation();
+ ee_quat_world_des_ = Eigen::Quaterniond(initial_transform.rotation());
+
+ // set nullspace equilibrium configuration to initial q
+ q_d_nullspace_ = q_initial;
+}
+
+void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
+ const ros::Duration & period)
+{
+ /* get target end-effector's twist */
+ // std::cout << std::fixed;
+ // std::cout << std::setprecision(5);
+ // std::cout << "Desired EE twist:\t" << ee_vel_world_des_.transpose() << std::endl;
+
+ // TODO: Add safety for FR3 end-effector's vel, acc, jerk limits
+
+ Vec3<double> v_des = ee_vel_world_des_.head(3);
+ Vec3<double> w_des = ee_vel_world_des_.tail(3);
+ ee_pos_world_des_ += v_des * period.toSec();
+ ee_quat_world_des_ = orient_utils::getNewQuat(ee_quat_world_des_, w_des, period.toSec());
+ ee_rpy_world_des_ = orient_utils::quaternionToRPY(ee_quat_world_des_, ee_rpy_world_des_prev_);
+
+ // TODO: Add conversion of Quat2RPY to make debugging easier
+
+ // get state variables
+ franka::RobotState robot_state = state_handle_->getRobotState();
+ std::array<double, 7> coriolis_array = model_handle_->getCoriolis();
+ std::array<double, 42> jacobian_array =
+ model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+
+ // convert to Eigen
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
+ Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d( // NOLINT (readability-identifier-naming)
+ robot_state.tau_J_d.data());
+ Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
+ Eigen::Vector3d position(transform.translation());
+ Eigen::Quaterniond orientation(transform.rotation());
+ ee_rpy_world_mes_ = orient_utils::quaternionToRPY(orientation, ee_rpy_world_mes_prev_);
+ // std::cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << std::endl;
+ // std::cout << "Measured EE pos [m]:\t" << position.transpose() << std::endl << std::endl;
+ // std::cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << std::endl;
+ // std::cout << "Measured EE RPY [rad]:\t" << ee_rpy_world_mes_.transpose() << std::endl <<
+ // std::endl;
+
+ // compute error to desired pose
+ // position error
+ Eigen::Matrix<double, 6, 1> error;
+ error.head(3) << position - ee_pos_world_des_;
+
+ // orientation error
+ if (ee_quat_world_des_.coeffs().dot(orientation.coeffs()) < 0.0)
+ {
+ orientation.coeffs() << -orientation.coeffs();
+ }
+ // "difference" quaternion
+ Eigen::Quaterniond error_quaternion(orientation.inverse() * ee_quat_world_des_);
+ error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
+ // Transform to base frame
+ error.tail(3) << -transform.rotation() * error.tail(3);
+
+ // compute control
+ // allocate variables
+ Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7), tau_fr(7);
+
+ tau_fr.setZero();
+
+ // pseudoinverse for nullspace handling
+ // kinematic pseuoinverse
+ Eigen::MatrixXd jacobian_transpose_pinv;
+ pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv);
+
+ // Cartesian PD control with damping ratio = 1
+ tau_task << jacobian.transpose() *
+ (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq));
+ // nullspace PD control with damping ratio = 1
+ tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) -
+ jacobian.transpose() * jacobian_transpose_pinv) *
+ (nullspace_stiffness_ * (q_d_nullspace_ - q) -
+ (2.0 * sqrt(nullspace_stiffness_)) * dq);
+
+ // calculate friction torque
+ for (size_t i = 0; i < 7; i++)
+ {
+ tau_fr(i) = fric_phi1[i] / (1 + exp(-fric_phi2[i] * (dq(i) + fric_phi3[i]))) -
+ fric_phi1[i] / (1 + exp(-fric_phi2[i] * fric_phi3[i]));
+ }
+
+ // Desired torque
+ tau_d << tau_task + tau_nullspace + coriolis + tau_fr;
+
+ // Saturate torque rate to avoid discontinuities
+ tau_d << saturateTorqueRate(tau_d, tau_J_d);
+ // std::cout << "Desired torque:\t" << tau_d.transpose() << std::endl;
+ for (size_t i = 0; i < 7; ++i)
+ {
+ joint_handles_[i].setCommand(tau_d(i));
+ }
+
+ /* update previous states */
+ ee_vel_world_des_prev_ = ee_vel_world_des_;
+ ee_acc_world_des_prev_ = ee_acc_world_des_;
+ ee_rpy_world_des_prev_ = ee_rpy_world_des_;
+ ee_rpy_world_mes_prev_ = ee_rpy_world_mes_;
+
+ // update parameters changed online either through dynamic reconfigure or through the interactive
+ // target by filtering
+ cartesian_stiffness_ =
+ filter_params_ * cartesian_stiffness_target_ + (1.0 - filter_params_) * cartesian_stiffness_;
+ cartesian_damping_ =
+ filter_params_ * cartesian_damping_target_ + (1.0 - filter_params_) * cartesian_damping_;
+ nullspace_stiffness_ =
+ filter_params_ * nullspace_stiffness_target_ + (1.0 - filter_params_) * nullspace_stiffness_;
+ std::lock_guard<std::mutex> position_d_target_mutex_lock(
+ position_and_orientation_d_target_mutex_);
+ position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_;
+ orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_);
+}
+
+Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate(
+ const Eigen::Matrix<double, 7, 1> & tau_d_calculated, const Eigen::Matrix<double, 7, 1> & tau_J_d)
+{ // NOLINT (readability-identifier-naming)
+ Eigen::Matrix<double, 7, 1> tau_d_saturated{};
+ for (size_t i = 0; i < 7; i++)
+ {
+ double difference = tau_d_calculated[i] - tau_J_d[i];
+ tau_d_saturated[i] =
+ tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
+ }
+ return tau_d_saturated;
+}
+
+void CartesianImpedanceExampleController::complianceParamCallback(
+ franka_example_controllers::compliance_paramConfig & config, uint32_t /*level*/)
+{
+ cartesian_stiffness_target_.setIdentity();
+
+ // 26.4.23 @Kevin, change so can set stiffness independent per DOF
+ // cartesian_stiffness_target_.topLeftCorner(3, 3)
+ // << config.translational_stiffness * Eigen::Matrix3d::Identity();
+ cartesian_stiffness_target_(0, 0) = config.translational_stiffness_x;
+ cartesian_stiffness_target_(1, 1) = config.translational_stiffness_y;
+ cartesian_stiffness_target_(2, 2) = config.translational_stiffness_z;
+
+ cartesian_stiffness_target_.bottomRightCorner(3, 3)
+ << config.rotational_stiffness * Eigen::Matrix3d::Identity();
+ cartesian_damping_target_.setIdentity();
+ // Damping ratio = 1
+ for (size_t i = 0; i < 6; i++)
+ {
+ cartesian_damping_target_(i, i) = 2.0 * sqrt(cartesian_stiffness_target_(i, i));
+ }
+ nullspace_stiffness_target_ = config.nullspace_stiffness;
+}
+
+void CartesianImpedanceExampleController::equilibriumPoseCallback(
+ const geometry_msgs::PoseStampedConstPtr & msg)
+{
+ std::lock_guard<std::mutex> position_d_target_mutex_lock(
+ position_and_orientation_d_target_mutex_);
+ position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
+ Eigen::Quaterniond last_orientation_d_target(orientation_d_target_);
+ orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y,
+ msg->pose.orientation.z, msg->pose.orientation.w;
+ if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0)
+ {
+ orientation_d_target_.coeffs() << -orientation_d_target_.coeffs();
+ }
+}
+
+void CartesianImpedanceExampleController::subscribe_target_twist(
+ const geometry_msgs::TwistConstPtr & msg)
+{
+ // std::cout << msg->linear.x << ", " << msg->linear.y << ", " << msg->linear.z << std::endl;
+ ee_vel_world_des_[0] = ee_vel_scale_[0] * msg->linear.x;
+ ee_vel_world_des_[1] = ee_vel_scale_[1] * msg->linear.y;
+ ee_vel_world_des_[2] = ee_vel_scale_[2] * msg->linear.z;
+ ee_vel_world_des_[3] = ee_vel_scale_[3] * msg->angular.x;
+ ee_vel_world_des_[4] = ee_vel_scale_[4] * msg->angular.y;
+ ee_vel_world_des_[5] = ee_vel_scale_[5] * msg->angular.z;
+}
+
+void CartesianImpedanceExampleController::subscribe_joy_command(
+ const sensor_msgs::JoyConstPtr & msg)
+{
+ btn_cmd_[0] = msg->buttons[0];
+ btn_cmd_[1] = msg->buttons[1];
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController,
+ controller_interface::ControllerBase)