diff --git a/.gitignore b/.gitignore
index 914e43bf52d4a975464da295a54cab9d51b3f971..507af4884359d1bb8867e58d1646c045ebb7fc7c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,7 +5,7 @@ fr3_impedance_controller_real/build/
fr3_impedance_controller_sim/build/
fr3_ros1_mujoco_interface/build/
fr3_ps3_joy_publisher/build/
+test/build/
assets/data/matlab/*.asv
assets/data/*.csv
-
# assets/data/*.bag
diff --git a/assets/data/250208_contact_01.bag b/assets/data/250208_contact_01.bag
new file mode 100644
index 0000000000000000000000000000000000000000..e78e36e90181dbabe15c50339041d1a85ef13072
Binary files /dev/null and b/assets/data/250208_contact_01.bag differ
diff --git a/assets/data/250208_contact_02.bag b/assets/data/250208_contact_02.bag
new file mode 100644
index 0000000000000000000000000000000000000000..4731655b23bfd46646160e7c2df04ae7a37c40ff
Binary files /dev/null and b/assets/data/250208_contact_02.bag differ
diff --git a/assets/data/bag2csv.md b/assets/data/bag2csv.md
new file mode 100644
index 0000000000000000000000000000000000000000..66c039489585f28272d23ffbb8733397f2e013dc
--- /dev/null
+++ b/assets/data/bag2csv.md
@@ -0,0 +1,5 @@
+```
+$ rosbag info <bag-file-name>
+$ rostopic echo -b <bag-file-name> -p <topic-name> > output.txt
+$ mv output.txt output.csv
+```
\ No newline at end of file
diff --git a/assets/data/matlab/experiment_data_analysis.m b/assets/data/matlab/experiment_data_analysis.m
new file mode 100644
index 0000000000000000000000000000000000000000..b8354f365c9d78c2b4f1ee76e0d7c6dbbd8f5f69
--- /dev/null
+++ b/assets/data/matlab/experiment_data_analysis.m
@@ -0,0 +1,248 @@
+%% load data
+clc; close all; clear;
+
+ctrl_state_file = '../rosbag/250208_contact_02_controller_state.csv';
+ctrl_state_data_table = readtable(ctrl_state_file);
+ctrl_state_data_array = table2array(ctrl_state_data_table);
+ctrl_state_data_array(:,2:4) = [];
+
+ft_sensor_file = '../rosbag/250208_contact_02_ft_sensor.csv';
+ft_sensor_data_table = readtable(ft_sensor_file);
+ft_sensor_data_array = table2array(ft_sensor_data_table);
+ft_sensor_data_array(:, 2:22) = [];
+
+%%
+vec_size = [1, 7, 7, 7, 7, 7, 7, 6, 6, 6, 6, 7, 7, 6, 6, 6, 6, 6];
+start_idx = [1, ];
+
+for i = 1:length(vec_size) - 1
+ start_idx(i+1) = start_idx(i) + vec_size(i);
+end
+
+data_cell = cell(1, length(vec_size));
+
+for i = 1:length(vec_size)
+ data_cell{i} = ctrl_state_data_array(:, start_idx(i):start_idx(i) + vec_size(i) - 1);
+end
+
+t_ctrl = data_cell{1};
+ddq_eul= data_cell{2};
+ddq_mob = data_cell{3};
+ddq_sosml = data_cell{4};
+tau_d_dob = data_cell{5};
+tau_d_mob = data_cell{6};
+tau_d_sosml = data_cell{7};
+F_d_fr3 = data_cell{8};
+F_d_dob = data_cell{9};
+F_d_mob = data_cell{10};
+F_d_sosml = data_cell{11};
+ee_pose_des = data_cell{12};
+ee_pose_mes = data_cell{13};
+ee_vel_des = data_cell{14};
+ee_vel_mes = data_cell{15};
+ee_acc_eul = data_cell{16};
+ee_acc_mob = data_cell{17};
+ee_acc_sosml = data_cell{18};
+
+%%
+
+
+offset = ft_sensor_data_array(1, :);
+t_ft = ft_sensor_data_array(:, 1);
+Fx_ft = ft_sensor_data_array(:, 2) - offset(2);
+Fy_ft = ft_sensor_data_array(:, 3) - offset(3);
+Fz_ft = ft_sensor_data_array(:, 4) - offset(4);
+Tx_ft = ft_sensor_data_array(:, 5) - offset(5);
+Ty_ft = ft_sensor_data_array(:, 6) - offset(6);
+Tz_ft = ft_sensor_data_array(:, 7) - offset(7);
+%%
+
+t_start = t_ctrl(1);
+t_end = t_ctrl(end);
+
+lw = 1.2;
+font = 'Times New Roman';
+fs_lgd = 14;
+fs_lbl = 22;
+fs_axis = 14;
+fs_title = 20;
+
+%% Joint acceleration
+% close all;
+figure();
+fig_ddq = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$\ddot{q}_1$', '$\ddot{q}_2$', '$\ddot{q}_3$', '$\ddot{q}_4$', '$\ddot{q}_5$', ...
+ '$\ddot{q}_6$', '$\ddot{q}_7$',};
+plt_indices = 1:7;
+
+for i = 1:length(plt_indices)
+ nexttile;
+ hold on;
+ grid;
+ plot(t_ctrl, ddq_eul(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'k');
+ plot(t_ctrl, ddq_mob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'r');
+ % plot(t, ddq_sosml(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 2 * lw, 'Color', 'g');
+ % plt2.Color(4) = 0.7;
+ xlim([t_start, t_end]);
+ % ylim([-5 5]);
+
+ ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
+
+ % if i == 7
+ % ylim([-1 1]);
+ % end
+
+ if i == 1
+ legend('Euler method', 'MOB', 'SOSML', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+ end
+end
+
+title(fig_ddq, 'Joint Acceleration $[rad/s^2]$', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_ddq, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Task acceleration
+% close all;
+figure();
+fig_ddx = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$\ddot{p}_x$', '$\ddot{p}_y$', '$\ddot{p}_z$', '$\dot\omega_{x}$', ...
+ '$\dot\omega_{y}$', '$\dot\omega_{z}$'};
+plt_indices = 1:6;
+
+for i = 1:length(plt_indices)
+ nexttile;
+ hold on;
+ grid;
+ plot(t_ctrl, ee_acc_eul(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'k');
+ plot(t_ctrl, ee_acc_mob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
+ % plot(t, ee_acc_sosml(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
+ xlim([t_start, t_end]);
+ ylabel(ytitles(i), 'Interpreter', 'latex', 'FontSize', fs_lbl);
+
+ % if i == 1 || i == 2 || i == 3
+ % ylim([-1 1]);
+ % elseif i == 4 || i == 5 || i == 6
+ % ylim([-2 2]);
+ % end
+
+ if i == 3
+ legend('Euler method', 'MOB estimation', 'SOSML estimation', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+ end
+end
+
+title(fig_ddx, 'Task Acceleration', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_ddx, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Pose Tracking Performance
+% close all;
+figure();
+fig_pose = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$p_{ee} \ [m]$', '$\psi_{ee} \ [rad]$'};
+plt_indices = 1:7;
+
+for i = 1:length(plt_indices)
+ nexttile;
+ hold on;
+ grid;
+ plot(t_ctrl, ee_pose_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
+ plot(t_ctrl, ee_pose_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', lw, 'Color', 'r');
+ xlim([t_start, t_end]);
+
+ if i == 1
+ ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
+ end
+
+ if i == 1
+ legend('Desired', 'Impedance control', 'Impedance control + DOB' , 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+ end
+end
+
+% for i = 1:length(plt_indices)
+% nexttile;
+% hold on;
+% grid;
+% plot(t, ee_rpy_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
+% plot(t, ee_rpy_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', lw, 'Color', 'r');
+% plot(t_dob, ee_rpy_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
+% % plot(t, fr_ee_rpy_mes(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
+% xlim([t_start, t_end]);
+%
+% if i == 1
+% ylabel(ytitles{mod(i-1, 2) + 1 + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
+% end
+% end
+
+title(fig_pose, 'Pose Tracking', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_pose, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Twist Tracking Performance
+figure();
+fig_twist = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$\dot{p}_{ee} \ [m/s]$', '$\omega_{ee} \ [rad/s]$'};
+plt_indices = 1:6;
+
+for i = 1:length(plt_indices)
+ nexttile;
+ hold on;
+ grid;
+ plot(t_ctrl, ee_vel_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
+ plot(t_ctrl, ee_vel_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 1.5 * lw, 'Color', 'r');
+ xlim([t_start, t_end]);
+
+ if i == 1 || i == 4
+ ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
+ end
+
+ if i == 1
+ legend('Desired', 'Measured', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+ end
+end
+
+title(fig_twist, 'Twist Tracking', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_twist, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Estimated disturbance
+% close all;
+figure();
+% ytitles = {'$\hat{F}_{ext} \ [N]$', '$\hat{T}_{ext} \ [N/m]$'};
+
+nexttile;
+hold on;
+grid;
+plot(t_ft, Fz_ft, 'LineStyle', '--', 'LineWidth', 1 * lw, 'Color', 'k');
+plot(t_ctrl, -1 * F_d_fr3(:, 3), 'LineStyle', '-', 'LineWidth', 2*lw, 'Color', '#EDB120');
+plot(t_ctrl, F_d_dob(:, 3), 'LineStyle', '-.', 'LineWidth', 2*lw, 'Color', '#0072BD');
+plot(t_ctrl, F_d_mob(:, 3), 'LineStyle', '-', 'LineWidth', 1 * lw, 'Color', '#D95319');
+% plot(t, F_d_sosml(:, 3), 'LineStyle', '-', 'LineWidth', 1 * lw, 'Color', 'g');
+xlim([t_start, t_end]);
+
+title('Task Disturbance Estimation', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel('Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
+
+%% Joint torque disturbance estiamtion
+% close all;
+figure();
+fig_ddq = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$\tau_1$', '$\tau_2$', '$\tau_3$', '$\tau_4$', '$\tau_5$', '$\tau_6$', '$\tau_7$',};
+plt_indices = 1:7;
+
+for i = 1:length(plt_indices)
+ nexttile;
+ hold on;
+ grid;
+ plot(t_ctrl, tau_d_dob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
+ plot(t_ctrl, tau_d_mob(:,plt_indices(i)), 'LineStyle', '--', 'LineWidth', 1.5 * lw, 'Color', 'b');
+ % plot(t, tau_d_sosml(:,plt_indices(i)), 'LineStyle', '--', 'LineWidth', 1.5 * lw, 'Color', 'g');
+ % plot(t, tau_c(:,plt_indices(i)) + tau_g(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
+ % plot(t, tau_c(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
+ xlim([t_start, t_end]);
+ % ylim([-5 5]);
+
+ ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
+
+ if i == 1
+ legend('MOB estimation', 'DOB estimation', 'MuJoCo', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+ end
+end
+
+title(fig_ddq, 'Joint Acceleration $[rad/s^2]$', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
+xlabel(fig_ddq, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
diff --git a/assets/model/double_pendulum.urdf b/assets/model/double_pendulum.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..15e89ccc59e480db54e192eeea6418c43b7064eb
--- /dev/null
+++ b/assets/model/double_pendulum.urdf
@@ -0,0 +1,87 @@
+<?xml version="1.0" ?>
+<robot name="double_pendulum">
+
+ <!-- Material Definitions -->
+ <material name="black">
+ <color rgba="0.0 0.0 0.0 1.0" />
+ </material>
+ <material name="blue">
+ <color rgba="0.0 0.0 1.0 1.0" />
+ </material>
+ <material name="red">
+ <color rgba="1.0 0.0 0.0 1.0" />
+ </material>
+
+ <!-- Base link -->
+ <link name="base_link"/>
+
+ <!-- First link -->
+ <link name="link1">
+ <inertial>
+ <origin xyz="0 0 0.5" rpy="0 0 0"/>
+ <mass value="1.0"/>
+ <inertia ixx="0.03" ixy="0.0" ixz="0.0" iyy="0.04" iyz="0.0" izz="0.05"/>
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0.5" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.1 0.1 1.0"/>
+ </geometry>
+ <material name="blue"/>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0.5" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.1 0.1 1.0"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <!-- Joint connecting base to first link -->
+ <joint name="joint1" type="revolute">
+ <parent link="base_link"/>
+ <child link="link1"/>
+ <origin xyz="0 0 1.5" rpy="-1.5708 0 0"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="10.0" velocity="5.0" lower="-3.14" upper="3.14"/>
+ </joint>
+
+ <!-- Second link -->
+ <link name="link2">
+ <inertial>
+ <origin xyz="0 0 0.5" rpy="0 0 0"/>
+ <mass value="1.0"/>
+ <inertia ixx="0.07" ixy="0.0" ixz="0.0" iyy="0.06" iyz="0.0" izz="0.08"/>
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0.5" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.1 0.1 1.0"/>
+ </geometry>
+ <material name="red"/>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0.5" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.1 0.1 1.0"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <!-- Joint connecting first link to second link -->
+ <joint name="joint2" type="revolute">
+ <parent link="link1"/>
+ <child link="link2"/>
+ <origin xyz="0.1 0 1.0" rpy="0 0 0"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="10.0" velocity="5.0" lower="-3.14" upper="3.14"/>
+ </joint>
+
+ <!-- TCP frame -->
+ <link name="tcp_link"/>
+ <joint name="tcp_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 1.0"/>
+ <parent link="link2"/>
+ <child link="tcp_link"/>
+ </joint>
+</robot>
diff --git a/cmake/ClangTools.cmake b/cmake/ClangTools.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..84b474014a4b3d49a3e4f1c79074bfdeb77d51be
--- /dev/null
+++ b/cmake/ClangTools.cmake
@@ -0,0 +1,65 @@
+include(CMakeParseArguments)
+
+find_program(CLANG_FORMAT_PROG clang-format-7 DOC "'clang-format' executable")
+if(CLANG_FORMAT_PROG AND NOT TARGET format)
+ add_custom_target(format)
+ add_custom_target(check-format)
+endif()
+find_program(CLANG_TIDY_PROG clang-tidy-7 DOC "'clang-tidy' executable")
+if(CLANG_TIDY_PROG AND NOT TARGET tidy)
+ if(NOT CMAKE_EXPORT_COMPILE_COMMANDS)
+ message(WARNING "Invoke Catkin/CMake with '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON'
+ to generate compilation database for 'clang-tidy'.")
+ endif()
+
+ add_custom_target(tidy)
+ add_custom_target(check-tidy)
+endif()
+
+function(add_format_target _target)
+ if(NOT CLANG_FORMAT_PROG)
+ return()
+ endif()
+ cmake_parse_arguments(ARG "" "" "FILES" ${ARGN})
+
+ add_custom_target(format-${_target}
+ COMMAND ${CLANG_FORMAT_PROG} -i ${ARG_FILES}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
+ COMMENT "Formatting ${_target} source code with clang-format"
+ VERBATIM
+ )
+ add_dependencies(format format-${_target})
+
+ add_custom_target(check-format-${_target}
+ COMMAND ${CLANG_FORMAT_PROG} -output-replacements-xml ${ARG_FILES} | grep "<replacement " > /dev/null && exit 1 || exit 0
+ WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
+ COMMENT "Checking ${_target} code formatting with clang-format"
+ VERBATIM
+ )
+ add_dependencies(check-format check-format-${_target})
+endfunction()
+
+function(add_tidy_target _target)
+ if(NOT CLANG_TIDY_PROG)
+ return()
+ endif()
+ cmake_parse_arguments(ARG "" "" "FILES;DEPENDS" ${ARGN})
+
+ add_custom_target(tidy-${_target}
+ COMMAND ${CLANG_TIDY_PROG} -fix -p=${CMAKE_BINARY_DIR} ${ARG_FILES}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
+ DEPENDS ${ARG_DEPENDS}
+ COMMENT "Running clang-tidy for ${_target}"
+ VERBATIM
+ )
+ add_dependencies(tidy tidy-${_target})
+
+ add_custom_target(check-tidy-${_target}
+ COMMAND ${CLANG_TIDY_PROG} -p=${CMAKE_BINARY_DIR} ${ARG_FILES} | grep . && exit 1 || exit 0
+ WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
+ DEPENDS ${ARG_DEPENDS}
+ COMMENT "Running clang-tidy for ${_target}"
+ VERBATIM
+ )
+ add_dependencies(check-tidy check-tidy-${_target})
+endfunction()
diff --git a/cmake/PepTools.cmake b/cmake/PepTools.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..1377a4c1e5db5ce2aeff48834ceec6ed42e029a6
--- /dev/null
+++ b/cmake/PepTools.cmake
@@ -0,0 +1,21 @@
+include(CMakeParseArguments)
+
+find_program(PEP_FORMAT_PROG pycodestyle DOC "'pycodestyle' executable")
+if(PEP_FORMAT_PROG AND NOT TARGET pyformat)
+ add_custom_target(check-pyformat)
+endif()
+
+function(add_pyformat_target _target)
+ if(NOT PEP_FORMAT_PROG)
+ return()
+ endif()
+ cmake_parse_arguments(ARG "" "" "FILES" ${ARGN})
+
+ add_custom_target(check-pyformat-${_target}
+ COMMAND ${PEP_FORMAT_PROG} ${ARG_FILES}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
+ COMMENT "Checking ${_target} code formatting with pycodestyle"
+ VERBATIM
+ )
+ add_dependencies(check-pyformat check-pyformat-${_target})
+endfunction()
\ No newline at end of file
diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt
index 9d4025de473be6d24009f9b1855d44952d2e959c..98a4b631b84e4f6e33cc9bba1d0c86daf06f2850 100644
--- a/common/CMakeLists.txt
+++ b/common/CMakeLists.txt
@@ -2,45 +2,39 @@
# 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
-################################################################################
+# --- Set minimum required version of cmake, project name and compile options
cmake_minimum_required(VERSION 3.10)
project(common)
-# Set C++ standard
+# --- Set C++ standard
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED True)
-################################################################################
-## Find required CMake packages : pinocchio, casadi, catkin, ROS-related ...
-################################################################################
+# --- 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
-################################################################################
+# --- 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
+ src/controller/*.cpp
+ src/math/*.cpp
+ src/motion/*.cpp
+ src/robot/*.cpp
+ src/utilities/*.cpp
+ # src/interface/*.cpp
)
include_directories(
- controller/include
- math/include
- motion/include
- robot/include
- utilities/include
+ include/controller
+ include/math
+ include/motion
+ include/robot
+ include/utilities
+ # include/interface
)
-################################################################################
-## Define executable, Add source files & Link libraries
-################################################################################
+# --- Define library, connecting source files & Link libraries
add_library(common SHARED ${SOURCES})
target_link_libraries(common
@@ -50,13 +44,14 @@ target_link_libraries(common
)
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>
- )
+PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/controller>
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/math>
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/motion>
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/robot>
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/utilities>
+ # $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/interface>
+)
install(TARGETS common
EXPORT commonTargets
@@ -68,18 +63,21 @@ install(EXPORT commonTargets
NAMESPACE common::
DESTINATION lib/cmake/common)
-install(DIRECTORY controller/include/
+install(DIRECTORY include/controller/
DESTINATION include)
-install(DIRECTORY math/include/
+# install(DIRECTORY include/interface/
+# DESTINATION include)
+
+install(DIRECTORY include/math/
DESTINATION include)
-install(DIRECTORY motion/include/
+install(DIRECTORY include/motion/
DESTINATION include)
-install(DIRECTORY robot/include/
+install(DIRECTORY include/robot/
DESTINATION include)
-install(DIRECTORY utilities/include/
+install(DIRECTORY include/utilities/
DESTINATION include)
diff --git a/common/include/controller/DisturbanceObserver.hpp b/common/include/controller/DisturbanceObserver.hpp
index 76c3f9f6b8220224fced25bfc269459f156c62fe..dc3caf48599429c8b05a5b152cc96334af9d3bda 100644
--- a/common/include/controller/DisturbanceObserver.hpp
+++ b/common/include/controller/DisturbanceObserver.hpp
@@ -5,9 +5,6 @@
#ifndef DISTURBANCE_OBSERVER_HPP_
#define DISTURBANCE_OBSERVER_HPP_
-/* C++ STL */
-#include <iostream>
-
/* Custom Headers */
#include "eigen_types.hpp"
@@ -17,19 +14,26 @@ class DisturbanceObserver
private:
T dt_; // timestep
VecX<T> fq_; // Q-filter cutoff-frequency [Hz]
- VecX<T> Fd_hat_; // estimated force disturbance [N]
+ VecX<T> Fd_hat_; // estimated force disturbance [N, Nm]
VecX<T> F_lhs_, F_lhs_prev_; // left-hand-side state of DOB
VecX<T> F_rhs_, F_rhs_prev_; // right-hand-side state of DOB
public:
- DisturbanceObserver(const T & dt, const VecX<T> & fq);
+ DisturbanceObserver(const VecX<T> & fq);
+
+ //* ----- SETTERS --------------------------------------------------------------------------------
+ void set_parameters(const T & dt, const VecX<T> & fq)
+ {
+ dt_ = dt;
+ fq_ = fq;
+ }
//* ----- METHODS --------------------------------------------------------------------------------
- // void update_previous_states()
- // {
- // F_lhs_prev_ = F_lhs_;
- // F_rhs_prev_ = F_rhs_;
- // };
+ void update_previous_states()
+ {
+ F_lhs_prev_ = F_lhs_;
+ F_rhs_prev_ = F_rhs_;
+ };
//* ----- GETTERS --------------------------------------------------------------------------------
/**
diff --git a/common/include/controller/ImpedanceController.hpp b/common/include/controller/ImpedanceController.hpp
index 8a9a6af86ed8f2cd04e38a4a37310a7d9c7acf8d..94e1fd8887b72b55433f235d18eebe7126e3f36e 100644
--- a/common/include/controller/ImpedanceController.hpp
+++ b/common/include/controller/ImpedanceController.hpp
@@ -10,33 +10,36 @@ private:
Vec6<T> x_0e_err_; // end-effector 6-DoF pose error
Vec6<T> x_0e_err_prev_; // previous error
Mat6<T> K_task_; // task-space stiffness matrix
- Mat6<T> B_task_; // task-space damping matrix
+ Mat6<T> D_task_; // task-space damping matrix
Vec6<T> F_task_; // computed task-space wrench from impedance controller
Vec7<T> tau_task_; // joint-spce impedance torque
Vec7<T> q_null_err_; // null-space joint configuration error
Vec7<T> q_null_err_prev_; // previous error
Mat7<T> K_null_; // null-space stiffness matrix
- Mat7<T> B_null_; // null-space damping matrix
+ Mat7<T> D_null_; // null-space damping matrix
Vec7<T> tau_null_; // computed null-space torque from impedance controller
public:
- ImpedanceController(const Mat6<T> & K_task, const Mat6<T> & B_task, const Mat7<T> & K_null,
- const Mat7<T> & B_null);
+ ImpedanceController();
+ //* ----- SETTERS --------------------------------------------------------------------------------
+ void set_parameters(const Mat6<T> & K_task, const Mat6<T> & D_task, //
+ const Mat7<T> & K_null, const Mat7<T> & D_null);
- //* ----- METHODS ---------------------------------------------------------------------------- *//
+ //* ----- METHODS --------------------------------------------------------------------------------
/**
* @param q_null_des : desired null-space joint configuration
* @param q_null_mes : measured null-space joint configuration
*/
void compute_null_error(const Vec7<T> & q_null_des, const Vec7<T> & q_null_mes);
+
void update_previous_states()
{
q_null_err_prev_ = q_null_err_;
x_0e_err_prev_ = x_0e_err_;
};
- //* ----- GETTERS ---------------------------------------------------------------------------- *//
+ //* ----- GETTERS --------------------------------------------------------------------------------
/**
* @param dt : timestep
* @param p_0e_des : desired end-effector position in {W} frame
@@ -46,6 +49,7 @@ public:
*/
Vec6<T> get_pose_error(const Vec3<T> & p_0e_des, const Vec3<T> & p_0e_mes,
const Quat<T> & q_0e_des, const Quat<T> & q_0e_mes);
+
Vec6<T> get_previous_pose_error();
/**
@@ -55,13 +59,13 @@ public:
/**
* @param dq_null_err : Time-derivative of null-space joint configuration error
- * @param J : end-effector Jacobian matrix
- * @param Jt_pinv : pseudo-inverse of Jacobian transpose matrix
+ * @param J_t : end-effector Jacobian transpose matrix
+ * @param J_t_pinv : pseudo-inverse of Jacobian transpose matrix
*/
- Vec7<T> get_null_impedance_torque(const Vec7<T> & dq_null_err, const Eigen::MatrixXd & J,
- const Eigen::MatrixXd & Jt_pinv);
+ Vec7<T> get_null_impedance_torque(const Vec7<T> & dq_null_err, const MatX<T> & J_t,
+ const MatX<T> & J_t_pinv);
- //* ----- PRINTER ---------------------------------------------------------------------------- *//
+ //* ----- PRINTER --------------------------------------------------------------------------------
void print_pose_error();
void print_impedance_torque();
};
diff --git a/common/include/controller/MomentumObserver.hpp b/common/include/controller/MomentumObserver.hpp
index a306c48fb3578e0a89315f93a74402eb9848e8dc..eccbc8b3a4f5433b707a24b99d3414e545c406e8 100644
--- a/common/include/controller/MomentumObserver.hpp
+++ b/common/include/controller/MomentumObserver.hpp
@@ -25,11 +25,18 @@ private:
VecX<T> ddq_hat_; // n x 1 estimated joint acceleration [rad/s^2]
public:
- MomentumObserver(const T & dt, const VecX<T> & r_th, const VecX<T> & k);
+ MomentumObserver(const VecX<T> & r_th, const VecX<T> & k);
- //* ----- SETTERS ---------------------------------------------------------------------------- *//
+ //* ----- SETTERS --------------------------------------------------------------------------------
void set_dynamics(const VecX<T> & tau, const MatX<T> & M, const MatX<T> & C, const VecX<T> & g);
+ void set_parameters(const T & dt, const VecX<T> & r_th, const VecX<T> & k)
+ {
+ dt_ = dt;
+ r_th_ = r_th;
+ K_.setIdentity().diagonal() = k;
+ }
+
//* ----- METHODS ---------------------------------------------------------------------------- *//
/**
* @brief compute residual vector (= low-pass filtered version of external torque)
diff --git a/common/include/controller/SlidingModeObserver.hpp b/common/include/controller/SlidingModeObserver.hpp
index 00f929c02da80e2c8be0cf7d0036e7c988344a4e..ebf1677f9f18668b1a1a1db66eff8cd25b2543b9 100644
--- a/common/include/controller/SlidingModeObserver.hpp
+++ b/common/include/controller/SlidingModeObserver.hpp
@@ -28,12 +28,23 @@ private:
MatX<T> S1_, S2_, T1_, T2_;
public:
- SlidingModeObserver(const T & dt, const VecX<T> & s1, const VecX<T> & s2, const VecX<T> & t1,
+ SlidingModeObserver(const VecX<T> & s1, const VecX<T> & s2, const VecX<T> & t1,
const VecX<T> & t2);
//* ----- SETTERS ---------------------------------------------------------------------------- *//
void set_dynamics(const VecX<T> & tau, const MatX<T> & M, const MatX<T> & C, const VecX<T> & g);
+ // ! this may cause compile-error
+ void set_parameters(const T & dt, const VecX<T> & s1, const VecX<T> & s2, const VecX<T> & t1,
+ const VecX<T> & t2)
+ {
+ dt_ = dt;
+ S1_.setIdentity().diagonal() = s1;
+ S2_.setIdentity().diagonal() = s2;
+ T1_.setIdentity().diagonal() = t1;
+ T2_.setIdentity().diagonal() = t2;
+ }
+
//* ----- METHODS ---------------------------------------------------------------------------- *//
void compute_momentum_estimation(const VecX<T> & dq);
// void update_previous_states();
diff --git a/common/include/interface/DRIMPC.hpp b/common/include/interface/DRIMPC.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/common/include/utilities/Timer.hpp b/common/include/utilities/Timer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..47591d7502861e4803d6f369654628dae36c2dac
--- /dev/null
+++ b/common/include/utilities/Timer.hpp
@@ -0,0 +1,62 @@
+/*! @file Timer.hpp
+ * @brief Timer for measuring how long things take
+ */
+
+#ifndef TIMER_HPP_
+#define TIMER_HPP_
+
+#include <assert.h>
+#include <stdint.h>
+#include <time.h>
+
+class Timer
+{
+private:
+ struct timespec start_time_;
+
+public:
+ /*!
+ * Construct and start timer
+ */
+ explicit Timer()
+ {
+ start();
+ };
+
+ /*!
+ * Start the timer
+ */
+ void start()
+ {
+ clock_gettime(CLOCK_MONOTONIC, &start_time_);
+ }
+
+ /*!
+ * Get milliseconds elapsed
+ */
+ double getMillisec()
+ {
+ return (double)getNanosec() / 1.e6;
+ }
+
+ /*!
+ *Get nanoseconds elapsed
+ */
+ int64_t getNanosec()
+ {
+ struct timespec now;
+ clock_gettime(CLOCK_MONOTONIC, &now);
+ return (int64_t)(now.tv_nsec - start_time_.tv_nsec) +
+ 1'000'000'000 * (now.tv_sec - start_time_.tv_sec);
+ }
+
+ /*!
+ * Get seconds elapsed
+ */
+ double getSeconds()
+ {
+ return (double)getNanosec() / 1.e9;
+ }
+};
+
+#endif // TIMER_HPP_
\ No newline at end of file
diff --git a/common/src/controller/DisturbanceObserver.cpp b/common/src/controller/DisturbanceObserver.cpp
index 96270bf8ad809cfbac386054a70c0dfdda14cb10..fa500e3ae69d5e3fc4ee3c61afd174fe24b25e78 100644
--- a/common/src/controller/DisturbanceObserver.cpp
+++ b/common/src/controller/DisturbanceObserver.cpp
@@ -4,21 +4,31 @@
#include "DisturbanceObserver.hpp"
+/* C++ STL */
#include <cmath>
#include <iostream>
using namespace std;
template <typename T>
-DisturbanceObserver<T>::DisturbanceObserver(const T & dt, const VecX<T> & fq) : dt_(dt), fq_(fq)
+DisturbanceObserver<T>::DisturbanceObserver(const VecX<T> & fq) : fq_(fq)
{
- Fd_hat_.setZero(fq_.size());
- F_lhs_.setZero(fq_.size());
- F_lhs_prev_.setZero(fq_.size());
- F_rhs_.setZero(fq_.size());
- F_rhs_prev_.setZero(fq_.size());
+ // FIXME: change this
+ // if ((fq_.array() <= 0).any())
+ // {
+ // throw std::invalid_argument("fq_ must be positive and non-zero.");
+ // }
+
+ int n = fq_.size();
+
+ Fd_hat_.setZero(n);
+ F_lhs_.setZero(n);
+ F_lhs_prev_.setZero(n);
+ F_rhs_.setZero(n);
+ F_rhs_prev_.setZero(n);
};
+//* ----- GETTERS ----------------------------------------------------------------------------------
/**
* @brief compute estimated force disturbance
* @param F : n x 1 Cartesian force by Impedance control
@@ -34,8 +44,24 @@ VecX<T> DisturbanceObserver<T>::get_force_disturbance(const VecX<T> & F, const M
throw std::invalid_argument("F, Me, and ddx must have the same sizes");
}
- VecX<T> wt; // wt = (2*PI*f)*t
- wt = (2.0 * M_PI * fq_) * dt_;
+ /* prevent NaN */
+ MatX<T> Me_safe = (Me.array().isFinite()).select(Me, 0);
+
+ if ((F.array().isNaN()).any())
+ {
+ throw std::runtime_error("Input F contains NaN values.");
+ }
+ if ((Me_safe.array().isNaN()).any())
+ {
+ throw std::runtime_error("Input Me contains NaN values.");
+ }
+ if ((ddx.array().isNaN()).any())
+ {
+ throw std::runtime_error("Input ddx contains NaN values.");
+ }
+
+ VecX<T> wt = (2.0 * M_PI * fq_) * dt_; // wt = (2*PI*f)*t
+ wt = wt.cwiseMin(1e3);
VecX<T> q = wt.array() / (1.0 + wt.array());
MatX<T> Q;
@@ -48,9 +74,6 @@ VecX<T> DisturbanceObserver<T>::get_force_disturbance(const VecX<T> & F, const M
Fd_hat_ = F_rhs_ - F_lhs_;
- F_lhs_prev_ = F_lhs_;
- F_rhs_prev_ = F_rhs_;
-
return Fd_hat_;
}
diff --git a/common/src/controller/ImpedanceController.cpp b/common/src/controller/ImpedanceController.cpp
index ee5db752b9df003550f60079a8a78fdd2101bc28..0bcc66e28127962d1aa240592930157f777207b6 100644
--- a/common/src/controller/ImpedanceController.cpp
+++ b/common/src/controller/ImpedanceController.cpp
@@ -1,13 +1,15 @@
#include "ImpedanceController.hpp"
+#include "orientation_utilities.hpp"
+
#include <iostream>
+namespace ori = orient_utils;
+
using namespace std;
template <typename T>
-ImpedanceController<T>::ImpedanceController(const Mat6<T> & K_task, const Mat6<T> & B_task,
- const Mat7<T> & K_null, const Mat7<T> & B_null)
- : K_task_(K_task), B_task_(B_task), K_null_(K_null), B_null_(B_null)
+ImpedanceController<T>::ImpedanceController()
{
x_0e_err_.setZero();
x_0e_err_prev_.setZero();
@@ -17,9 +19,25 @@ ImpedanceController<T>::ImpedanceController(const Mat6<T> & K_task, const Mat6<T
q_null_err_.setZero();
q_null_err_prev_.setZero();
tau_null_.setZero();
+
+ K_task_.setIdentity();
+ D_task_.setIdentity();
+ K_null_.setIdentity();
+ D_null_.setIdentity();
+}
+
+//* ----- SETTERS ----------------------------------------------------------------------------------
+template <typename T>
+void ImpedanceController<T>::set_parameters(const Mat6<T> & K_task, const Mat6<T> & D_task,
+ const Mat7<T> & K_null, const Mat7<T> & D_null)
+{
+ K_task_ = K_task;
+ D_task_ = D_task;
+ K_null_ = K_null;
+ D_null_ = D_null;
}
-//* ----- METHODS ------------------------------------------------------------------------------ *//
+//* ----- METHODS ----------------------------------------------------------------------------------
template <typename T>
void ImpedanceController<T>::compute_null_error(const Vec7<T> & q_null_des,
const Vec7<T> & q_null_mes)
@@ -27,7 +45,7 @@ void ImpedanceController<T>::compute_null_error(const Vec7<T> & q_null_des,
q_null_err_ = q_null_des - q_null_mes;
}
-//* ----- GETTERS ------------------------------------------------------------------------------ *//
+//* ----- GETTERS ----------------------------------------------------------------------------------
template <typename T>
Vec6<T> ImpedanceController<T>::get_pose_error(const Vec3<T> & p_0e_des, const Vec3<T> & p_0e_mes,
const Quat<T> & q_0e_des, const Quat<T> & q_0e_mes)
@@ -44,8 +62,10 @@ Vec6<T> ImpedanceController<T>::get_pose_error(const Vec3<T> & p_0e_des, const V
q_new.coeffs() << q_0e_mes.coeffs();
}
Quat<T> q_0e_err = q_new.inverse() * q_0e_des;
+ RotMat<T> R = ori::quaternionToRotationMatrix(q_new);
x_0e_err_.tail(3) << q_0e_err.x(), q_0e_err.y(), q_0e_err.z();
- x_0e_err_.tail(3) << q_new * x_0e_err_.tail(3);
+ // x_0e_err_.tail(3) << q_new * x_0e_err_.tail(3);
+ x_0e_err_.tail(3) << R * x_0e_err_.tail(3);
return x_0e_err_;
}
@@ -62,16 +82,16 @@ Vec6<T> ImpedanceController<T>::get_previous_pose_error()
template <typename T>
Vec6<T> ImpedanceController<T>::get_task_impedance_force(const Vec6<T> & dx_0e_err)
{
- return K_task_ * x_0e_err_ + B_task_ * dx_0e_err;
+ return K_task_ * x_0e_err_ + D_task_ * dx_0e_err;
}
template <typename T>
Vec7<T> ImpedanceController<T>::get_null_impedance_torque(const Vec7<T> & dq_null_err,
- const Eigen::MatrixXd & J,
- const Eigen::MatrixXd & Jt_pinv)
+ const MatX<T> & J_t,
+ const MatX<T> & J_t_pinv)
{
- return (Eigen::MatrixXd::Identity(7, 7) - J.transpose() * Jt_pinv) *
- (K_null_ * q_null_err_ - B_null_ * dq_null_err);
+ Mat7<T> I = Mat7<T>::Identity();
+ return (I - J_t * J_t_pinv) * (K_null_ * q_null_err_ - D_null_ * dq_null_err);
}
//* ----- PRINTER ------------------------------------------------------------------------------ *//
diff --git a/common/src/controller/MomentumObserver.cpp b/common/src/controller/MomentumObserver.cpp
index 051e9ee794fa222e3953dd7ae1a26658c65a9254..23e66b275a0468d8de50cfcf4ec9924466112265 100644
--- a/common/src/controller/MomentumObserver.cpp
+++ b/common/src/controller/MomentumObserver.cpp
@@ -10,22 +10,22 @@
using namespace std;
template <typename T>
-MomentumObserver<T>::MomentumObserver(const T & dt, const VecX<T> & r_th, const VecX<T> & k)
- : dt_(dt), r_th_(r_th)
+MomentumObserver<T>::MomentumObserver(const VecX<T> & r_th, const VecX<T> & k) : r_th_(r_th)
{
- size_t nv = r_th_.size();
+ int n = r_th_.size();
- tau_.setZero(nv);
- M_.setZero(nv, nv);
- C_.setZero(nv, nv);
- g_.setZero(nv);
- r_.setZero(nv);
- ddq_hat_.setZero(nv);
+ tau_.setZero(n);
+ M_.setZero(n, n);
+ C_.setZero(n, n);
+ g_.setZero(n);
+ r_.setZero(n);
+ ddq_hat_.setZero(n);
- K_.setIdentity(nv, nv);
+ K_.setIdentity(n, n);
K_.diagonal() = k;
}
+//* ----- SETTERS ----------------------------------------------------------------------------------
template <typename T>
void MomentumObserver<T>::set_dynamics(const VecX<T> & tau, const MatX<T> & M, const MatX<T> & C,
const VecX<T> & g)
diff --git a/common/src/controller/SlidingModeObserver.cpp b/common/src/controller/SlidingModeObserver.cpp
index ef6beaa2065099701c08cf22408dab39eb6a50c3..d27921cf0c6adcff53e8f1af159a8e415a5eebd8 100644
--- a/common/src/controller/SlidingModeObserver.cpp
+++ b/common/src/controller/SlidingModeObserver.cpp
@@ -15,35 +15,27 @@
using namespace std;
template <typename T>
-SlidingModeObserver<T>::SlidingModeObserver(const T & dt, const VecX<T> & s1, const VecX<T> & s2,
+SlidingModeObserver<T>::SlidingModeObserver(const VecX<T> & s1, const VecX<T> & s2,
const VecX<T> & t1, const VecX<T> & t2)
- : dt_(dt_)
{
// std::cout << std::setprecision(6);
// std::cout << std::fixed;
- size_t nv = s1.size();
-
- tau_.setZero(nv);
- M_.setZero(nv, nv);
- C_.setZero(nv, nv);
- g_.setZero(nv);
-
- x1_hat_.setZero(nv);
- dx1_hat_.setZero(nv);
- x2_hat_.setZero(nv);
- dx2_hat_.setZero(nv);
-
- S1_.setIdentity(nv, nv);
- S1_.diagonal() = s1;
-
- S2_.setIdentity(nv, nv);
- S2_.diagonal() = s2;
-
- T1_.setIdentity(nv, nv);
- T1_.diagonal() = t1;
-
- T2_.setIdentity(nv, nv);
- T2_.diagonal() = t2;
+ int n = s1.size();
+
+ tau_.setZero(n);
+ M_.setZero(n, n);
+ C_.setZero(n, n);
+ g_.setZero(n);
+
+ x1_hat_.setZero(n);
+ dx1_hat_.setZero(n);
+ x2_hat_.setZero(n);
+ dx2_hat_.setZero(n);
+
+ S1_.setIdentity(n, n).diagonal() = s1;
+ S2_.setIdentity(n, n).diagonal() = s2;
+ T1_.setIdentity(n, n).diagonal() = t1;
+ T2_.setIdentity(n, n).diagonal() = t2;
};
//* ----- SETTERS ------------------------------------------------------------------------------ *//
@@ -72,6 +64,7 @@ void SlidingModeObserver<T>::compute_momentum_estimation(const VecX<T> & dq)
throw std::invalid_argument("Input sizes do not match internal state size.");
}
+ // ! this may cause NaN
VecX<T> f = M_.inverse() * (tau_ - C_ * dq - g_);
VecX<T> x1 = dq;
diff --git a/common/src/interface/DRIMPC.cpp b/common/src/interface/DRIMPC.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a054433e665a94f2e4209933f3a663d911c08da6
--- /dev/null
+++ b/common/src/interface/DRIMPC.cpp
@@ -0,0 +1,3 @@
+/**
+ * @brief Disturbance-rejeccted Impedance Predictive Control (DRIMPC)
+ */
\ No newline at end of file
diff --git a/flake.lock b/flake.lock
index 63325e36a0e8d20f8482e34facdf03d65ab64e20..d4b0cf3054d1550d8b38b8f7479d60ab5140f4ed 100644
--- a/flake.lock
+++ b/flake.lock
@@ -138,11 +138,11 @@
},
"nixpkgs_2411": {
"locked": {
- "lastModified": 1737672001,
- "narHash": "sha256-YnHJJ19wqmibLQdUeq9xzE6CjrMA568KN/lFPuSVs4I=",
+ "lastModified": 1738843498,
+ "narHash": "sha256-7x+Q4xgFj9UxZZO9aUDCR8h4vyYut4zPUvfj3i+jBHE=",
"owner": "nixos",
"repo": "nixpkgs",
- "rev": "035f8c0853c2977b24ffc4d0a42c74f00b182cd8",
+ "rev": "f5a32fa27df91dfc4b762671a0e0a859a8a0058f",
"type": "github"
},
"original": {
diff --git a/flake.nix b/flake.nix
index 4ffcdd98fd4196dda84210f113b41c6ebd240e58..7a14afb18648772fa7152cb20aff45a296286044 100644
--- a/flake.nix
+++ b/flake.nix
@@ -12,10 +12,28 @@
outputs = inputs@{ self, ... }:
let
system = "x86_64-linux";
-
+
+ franka-overrides = final: prev: {
+ rosPackages = prev.rosPackages // {
+ noetic = prev.rosPackages.noetic.overrideScope( finalPkg: prevPkg: {
+ libfranka = prevPkg.libfranka.overrideAttrs (old: rec {
+ version = "0.11.0";
+ src = prev.fetchFromGitHub {
+ owner = "frankaemika";
+ repo = "libfranka";
+ rev = version;
+ fetchSubmodules=true;
+ sha256 = "sha256-ugzXWI4ss9+mwWoHR+0T22LZ+SKviJJRQEIDSDfbe/k=";
+ };
+ });
+ });
+ };
+ };
+
overlays = [
inputs.nix-ros-overlay.overlays.default
inputs.nixgl.overlay
+ franka-overrides
];
pkgs = import inputs.nixpkgs {
@@ -27,7 +45,7 @@
};
rospkgs = pkgs.rosPackages.noetic;
-
+
rosPackages = with rospkgs; [
ros-core
catkin # for builds, probably not yet needed
@@ -39,27 +57,38 @@
joy
joy-teleop
+ interactive-markers
+ spacenav-node
+ rqt-reconfigure
+ dynamic-reconfigure
+ rqt-plot
+ # rviz # adding this induces nix develop error
+ # python-qt-binding
+ # qt-gui-core
+ # py-binding-tools
+
## for impedance controller
- franka-example-controllers
- #franka-hw
- #combined-robot-hw
- #joint-limits-interface
- #controller-interface
- #actionlib
+ # franka-example-controllers
+ # franka-hw
+ # combined-robot-hw
+ # joint-limits-interface
+ # controller-interface
+ # actionlib
## standard
- #control-msgs # needed for JointTrajInterface
- #moveit-msgs
+ # control-msgs # needed for JointTrajInterface
+ # moveit-msgs
franka-msgs
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 {};
+ fr3-impedance-controller = rospkgs.callPackage ./fr3_impedance_controller_real {
+ inherit common fr3-custom-msgs;
+ };
+
projectPackages = [
common
@@ -72,6 +101,7 @@
pkgs.glfw
pkgs.nixgl.nixGLIntel
];
+
in
{
devShells.${system} = {
@@ -83,6 +113,8 @@
#pkgs.python3Packages.pincas
pkgs_2411.casadi
pkgs_2411.pinocchio
+ pkgs.python311Packages.pyqt5 # for rqt_reconfigure
+ # pkgs.python311Packages.pandas
];
})
];
diff --git a/fr3_custom_msgs/CMakeLists.txt b/fr3_custom_msgs/CMakeLists.txt
index 0b32be8b651e169e8dc70d53567dc3f19892ee27..ce4e40edf6c11fb7c048154f46f5284277045aa5 100644
--- a/fr3_custom_msgs/CMakeLists.txt
+++ b/fr3_custom_msgs/CMakeLists.txt
@@ -2,16 +2,17 @@ cmake_minimum_required(VERSION 3.10)
project(fr3_custom_msgs)
find_package(catkin REQUIRED COMPONENTS
+ std_srvs
std_msgs
sensor_msgs
geometry_msgs
message_generation
- std_srvs
)
# --- Generate messages in the 'msg' folder
add_message_files(
FILES
+ ControllerState.msg
MujocoCommand.msg
MujocoSensor.msg
)
@@ -20,16 +21,16 @@ add_message_files(
# !! generate_messages() must be called before catkin_package() !!
generate_messages(
DEPENDENCIES
+ std_srvs
std_msgs
sensor_msgs
geometry_msgs
- std_srvs
)
catkin_package(CATKIN_DEPENDS
message_runtime
+ std_srvs
std_msgs
sensor_msgs
geometry_msgs
- std_srvs
)
\ No newline at end of file
diff --git a/fr3_custom_msgs/msg/ControllerState.msg b/fr3_custom_msgs/msg/ControllerState.msg
new file mode 100644
index 0000000000000000000000000000000000000000..36a6859bb1679838538548b50d4f180a01aeab16
--- /dev/null
+++ b/fr3_custom_msgs/msg/ControllerState.msg
@@ -0,0 +1,18 @@
+std_msgs/Header header
+float64[7] ddq_eul # joint accel by Euler method
+float64[7] ddq_mob # ........... estimation by MOB
+float64[7] ddq_sosml # ........... estimation by SOSML
+float64[7] tau_dist_dob # joint torque disturbance estimation by DOB
+float64[7] tau_dist_mob # ................................... by MOB
+float64[7] tau_dist_sosml # ................................... by SOSML
+geometry_msgs/Wrench F_dist_franka # wrench disturbance estimation from franka (world frame)
+geometry_msgs/Wrench F_dist_dob # ............................. by DOB
+geometry_msgs/Wrench F_dist_mob # ............................. by MOB
+geometry_msgs/Wrench F_dist_sosml # ............................. by SOSML
+geometry_msgs/Pose ee_pose_world_des # end-effector pose reference (world frame)
+geometry_msgs/Pose ee_pose_world_mes # ................. measurement
+geometry_msgs/Twist ee_vel_world_des # end-effector twist reference (world frame)
+geometry_msgs/Twist ee_vel_world_mes # .................. measurement
+geometry_msgs/Accel ee_acc_world_eul # task accel by Euler method (world frame)
+geometry_msgs/Accel ee_acc_world_mob # .......... estimation by MOB
+geometry_msgs/Accel ee_acc_world_sosml # .......... estimation by SOSML
diff --git a/fr3_impedance_controller_real/CMakeLists.txt b/fr3_impedance_controller_real/CMakeLists.txt
index 34e464adad3724270962fbf824cb520014d94e51..534c72f95b20ef452b68f6108bb18f4f77f3cb9d 100644
--- a/fr3_impedance_controller_real/CMakeLists.txt
+++ b/fr3_impedance_controller_real/CMakeLists.txt
@@ -1,8 +1,7 @@
cmake_minimum_required(VERSION 3.4)
-project(fr3_impedance_controller_real)
+project(franka_example_controllers)
set(CMAKE_BUILD_TYPE Release)
-# set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -12,7 +11,6 @@ find_package(catkin REQUIRED COMPONENTS
eigen_conversions
franka_hw
franka_gripper
- franka_example_controllers
geometry_msgs
hardware_interface
joint_limits_interface
@@ -25,23 +23,28 @@ find_package(catkin REQUIRED COMPONENTS
rospy
urdf
visualization_msgs
- sensor_msgs
)
+# --- Add dependencies
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
-#)
+# @Jeong : This should be included, otherwise, linking error happen (2025.02.07)
+find_package(pinocchio REQUIRED)
+find_package(common REQUIRED)
+find_package(fr3_custom_msgs REQUIRED)
+
+# add_message_files(FILES
+# JointTorqueComparison.msg
+# )
generate_messages()
generate_dynamic_reconfigure_options(
- cfg/compliance_param.cfg
+ cfg/controller_param.cfg
)
catkin_package(
@@ -53,7 +56,6 @@ catkin_package(
eigen_conversions
franka_hw
franka_gripper
- franka_example_controllers
geometry_msgs
hardware_interface
joint_limits_interface
@@ -65,15 +67,14 @@ catkin_package(
roscpp
urdf
visualization_msgs
- sensor_msgs
DEPENDS Franka
)
-add_library(${PROJECT_NAME}
- src/fr3_impedance_controller_real.cpp
+add_library(franka_example_controllers
+ src/cartesian_impedance_example_controller.cpp
)
-add_dependencies(${PROJECT_NAME}
+add_dependencies(franka_example_controllers
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
${PROJECT_NAME}_generate_messages_cpp
@@ -81,24 +82,45 @@ add_dependencies(${PROJECT_NAME}
${PROJECT_NAME}_gencfg
)
-target_link_libraries(${PROJECT_NAME} PUBLIC
+target_link_libraries(franka_example_controllers PUBLIC
${Franka_LIBRARIES}
${catkin_LIBRARIES}
+ # @Jeong : This should be included, otherwise, linking error happen (2025.02.07)
+ pinocchio::pinocchio
+ common::common
)
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
+target_include_directories(franka_example_controllers SYSTEM PUBLIC
${Franka_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
-target_include_directories(${PROJECT_NAME} PUBLIC
+
+target_include_directories(franka_example_controllers PUBLIC
include
- ${CMAKE_CURRENT_SOURCE_DIR}/../common/utilities/include
- ${CMAKE_CURRENT_SOURCE_DIR}/../common/math/include
)
+## @Jeongwoo : comment following lines cause they looks not necessary right now
+# add_executable(teleop_gripper_node
+# src/teleop_gripper_node.cpp
+# )
+# target_include_directories(teleop_gripper_node PUBLIC
+# ${catkin_INCLUDE_DIRS}
+# )
+# target_link_libraries(teleop_gripper_node PUBLIC
+# ${Franka_LIBRARIES}
+# ${catkin_LIBRARIES}
+# )
+# add_dependencies(teleop_gripper_node
+# ${${PROJECT_NAME}_EXPORTED_TARGETS}
+# ${catkin_EXPORTED_TARGETS}
+# ${PROJECT_NAME}_generate_messages_cpp
+# ${PROJECT_NAME}_gencpp
+# ${PROJECT_NAME}_gencfg
+# )
+
## Installation
-install(TARGETS ${PROJECT_NAME}
+install(TARGETS franka_example_controllers
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -112,10 +134,16 @@ install(DIRECTORY launch
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
-install(FILES fr3_impedance_controller_real_plugin.xml
+install(FILES franka_example_controllers_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
+## @Jeong : comment following lines cause these scripts are not necessary for this project
+# catkin_install_python(
+# PROGRAMS scripts/interactive_marker.py scripts/move_to_start.py scripts/dual_arm_interactive_marker.py
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
## Tools
include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
RESULT_VARIABLE CLANG_TOOLS
@@ -125,8 +153,6 @@ if(CLANG_TOOLS)
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
@@ -138,7 +164,8 @@ 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()
+## @Jeong : comment following lines cause these scripts are not necessary for this project
+# if(PEP_TOOLS)
+# file(GLOB_RECURSE PYSOURCES ${CMAKE_CURRENT_SOURCE_DIR}/scripts/*.py)
+# add_pyformat_target(franka_control FILES ${PYSOURCES})
+# endif()
diff --git a/fr3_impedance_controller_real/bak/CMakeLists_bak.txt b/fr3_impedance_controller_real/bak/CMakeLists_bak.txt
new file mode 100644
index 0000000000000000000000000000000000000000..34e464adad3724270962fbf824cb520014d94e51
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/CMakeLists_bak.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/fr3_impedance_controller_real/bak/cartesian_impedance_mpc.launch b/fr3_impedance_controller_real/bak/cartesian_impedance_mpc.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a11b1721c730436099ae15eec180eff7f5533c8e
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_impedance_mpc.launch
@@ -0,0 +1,16 @@
+<?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="0.03" />
+ <arg name="angular_gain" default="0.05" />
+
+ <include file="$(find franka_control)/launch/franka_control.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="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
+</launch>
diff --git a/fr3_impedance_controller_real/bak/cartesian_pose_controller.cpp b/fr3_impedance_controller_real/bak/cartesian_pose_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a5a3f6a9571505f09cafb1ee66a030a7b4dd282a
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_pose_controller.cpp
@@ -0,0 +1,198 @@
+// 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_pose_controller.h>
+
+#include <cmath>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include <controller_interface/controller_base.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+#include <geometry_msgs/PoseArray.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+
+namespace franka_example_controllers {
+
+bool CartesianPoseController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
+ if (cartesian_pose_interface_ == nullptr) {
+ ROS_ERROR(
+ "CartesianPoseExampleController: Could not get Cartesian Pose "
+ "interface from hardware");
+ return false;
+ }
+
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ try {
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface_->getHandle(arm_id + "_robot"));
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("CartesianPoseExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ // try {
+ // auto state_handle = state_interface->getHandle(arm_id + "_robot");
+
+ // std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ // for (size_t i = 0; i < q_start.size(); i++) {
+ // if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ // ROS_ERROR_STREAM(
+ // "CartesianPoseExampleController: Robot is not in the expected starting position for "
+ // "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
+ // "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ // return false;
+ // }
+ // }
+ // } catch (const hardware_interface::HardwareInterfaceException& e) {
+ // ROS_ERROR_STREAM(
+ // "CartesianPoseExampleController: Exception getting state handle: " << e.what());
+ // return false;
+ // }
+
+ single_pos_subscriber = node_handle.subscribe(
+ "/diffusion_policy_cartesian_pose", 1,
+ &CartesianPoseController::singlePosCallback, this);
+
+ return true;
+}
+
+void CartesianPoseController::starting(const ros::Time& /* time */) {
+ initial_pose = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ std::cout << "initial_pose = [" << initial_pose[12] << ", " << initial_pose[13] << ", " << initial_pose[14] << "]" << std::endl;
+}
+
+void CartesianPoseController::singlePosCallback(const geometry_msgs::Pose::ConstPtr& msg) {
+ new_pose = *msg;
+ is_single_pos_received = true;
+ last_msg_time = ros::Time::now();
+}
+
+
+//1000Hz
+void CartesianPoseController::update(const ros::Time& time ,
+ const ros::Duration& period) {
+ if (is_single_pos_received && (ros::Time::now() - last_msg_time).toSec() > 0.2) {
+ is_single_pos_received = false;
+ current_pose_tcp = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ }
+
+ if (is_single_pos_received) {
+ current_pose_tcp = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ std::cout << "current_pose_tcp = [" << current_pose_tcp[12] << ", " << current_pose_tcp[13] << ", " << current_pose_tcp[14] << "]" << std::endl;
+ std::cout << "new_pose = [" << new_pose.position.x << ", " << new_pose.position.y << ", " << new_pose.position.z << "]" << std::endl;
+
+ // calculate current velocity, acceleration and jerk
+ std::array<double, 3> current_vel = {(new_pose.position.x - current_pose_tcp[12]) / traj_step_time_,
+ (new_pose.position.y - current_pose_tcp[13]) / traj_step_time_,
+ (new_pose.position.z - current_pose_tcp[14]) / traj_step_time_,};
+ std::array<double, 3> current_acc = {(current_vel[0] - old_vel[0]) / traj_step_time_,
+ (current_vel[1] - old_vel[1]) / traj_step_time_,
+ (current_vel[2] - old_vel[2]) / traj_step_time_,};
+ std::array<double, 3> current_jerk = {(current_acc[0] - old_acc[0]) / traj_step_time_,
+ (current_acc[1] - old_acc[1]) / traj_step_time_,
+ (current_acc[2] - old_acc[2]) / traj_step_time_,};
+
+ //jerk check
+ double jerk = sqrt(pow(current_jerk[0], 2) + pow(current_jerk[1], 2) + pow(current_jerk[2], 2));
+ int sign = 0;
+ if (fabs(jerk) > MAX_JERK) {
+ ROS_WARN_THROTTLE(1.0, "Jerk exceeds the limit.");
+ for (int i = 0; i <3; i++) {
+ if (fabs(current_jerk[i]) > MAX_JERK) {
+ sign = (current_jerk[i] > 0) ? 1 : -1;
+ current_acc[i] = old_acc[i] + sign * MAX_JERK * traj_step_time_;
+ current_vel[i] = old_vel[i] + current_acc[i] * traj_step_time_;
+ }
+ }
+ }
+
+ // acceleration check
+ double acc = sqrt(pow(current_acc[0], 2) + pow(current_acc[1], 2) + pow(current_acc[2], 2));
+ if(fabs(acc) > MAX_ACC) {
+ ROS_WARN_THROTTLE(1.0, "Acceleration exceeds the limit.");
+ for(int i = 0; i < 3; i++) {
+ if (fabs(current_acc[i]) > MAX_ACC) {
+ sign = (current_acc[i] > 0) ? 1 : -1;
+ current_vel[i] = old_vel[i] + sign * MAX_ACC * traj_step_time_;
+ }
+ }
+ }
+
+ // velocity check
+ double vel = sqrt(pow(current_vel[0], 2) + pow(current_vel[1], 2) + pow(current_vel[2], 2));
+ if(vel > MAX_VEL) {
+ ROS_WARN_THROTTLE(1.0, "Velocity exceeds the limit.");
+ for(int i = 0; i < 3; i++) {
+ if(fabs(current_vel[i]) > MAX_VEL) {
+ double sign = (current_vel[i] > 0) ? 1 : -1;
+ if(i == 0)
+ {
+ new_pose.position.x = current_pose_tcp[12] + sign * MAX_VEL * traj_step_time_;
+ }
+ else if (i == 1)
+ {
+ new_pose.position.y = current_pose_tcp[13] + sign * MAX_VEL * traj_step_time_;
+ }
+ else // i == 2
+ {
+ new_pose.position.z = current_pose_tcp[14] + sign * MAX_VEL * traj_step_time_;
+ }
+ }
+ }
+ }
+ else {
+ // update new pose with (limited) velocity
+ new_pose.position.x = current_pose_tcp[12] + current_vel[0] * traj_step_time_;
+ new_pose.position.y = current_pose_tcp[13] + current_vel[1] * traj_step_time_;
+ new_pose.position.z = current_pose_tcp[14] + current_vel[2] * traj_step_time_;
+ }
+
+ double dx = (new_pose.position.x - current_pose_tcp[12]) * update_step_size_;
+ double dy = (new_pose.position.y - current_pose_tcp[13]) * update_step_size_;
+ double dz = (new_pose.position.z - current_pose_tcp[14]) * update_step_size_;
+
+ // set new pose
+ current_pose_tcp[12] += dx;
+ current_pose_tcp[13] += dy;
+ current_pose_tcp[14] += dz;
+ // print current_pose_
+ // ROS_INFO("current_pose_ = [%f, %f, %f]", current_pose_[12], current_pose_[13], current_pose_[14]);
+ cartesian_pose_handle_->setCommand(current_pose_tcp);
+
+ // update old values
+ old_vel = current_vel;
+ old_acc = current_acc;
+
+ COUNT += 1;
+ std::cout << "COUNT = " << COUNT << std::endl;
+ }
+ else {
+ ROS_WARN_THROTTLE(1.0, "No single pose msg received.");
+ current_pose_tcp = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ cartesian_pose_handle_->setCommand(current_pose_tcp);
+ }
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianPoseController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/cartesian_pose_controller.h b/fr3_impedance_controller_real/bak/cartesian_pose_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c270bde1c81506125e8fc5cbb86e114b0303368
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_pose_controller.h
@@ -0,0 +1,54 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <array>
+#include <memory>
+#include <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <geometry_msgs/PoseArray.h>
+
+namespace franka_example_controllers {
+
+class CartesianPoseController
+ : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+ private:
+ franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
+ std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
+ double elapsed_time;
+ std::array<double, 16> initial_pose{};
+ std::array<double, 16> current_pose_tcp{};
+
+ std::array<double, 3> old_vel{0.0, 0.0, 0.0};
+ std::array<double, 3> old_acc{0.0, 0.0, 0.0};
+
+ const double traj_step_time_ = 0.1;
+ const double update_step_size_ = 0.001;
+ const double MAX_VEL = 1.7; //1.7
+ const double MAX_ACC = 6; //13
+ const double MAX_JERK = 500.0; //6500
+
+ int COUNT = 0;
+
+ ros::Subscriber single_pos_subscriber;
+ void singlePosCallback(const geometry_msgs::Pose::ConstPtr& msg);
+ geometry_msgs::Pose new_pose;
+ bool is_single_pos_received = false;
+ ros::Time last_msg_time;
+
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/cartesian_pose_controller.launch b/fr3_impedance_controller_real/bak/cartesian_pose_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..dc62a60d56c98f762be1a1026134d010e245160c
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_pose_controller.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
+ <arg name="arm_id" default="$(arg robot)" />
+ <arg name="linear_gain" default="0.03" />
+ <arg name="angular_gain" default="0.05" />
+ <arg name="load_gripper" default="true" />
+ <include file="$(find franka_control)/launch/franka_control.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_pose_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/cartesian_pose_example_controller.cpp b/fr3_impedance_controller_real/bak/cartesian_pose_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a916ed629abbe55c16a58b6bd4abec0ae16024a1
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_pose_example_controller.cpp
@@ -0,0 +1,93 @@
+// 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_pose_example_controller.h>
+
+#include <cmath>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include <controller_interface/controller_base.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+namespace franka_example_controllers {
+
+bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
+ if (cartesian_pose_interface_ == nullptr) {
+ ROS_ERROR(
+ "CartesianPoseExampleController: Could not get Cartesian Pose "
+ "interface from hardware");
+ return false;
+ }
+
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ try {
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface_->getHandle(arm_id + "_robot"));
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("CartesianPoseExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ try {
+ auto state_handle = state_interface->getHandle(arm_id + "_robot");
+
+ std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "CartesianPoseExampleController: Robot is not in the expected starting position for "
+ "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
+ "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ return false;
+ }
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianPoseExampleController: Exception getting state handle: " << e.what());
+ return false;
+ }
+
+ return true;
+}
+
+void CartesianPoseExampleController::starting(const ros::Time& /* time */) {
+ initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ elapsed_time_ = ros::Duration(0.0);
+}
+
+void CartesianPoseExampleController::update(const ros::Time& /* time */,
+ const ros::Duration& period) {
+ elapsed_time_ += period;
+
+ double radius = 0.15;
+ double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * elapsed_time_.toSec()));
+ double delta_x = radius * std::sin(angle);
+ double delta_z = radius * (std::cos(angle) - 1);
+ std::array<double, 16> new_pose = initial_pose_;
+ new_pose[12] -= delta_x;
+ new_pose[14] -= delta_z;
+ cartesian_pose_handle_->setCommand(new_pose);
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianPoseExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/cartesian_pose_example_controller.h b/fr3_impedance_controller_real/bak/cartesian_pose_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..e712c7a88c301a531ac214ecb09ec5ad7ccab831
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_pose_example_controller.h
@@ -0,0 +1,34 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <array>
+#include <memory>
+#include <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_cartesian_command_interface.h>
+
+namespace franka_example_controllers {
+
+class CartesianPoseExampleController
+ : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+ private:
+ franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
+ std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
+ ros::Duration elapsed_time_;
+ std::array<double, 16> initial_pose_{};
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/cartesian_pose_example_controller.launch b/fr3_impedance_controller_real/bak/cartesian_pose_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a619d53bd7183ad7ad75e3843c0d51dab0afe2ab
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_pose_example_controller.launch
@@ -0,0 +1,9 @@
+<?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)" />
+ <include file="$(find franka_control)/launch/franka_control.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_pose_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/cartesian_sinpose_controller.cpp b/fr3_impedance_controller_real/bak/cartesian_sinpose_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0555c2201fce191d2d39d07691f42aa9c034dff1
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_sinpose_controller.cpp
@@ -0,0 +1,146 @@
+// Copyright (c) 2023 Franka Robotics GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/cartesian_sinpose_controller.h>
+
+#include <cmath>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include <controller_interface/controller_base.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+namespace franka_example_controllers
+{
+bool CartesianSinPoseExampleController::init(hardware_interface::RobotHW * robot_hardware,
+ ros::NodeHandle & node_handle)
+{
+ cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
+ if (cartesian_pose_interface_ == nullptr)
+ {
+ ROS_ERROR(
+ "CartesianSinPoseExampleController: Could not get Cartesian Pose "
+ "interface from hardware");
+ return false;
+ }
+
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id))
+ {
+ ROS_ERROR("CartesianSinPoseExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ try
+ {
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface_->getHandle(arm_id + "_robot"));
+ }
+ catch (const hardware_interface::HardwareInterfaceException & e)
+ {
+ ROS_ERROR_STREAM(
+ "CartesianSinPoseExampleController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr)
+ {
+ ROS_ERROR("CartesianSinPoseExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+// try
+// {
+// auto state_handle = state_interface->getHandle(arm_id + "_robot");
+
+// std::array<double, 7> q_start{ { 0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4 } };
+// for (size_t i = 0; i < q_start.size(); i++)
+// {
+// if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1)
+// {
+// ROS_ERROR_STREAM(
+// "CartesianSinPoseExampleController: Robot is not in the expected starting position for "
+// "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
+// "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+// return false;
+// }
+// }
+// }
+// catch (const hardware_interface::HardwareInterfaceException & e)
+// {
+// ROS_ERROR_STREAM(
+// "CartesianSinPoseExampleController: Exception getting state handle: " << e.what());
+// return false;
+// }
+
+ return true;
+}
+
+void CartesianSinPoseExampleController::starting(const ros::Time & /* time */)
+{
+ initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ elapsed_time_ = ros::Duration(0.0);
+}
+
+void CartesianSinPoseExampleController::update(const ros::Time & /* time */,
+ const ros::Duration & period)
+{
+ elapsed_time_ += period;
+
+ // double radius = 0.15;
+ // double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * elapsed_time_.toSec()));
+ // double delta_x = radius * std::sin(angle);
+ // double delta_z = radius * (std::cos(angle) - 1);
+ std::array<double, 16> new_pose = initial_pose_;
+ // new_pose[12] -= delta_x;
+ // new_pose[14] -= delta_z;
+
+ double Tp = 2.0; // period = 2 [sec]
+ double fp = 1 / Tp; // frequency = 0.5 [Hz]
+ double delta = 0.05; // displacement = 0.05 [m]
+
+ double t1 = 1.0;
+ double t2 = 6.0;
+ double t3 = 11.0;
+
+
+ if (elapsed_time_.toSec() >= t1 && elapsed_time_.toSec() < t1 + 4.0)
+ {
+ new_pose[12] =
+ initial_pose_[12] + 0.5 * delta * (1 - std::cos(2 * M_PI * fp * (elapsed_time_.toSec() - t1)));
+ }
+ else if (elapsed_time_.toSec() >= t1 + 4.0 && elapsed_time_.toSec() < t2)
+ {
+ new_pose = initial_pose_;
+ }
+ else if (elapsed_time_.toSec() >= t2 && elapsed_time_.toSec() < t2 + 4.0)
+ {
+ new_pose[13] =
+ initial_pose_[13] + 0.5 * delta * (1 - std::cos(2 * M_PI * fp * (elapsed_time_.toSec() - t2)));
+ }
+ else if (elapsed_time_.toSec() >= t2 + 4.0 && elapsed_time_.toSec() < t3)
+ {
+ new_pose = initial_pose_;
+ }
+ else if (elapsed_time_.toSec() >= t3 && elapsed_time_.toSec() < t3 + 4.0)
+ {
+ new_pose[14] =
+ initial_pose_[14] + 0.5 * delta * (1 - std::cos(2 * M_PI * fp * (elapsed_time_.toSec() - t3)));
+ }
+ else
+ {
+ new_pose = initial_pose_;
+ }
+ std::cout << "px: " << new_pose[12] << " py: " << new_pose[13] << " pz: " << new_pose[14] << std::endl;
+
+ cartesian_pose_handle_->setCommand(new_pose);
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianSinPoseExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/cartesian_sinpose_controller.h b/fr3_impedance_controller_real/bak/cartesian_sinpose_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..559ccd783014cb3d7364c7ad4f6b78a8c7f391bb
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_sinpose_controller.h
@@ -0,0 +1,35 @@
+// Copyright (c) 2023 Franka Robotics GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <array>
+#include <memory>
+#include <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_cartesian_command_interface.h>
+
+namespace franka_example_controllers
+{
+class CartesianSinPoseExampleController :
+ public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
+ franka_hw::FrankaStateInterface>
+{
+public:
+ bool init(hardware_interface::RobotHW * robot_hardware, ros::NodeHandle & node_handle) override;
+ void starting(const ros::Time &) override;
+ void update(const ros::Time &, const ros::Duration & period) override;
+
+private:
+ franka_hw::FrankaPoseCartesianInterface * cartesian_pose_interface_;
+ std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
+ ros::Duration elapsed_time_;
+ std::array<double, 16> initial_pose_{};
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/cartesian_sinpose_controller.launch b/fr3_impedance_controller_real/bak/cartesian_sinpose_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..1ff5cfb431a21caa9fde46c975f156f91ef9162a
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_sinpose_controller.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
+ <arg name="arm_id" default="$(arg robot)" />
+ <arg name="linear_gain" default="0.03" />
+ <arg name="angular_gain" default="0.05" />
+ <arg name="load_gripper" default="true" />
+ <include file="$(find franka_control)/launch/franka_control.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_sinpose_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/cartesian_traj_controller.cpp b/fr3_impedance_controller_real/bak/cartesian_traj_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d08b6c7078df46d2f1d404bc9970acaccce36840
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_traj_controller.cpp
@@ -0,0 +1,214 @@
+// 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_traj_controller.h>
+
+#include <cmath>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include <controller_interface/controller_base.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+#include <geometry_msgs/PoseArray.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+
+namespace franka_example_controllers {
+
+bool CartesianTrajController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
+ if (cartesian_pose_interface_ == nullptr) {
+ ROS_ERROR(
+ "CartesianPoseExampleController: Could not get Cartesian Pose "
+ "interface from hardware");
+ return false;
+ }
+
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ try {
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface_->getHandle(arm_id + "_robot"));
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("CartesianPoseExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ // Initialize the pose array subscriber
+ traj_subscriber = node_handle.subscribe(
+ "/diffusion_policy_cartesian_pose_traj", 1,
+ &CartesianTrajController::trajCallback, this);
+
+ return true;
+}
+
+void CartesianTrajController::starting(const ros::Time& /* time */) {
+ initial_pose = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ std::cout << "initial_pose = [" << initial_pose[12] << ", " << initial_pose[13] << ", " << initial_pose[14] << "]" << std::endl;
+}
+
+//10Hz
+void CartesianTrajController::trajCallback(const geometry_msgs::PoseArray::ConstPtr& msg) {
+ new_traj = *msg;
+ is_traj_received = true;
+ last_msg_time = ros::Time::now();
+}
+
+//1000Hz
+void CartesianTrajController::update(const ros::Time& time ,
+ const ros::Duration& period) {
+ if (is_traj_received && (ros::Time::now() - last_msg_time).toSec() > traj_step_time * 2) {
+ is_traj_received = false;
+ current_pose_tcp = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ ROS_WARN_THROTTLE(1.0, "PoseArray msg stopped streaming.");
+ }
+
+ if (is_traj_received) {
+ // if (fmod(COUNT * update_step_size, control_horizon * traj_step_time) == 0) {
+ // traj_to_control = new_traj;
+ // }
+ // traj_to_control = new_traj;
+
+
+ current_pose_tcp = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ std::cout << "current_pose_tcp = [" << current_pose_tcp[12] << ", " << current_pose_tcp[13] << ", " << current_pose_tcp[14] << "]" << std::endl;
+
+ // find the index of the closest pose in the trajectory and add a offset to it
+ std::vector<double> current_tcp_pos_vec;
+ current_tcp_pos_vec.assign(current_pose_tcp.begin()+12, current_pose_tcp.begin()+15);
+ std::vector<double> distances = calc_distances(new_traj, current_tcp_pos_vec);
+ auto min_value = std::min_element(distances.begin(), distances.end());
+ int min_index = std::distance(distances.begin(), min_value);
+ std::cout << "min_index = " << min_index << std::endl;
+ int control_index = std::min(min_index+control_horizon, static_cast<int>(new_traj.poses.size())-1);
+ std::cout << "control_index = " << control_index << std::endl;
+ new_pose = new_traj.poses[control_index];
+ std::cout << "new_pose = [" << new_pose.position.x << ", " << new_pose.position.y << ", " << new_pose.position.z << "]" << std::endl;
+ //calculate the distance between the current pose
+
+ // calculate current velocity, acceleration and jerk
+ std::array<double, 3> current_vel = {(new_pose.position.x - current_pose_tcp[12]) / traj_step_time,
+ (new_pose.position.y - current_pose_tcp[13]) / traj_step_time,
+ (new_pose.position.z - current_pose_tcp[14]) / traj_step_time,};
+ std::array<double, 3> current_acc = {(current_vel[0] - old_vel[0]) / traj_step_time,
+ (current_vel[1] - old_vel[1]) / traj_step_time,
+ (current_vel[2] - old_vel[2]) / traj_step_time,};
+ std::array<double, 3> current_jerk = {(current_acc[0] - old_acc[0]) / traj_step_time,
+ (current_acc[1] - old_acc[1]) / traj_step_time,
+ (current_acc[2] - old_acc[2]) / traj_step_time,};
+
+ //jerk check
+ double jerk = sqrt(pow(current_jerk[0], 2) + pow(current_jerk[1], 2) + pow(current_jerk[2], 2));
+ int sign = 0;
+ if (fabs(jerk) > MAX_JERK) {
+ ROS_WARN_THROTTLE(1.0, "Jerk exceeds the limit.");
+ for (int i = 0; i <3; i++) {
+ if (fabs(current_jerk[i]) > MAX_JERK) {
+ sign = (current_jerk[i] > 0) ? 1 : -1;
+ current_acc[i] = old_acc[i] + sign * MAX_JERK * traj_step_time;
+ current_vel[i] = old_vel[i] + current_acc[i] * traj_step_time;
+ }
+ }
+ }
+
+ // acceleration check
+ double acc = sqrt(pow(current_acc[0], 2) + pow(current_acc[1], 2) + pow(current_acc[2], 2));
+ if(fabs(acc) > MAX_ACC) {
+ ROS_WARN_THROTTLE(1.0, "Acceleration exceeds the limit.");
+ for(int i = 0; i < 3; i++) {
+ if (fabs(current_acc[i]) > MAX_ACC) {
+ sign = (current_acc[i] > 0) ? 1 : -1;
+ current_vel[i] = old_vel[i] + sign * MAX_ACC * traj_step_time;
+ }
+ }
+ }
+
+ // velocity check
+ double vel = sqrt(pow(current_vel[0], 2) + pow(current_vel[1], 2) + pow(current_vel[2], 2));
+ if(vel > MAX_VEL) {
+ ROS_WARN_THROTTLE(1.0, "Velocity exceeds the limit.");
+ for(int i = 0; i < 3; i++) {
+ if(fabs(current_vel[i]) > MAX_VEL) {
+ double sign = (current_vel[i] > 0) ? 1 : -1;
+ if(i == 0)
+ {
+ new_pose.position.x = current_pose_tcp[12] + sign * MAX_VEL * traj_step_time;
+ }
+ else if (i == 1)
+ {
+ new_pose.position.y = current_pose_tcp[13] + sign * MAX_VEL * traj_step_time;
+ }
+ else // i == 2
+ {
+ new_pose.position.z = current_pose_tcp[14] + sign * MAX_VEL * traj_step_time;
+ }
+ }
+ }
+ }
+ else {
+ // update new pose with (limited) velocity
+ new_pose.position.x = current_pose_tcp[12] + current_vel[0] * traj_step_time;
+ new_pose.position.y = current_pose_tcp[13] + current_vel[1] * traj_step_time;
+ new_pose.position.z = current_pose_tcp[14] + current_vel[2] * traj_step_time;
+ }
+
+ double dx = (new_pose.position.x - current_pose_tcp[12]) * update_step_size;
+ double dy = (new_pose.position.y - current_pose_tcp[13]) * update_step_size;
+ double dz = (new_pose.position.z - current_pose_tcp[14]) * update_step_size;
+ std::cout << "dx = " << dx << ", dy = " << dy << ", dz = " << dz << std::endl;
+
+ // set new pose
+ current_pose_tcp[12] += dx;
+ current_pose_tcp[13] += dy;
+ current_pose_tcp[14] += dz;
+ // print current_pose_
+ // ROS_INFO("current_pose_ = [%f, %f, %f]", current_pose_[12], current_pose_[13], current_pose_[14]);
+ cartesian_pose_handle_->setCommand(current_pose_tcp);
+
+ // update old values
+ old_vel = current_vel;
+ old_acc = current_acc;
+
+ COUNT += 1;
+ std::cout << "COUNT = " << COUNT << std::endl;
+ }
+ else {
+ ROS_WARN_THROTTLE(1.0, "No PoseArray msg received.");
+ current_pose_tcp = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ cartesian_pose_handle_->setCommand(current_pose_tcp);
+ }
+}
+
+// calculate the distances between the pose and all poses in a posearray
+std::vector<double> CartesianTrajController::calc_distances(const geometry_msgs::PoseArray& pose_array, const std::vector<double>& tcp_pos) {
+ // array to save the distances
+ std::vector<double> distances;
+ for (int i = 0; i < pose_array.poses.size(); i++) {
+ double dx = pose_array.poses[i].position.x - tcp_pos.at(0);
+ double dy = pose_array.poses[i].position.y - tcp_pos.at(1);
+ double dz = pose_array.poses[i].position.z - tcp_pos.at(2);
+ double distance = sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2));
+ distances.push_back(distance);
+ }
+ return distances;
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianTrajController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/cartesian_traj_controller.h b/fr3_impedance_controller_real/bak/cartesian_traj_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..0475b1120fdfbba89cef9efc18750909d5e5298e
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_traj_controller.h
@@ -0,0 +1,60 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <array>
+#include <memory>
+#include <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <geometry_msgs/PoseArray.h>
+
+namespace franka_example_controllers {
+
+class CartesianTrajController
+ : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+ private:
+ franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
+ std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
+ double elapsed_time;
+ std::array<double, 16> initial_pose{};
+ std::array<double, 16> current_pose_tcp{};
+
+ std::array<double, 3> old_vel{0.0, 0.0, 0.0};
+ std::array<double, 3> old_acc{0.0, 0.0, 0.0};
+
+ const double traj_step_time = 0.2;
+ const double update_step_size = 0.001;
+ const double MAX_VEL = 1.7; //1.7
+ const double MAX_ACC = 6; //13
+ const double MAX_JERK = 1000.0; //6500
+
+ int COUNT = 0;
+
+ ros::Subscriber traj_subscriber;
+ void trajCallback(const geometry_msgs::PoseArray::ConstPtr& msg);
+ geometry_msgs::PoseArray new_traj;
+ geometry_msgs::PoseArray traj_to_control;
+ geometry_msgs::Pose new_pose;
+ bool is_traj_received = false;
+ ros::Time last_msg_time;
+ double traj_start_time_;
+ int control_horizon = 7;
+
+ std::vector<double> calc_distances(const geometry_msgs::PoseArray& pose_array, const std::vector<double>& tcp_pos);
+
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/cartesian_traj_controller.launch b/fr3_impedance_controller_real/bak/cartesian_traj_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..599d25910274fd2f4c25189f7f3366b0803f306c
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_traj_controller.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
+ <arg name="arm_id" default="$(arg robot)" />
+ <arg name="linear_gain" default="0.03" />
+ <arg name="angular_gain" default="0.05" />
+ <arg name="load_gripper" default="true" />
+ <include file="$(find franka_control)/launch/franka_control.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_traj_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/cartesian_traj_twist_correct_controller.cpp b/fr3_impedance_controller_real/bak/cartesian_traj_twist_correct_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..42eb2aefea304475988e74945cb23c232d699af3
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_traj_twist_correct_controller.cpp
@@ -0,0 +1,226 @@
+// 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_traj_twist_correct_controller.h>
+
+#include <cmath>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include <controller_interface/controller_base.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+#include <geometry_msgs/PoseArray.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+
+namespace franka_example_controllers {
+
+bool CartesianTrajTwistCorrectController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
+ if (cartesian_pose_interface_ == nullptr) {
+ ROS_ERROR(
+ "CartesianPoseExampleController: Could not get Cartesian Pose "
+ "interface from hardware");
+ return false;
+ }
+
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ try {
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface_->getHandle(arm_id + "_robot"));
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("CartesianPoseExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ // Initialize the pose array subscriber
+ traj_subscriber = node_handle.subscribe(
+ "/diffusion_policy_cartesian_pose_traj", 1,
+ &CartesianTrajTwistCorrectController::trajCallback, this);
+
+ twist_subscriber = node_handle.subscribe("/spacenav/twist", 1, &CartesianTrajTwistCorrectController::twistCallback, this);
+
+ return true;
+}
+
+void CartesianTrajTwistCorrectController::starting(const ros::Time& /* time */) {
+ initial_pose = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ std::cout << "initial_pose = [" << initial_pose[12] << ", " << initial_pose[13] << ", " << initial_pose[14] << "]" << std::endl;
+}
+
+//10Hz
+void CartesianTrajTwistCorrectController::trajCallback(const geometry_msgs::PoseArray::ConstPtr& msg) {
+ new_traj = *msg;
+ is_traj_received = true;
+ last_msg_time = ros::Time::now();
+}
+
+// 30Hz
+void CartesianTrajTwistCorrectController::twistCallback(const geometry_msgs::TwistConstPtr& msg) {
+ // std::cout << "twist by joystick: " << msg->linear.x << ", " << msg->linear.y << ", " << msg->linear.z << std::endl; // 0.4 ~ 0.6
+ twist_command.linear.x = alpha * msg->linear.x;
+ twist_command.linear.y = alpha * msg->linear.y;
+ twist_command.linear.z = alpha * msg->linear.z;
+ twist_command.angular.x = 0; //angular_gain_ * msg->angular.x;
+ twist_command.angular.y = 0; //angular_gain_ * msg->angular.y;
+ twist_command.angular.z = 0; //angular_gain_ * msg->angular.z;
+}
+
+//1000Hz
+void CartesianTrajTwistCorrectController::update(const ros::Time& time ,
+ const ros::Duration& period) {
+ if (is_traj_received && (ros::Time::now() - last_msg_time).toSec() > traj_step_time * 2) {
+ is_traj_received = false;
+ current_pose_tcp = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ ROS_WARN_THROTTLE(1.0, "PoseArray msg stopped streaming.");
+ }
+
+ if (is_traj_received) {
+ current_pose_tcp = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ std::cout << "current_pose_tcp = [" << current_pose_tcp[12] << ", " << current_pose_tcp[13] << ", " << current_pose_tcp[14] << "]" << std::endl;
+
+ // find the index of the closest pose in the trajectory and add a offset to it
+ std::vector<double> current_tcp_pos_vec;
+ current_tcp_pos_vec.assign(current_pose_tcp.begin()+12, current_pose_tcp.begin()+15);
+ std::vector<double> distances = calc_distances(new_traj, current_tcp_pos_vec);
+ auto min_value = std::min_element(distances.begin(), distances.end());
+ int min_index = std::distance(distances.begin(), min_value);
+ std::cout << "min_index = " << min_index << std::endl;
+ int control_index = std::min(min_index+control_horizon, static_cast<int>(new_traj.poses.size())-1);
+ std::cout << "control_index = " << control_index << std::endl;
+ new_pose = new_traj.poses[control_index];
+ std::cout << "new_pose = [" << new_pose.position.x << ", " << new_pose.position.y << ", " << new_pose.position.z << "]" << std::endl;
+
+ //add the twist correction from twist command to the new_pose
+ // std::cout << "twist_command = [" << twist_command.linear.x << ", " << twist_command.linear.y << ", " << twist_command.linear.z << "]" << std::endl;
+
+ new_pose.position.x += twist_command.linear.x;
+ new_pose.position.y += twist_command.linear.y;
+ new_pose.position.z += twist_command.linear.z;
+
+ // calculate current velocity, acceleration and jerk
+ std::array<double, 3> current_vel = {(new_pose.position.x - current_pose_tcp[12]) / traj_step_time,
+ (new_pose.position.y - current_pose_tcp[13]) / traj_step_time,
+ (new_pose.position.z - current_pose_tcp[14]) / traj_step_time,};
+ std::array<double, 3> current_acc = {(current_vel[0] - old_vel[0]) / traj_step_time,
+ (current_vel[1] - old_vel[1]) / traj_step_time,
+ (current_vel[2] - old_vel[2]) / traj_step_time,};
+ std::array<double, 3> current_jerk = {(current_acc[0] - old_acc[0]) / traj_step_time,
+ (current_acc[1] - old_acc[1]) / traj_step_time,
+ (current_acc[2] - old_acc[2]) / traj_step_time,};
+
+ //jerk check
+ double jerk = sqrt(pow(current_jerk[0], 2) + pow(current_jerk[1], 2) + pow(current_jerk[2], 2));
+ int sign = 0;
+ if (fabs(jerk) > MAX_JERK) {
+ ROS_WARN_THROTTLE(1.0, "Jerk exceeds the limit.");
+ for (int i = 0; i <3; i++) {
+ if (fabs(current_jerk[i]) > MAX_JERK) {
+ sign = (current_jerk[i] > 0) ? 1 : -1;
+ current_acc[i] = old_acc[i] + sign * MAX_JERK * traj_step_time;
+ current_vel[i] = old_vel[i] + current_acc[i] * traj_step_time;
+ }
+ }
+ }
+
+ // acceleration check
+ double acc = sqrt(pow(current_acc[0], 2) + pow(current_acc[1], 2) + pow(current_acc[2], 2));
+ if(fabs(acc) > MAX_ACC) {
+ ROS_WARN_THROTTLE(1.0, "Acceleration exceeds the limit.");
+ for(int i = 0; i < 3; i++) {
+ if (fabs(current_acc[i]) > MAX_ACC) {
+ sign = (current_acc[i] > 0) ? 1 : -1;
+ current_vel[i] = old_vel[i] + sign * MAX_ACC * traj_step_time;
+ }
+ }
+ }
+
+ // velocity check
+ double vel = sqrt(pow(current_vel[0], 2) + pow(current_vel[1], 2) + pow(current_vel[2], 2));
+ if(vel > MAX_VEL) {
+ ROS_WARN_THROTTLE(1.0, "Velocity exceeds the limit.");
+ for(int i = 0; i < 3; i++) {
+ if(fabs(current_vel[i]) > MAX_VEL) {
+ double sign = (current_vel[i] > 0) ? 1 : -1;
+ if(i == 0)
+ {
+ new_pose.position.x = current_pose_tcp[12] + sign * MAX_VEL * traj_step_time;
+ }
+ else if (i == 1)
+ {
+ new_pose.position.y = current_pose_tcp[13] + sign * MAX_VEL * traj_step_time;
+ }
+ else // i == 2
+ {
+ new_pose.position.z = current_pose_tcp[14] + sign * MAX_VEL * traj_step_time;
+ }
+ }
+ }
+ }
+ else {
+ // update new pose with (limited) velocity
+ new_pose.position.x = current_pose_tcp[12] + current_vel[0] * traj_step_time;
+ new_pose.position.y = current_pose_tcp[13] + current_vel[1] * traj_step_time;
+ new_pose.position.z = current_pose_tcp[14] + current_vel[2] * traj_step_time;
+ }
+
+ double dx = (new_pose.position.x - current_pose_tcp[12]) * update_step_size;
+ double dy = (new_pose.position.y - current_pose_tcp[13]) * update_step_size;
+ double dz = (new_pose.position.z - current_pose_tcp[14]) * update_step_size;
+
+ // set new pose
+ current_pose_tcp[12] += dx;
+ current_pose_tcp[13] += dy;
+ current_pose_tcp[14] += dz;
+ // print current_pose_
+ // ROS_INFO("current_pose_ = [%f, %f, %f]", current_pose_[12], current_pose_[13], current_pose_[14]);
+ cartesian_pose_handle_->setCommand(current_pose_tcp);
+
+ // update old values
+ old_vel = current_vel;
+ old_acc = current_acc;
+
+ COUNT += 1;
+ std::cout << "COUNT = " << COUNT << std::endl;
+ }
+ else {
+ ROS_WARN_THROTTLE(1.0, "No PoseArray msg received.");
+ current_pose_tcp = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ cartesian_pose_handle_->setCommand(current_pose_tcp);
+ }
+}
+
+// calculate the distances between the pose and all poses in a posearray
+std::vector<double> CartesianTrajTwistCorrectController::calc_distances(const geometry_msgs::PoseArray& pose_array, const std::vector<double>& tcp_pos) {
+ // array to save the distances
+ std::vector<double> distances;
+ for (int i = 0; i < pose_array.poses.size(); i++) {
+ double dx = pose_array.poses[i].position.x - tcp_pos.at(0);
+ double dy = pose_array.poses[i].position.y - tcp_pos.at(1);
+ double dz = pose_array.poses[i].position.z - tcp_pos.at(2);
+ double distance = sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2));
+ distances.push_back(distance);
+ }
+ return distances;
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianTrajTwistCorrectController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/cartesian_traj_twist_correct_controller.h b/fr3_impedance_controller_real/bak/cartesian_traj_twist_correct_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..da612d9b7a8400ab644e301c3fc581820989cf01
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_traj_twist_correct_controller.h
@@ -0,0 +1,68 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <array>
+#include <memory>
+#include <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/Joy.h>
+
+namespace franka_example_controllers {
+
+class CartesianTrajTwistCorrectController
+ : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+
+ private:
+ franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
+ std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
+ double elapsed_time;
+ std::array<double, 16> initial_pose{};
+ std::array<double, 16> current_pose_tcp{};
+
+ std::array<double, 3> old_vel{0.0, 0.0, 0.0};
+ std::array<double, 3> old_acc{0.0, 0.0, 0.0};
+
+ const double alpha = 0.03; // gain for trajectory correction
+
+ const double traj_step_time = 0.2;
+ const double update_step_size = 0.001;
+ const double MAX_VEL = 1.7; //1.7
+ const double MAX_ACC = 6; //13
+ const double MAX_JERK = 1000.0; //6500
+
+ int COUNT = 0;
+
+ ros::Subscriber traj_subscriber;
+ ros::Subscriber twist_subscriber;
+ void trajCallback(const geometry_msgs::PoseArray::ConstPtr& msg);
+ void twistCallback(const geometry_msgs::TwistConstPtr& msg);
+
+ geometry_msgs::PoseArray new_traj;
+ geometry_msgs::Pose new_pose;
+ geometry_msgs::Twist twist_command;
+ bool is_traj_received = false;
+ ros::Time last_msg_time;
+ double traj_start_time_;
+ int control_horizon = 7;
+
+ std::vector<double> calc_distances(const geometry_msgs::PoseArray& pose_array, const std::vector<double>& tcp_pos);
+
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/cartesian_traj_twist_correct_controller.launch b/fr3_impedance_controller_real/bak/cartesian_traj_twist_correct_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..55668126b1f9bee2253dec25a426ca06b6ae57e0
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_traj_twist_correct_controller.launch
@@ -0,0 +1,13 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
+ <arg name="arm_id" default="$(arg robot)" />
+ <arg name="linear_gain" default="0.03" />
+ <arg name="angular_gain" default="0.05" />
+ <arg name="load_gripper" default="true" />
+ <include file="$(find spacenav_node)/launch/classic.launch" pass_all_args="true"/>
+ <include file="$(find franka_control)/launch/franka_control.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_traj_twist_correct_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/cartesian_velocity_example_controller.cpp b/fr3_impedance_controller_real/bak/cartesian_velocity_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..67c34a255b04180b5cb4cfb5d48c003b4637705e
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_velocity_example_controller.cpp
@@ -0,0 +1,101 @@
+// 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_velocity_example_controller.h>
+
+#include <array>
+#include <cmath>
+#include <memory>
+#include <string>
+
+#include <controller_interface/controller_base.h>
+#include <hardware_interface/hardware_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+namespace franka_example_controllers {
+
+bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("CartesianVelocityExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ velocity_cartesian_interface_ =
+ robot_hardware->get<franka_hw::FrankaVelocityCartesianInterface>();
+ if (velocity_cartesian_interface_ == nullptr) {
+ ROS_ERROR(
+ "CartesianVelocityExampleController: Could not get Cartesian velocity interface from "
+ "hardware");
+ return false;
+ }
+ try {
+ velocity_cartesian_handle_ = std::make_unique<franka_hw::FrankaCartesianVelocityHandle>(
+ velocity_cartesian_interface_->getHandle(arm_id + "_robot"));
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianVelocityExampleController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("CartesianVelocityExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ try {
+ auto state_handle = state_interface->getHandle(arm_id + "_robot");
+
+ std::array<double, 7> q_start = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "CartesianVelocityExampleController: Robot is not in the expected starting position "
+ "for running this example. Run `roslaunch franka_example_controllers "
+ "move_to_start.launch robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` "
+ "first.");
+ return false;
+ }
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianVelocityExampleController: Exception getting state handle: " << e.what());
+ return false;
+ }
+
+ return true;
+}
+
+void CartesianVelocityExampleController::starting(const ros::Time& /* time */) {
+ elapsed_time_ = ros::Duration(0.0);
+}
+
+void CartesianVelocityExampleController::update(const ros::Time& /* time */,
+ const ros::Duration& period) {
+ elapsed_time_ += period;
+
+ double time_max = 4.0;
+ double v_max = 0.05;
+ double angle = M_PI / 4.0;
+ double cycle = std::floor(
+ pow(-1.0, (elapsed_time_.toSec() - std::fmod(elapsed_time_.toSec(), time_max)) / time_max));
+ double v = cycle * v_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * elapsed_time_.toSec()));
+ double v_x = std::cos(angle) * v;
+ double v_z = -std::sin(angle) * v;
+ std::array<double, 6> command = {{v_x, 0.0, v_z, 0.0, 0.0, 0.0}};
+ velocity_cartesian_handle_->setCommand(command);
+}
+
+void CartesianVelocityExampleController::stopping(const ros::Time& /*time*/) {
+ // WARNING: DO NOT SEND ZERO VELOCITIES HERE AS IN CASE OF ABORTING DURING MOTION
+ // A JUMP TO ZERO WILL BE COMMANDED PUTTING HIGH LOADS ON THE ROBOT. LET THE DEFAULT
+ // BUILT-IN STOPPING BEHAVIOR SLOW DOWN THE ROBOT.
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianVelocityExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/cartesian_velocity_example_controller.h b/fr3_impedance_controller_real/bak/cartesian_velocity_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..95d4ace9a9e438d98e684fc1716b86487e793b17
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_velocity_example_controller.h
@@ -0,0 +1,32 @@
+// 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 <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <franka_hw/franka_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+namespace franka_example_controllers {
+
+class CartesianVelocityExampleController : public controller_interface::MultiInterfaceController<
+ franka_hw::FrankaVelocityCartesianInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+ void starting(const ros::Time&) override;
+ void stopping(const ros::Time&) override;
+
+ private:
+ franka_hw::FrankaVelocityCartesianInterface* velocity_cartesian_interface_;
+ std::unique_ptr<franka_hw::FrankaCartesianVelocityHandle> velocity_cartesian_handle_;
+ ros::Duration elapsed_time_;
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/cartesian_velocity_example_controller.launch b/fr3_impedance_controller_real/bak/cartesian_velocity_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..0eb440525daaa418f494eb36197b6e58c01cae43
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/cartesian_velocity_example_controller.launch
@@ -0,0 +1,9 @@
+<?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)" />
+ <include file="$(find franka_control)/launch/franka_control.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_velocity_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/cfg/compliance_param.cfg b/fr3_impedance_controller_real/bak/compliance_param.cfg
similarity index 100%
rename from fr3_impedance_controller_real/cfg/compliance_param.cfg
rename to fr3_impedance_controller_real/bak/compliance_param.cfg
diff --git a/fr3_impedance_controller_real/bak/desired_mass_param.cfg b/fr3_impedance_controller_real/bak/desired_mass_param.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..2fb2f0a73713aca800ce8d9c64479dc5b7288e6d
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/desired_mass_param.cfg
@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("desired_mass", double_t, 0, "desired mass for rendered force due to gravity applied in the z axis", 0.0, 0.0, 2.0)
+gen.add("k_p", double_t, 0, "force P gain", 0.0, 0.0, 2.0)
+gen.add("k_i", double_t, 0, "force I gain", 0.0, 0.0, 2.0)
+
+exit(gen.generate(PACKAGE, "dynamic_mass", "desired_mass_param"))
diff --git a/fr3_impedance_controller_real/bak/dual_arm_cartesian_impedance_example_controller.cpp b/fr3_impedance_controller_real/bak/dual_arm_cartesian_impedance_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2fe0af19b539d7daab825629957af888dab44fe0
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/dual_arm_cartesian_impedance_example_controller.cpp
@@ -0,0 +1,412 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/dual_arm_cartesian_impedance_example_controller.h>
+
+#include <cmath>
+#include <functional>
+#include <memory>
+
+#include <controller_interface/controller_base.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <franka/robot_state.h>
+#include <franka_example_controllers/pseudo_inversion.h>
+#include <franka_hw/trigger_rate.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+#include <ros/transport_hints.h>
+#include <tf/transform_listener.h>
+#include <tf_conversions/tf_eigen.h>
+
+namespace franka_example_controllers {
+
+bool DualArmCartesianImpedanceExampleController::initArm(
+ hardware_interface::RobotHW* robot_hw,
+ const std::string& arm_id,
+ const std::vector<std::string>& joint_names) {
+ FrankaDataContainer arm_data;
+ auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
+ if (model_interface == nullptr) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Error getting model interface from hardware");
+ return false;
+ }
+ try {
+ arm_data.model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+ model_interface->getHandle(arm_id + "_model"));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: 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(
+ "DualArmCartesianImpedanceExampleController: Error getting state interface from hardware");
+ return false;
+ }
+ try {
+ arm_data.state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+ state_interface->getHandle(arm_id + "_robot"));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: 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(
+ "DualArmCartesianImpedanceExampleController: Error getting effort joint interface from "
+ "hardware");
+ return false;
+ }
+ for (size_t i = 0; i < 7; ++i) {
+ try {
+ arm_data.joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
+ } catch (const hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Exception getting joint handles: "
+ << ex.what());
+ return false;
+ }
+ }
+
+ arm_data.position_d_.setZero();
+ arm_data.orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+ arm_data.position_d_target_.setZero();
+ arm_data.orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+
+ arm_data.cartesian_stiffness_.setZero();
+ arm_data.cartesian_damping_.setZero();
+
+ arms_data_.emplace(std::make_pair(arm_id, std::move(arm_data)));
+
+ return true;
+}
+
+bool DualArmCartesianImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle) {
+ std::vector<double> cartesian_stiffness_vector;
+ std::vector<double> cartesian_damping_vector;
+
+ if (!node_handle.getParam("left/arm_id", left_arm_id_)) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Could not read parameter left_arm_id_");
+ return false;
+ }
+ std::vector<std::string> left_joint_names;
+ if (!node_handle.getParam("left/joint_names", left_joint_names) || left_joint_names.size() != 7) {
+ ROS_ERROR(
+ "DualArmCartesianImpedanceExampleController: Invalid or no left_joint_names parameters "
+ "provided, "
+ "aborting controller init!");
+ return false;
+ }
+
+ if (!node_handle.getParam("right/arm_id", right_arm_id_)) {
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Could not read parameter right_arm_id_");
+ return false;
+ }
+
+ boost::function<void(const geometry_msgs::PoseStamped::ConstPtr&)> callback =
+ boost::bind(&DualArmCartesianImpedanceExampleController::targetPoseCallback, this, _1);
+
+ ros::SubscribeOptions subscribe_options;
+ subscribe_options.init("centering_frame_target_pose", 1, callback);
+ subscribe_options.transport_hints = ros::TransportHints().reliable().tcpNoDelay();
+ sub_target_pose_left_ = node_handle.subscribe(subscribe_options);
+
+ std::vector<std::string> right_joint_names;
+ if (!node_handle.getParam("right/joint_names", right_joint_names) ||
+ right_joint_names.size() != 7) {
+ ROS_ERROR(
+ "DualArmCartesianImpedanceExampleController: Invalid or no right_joint_names parameters "
+ "provided, "
+ "aborting controller init!");
+ return false;
+ }
+
+ bool left_success = initArm(robot_hw, left_arm_id_, left_joint_names);
+ bool right_success = initArm(robot_hw, right_arm_id_, right_joint_names);
+
+ dynamic_reconfigure_compliance_param_node_ =
+ ros::NodeHandle("dynamic_reconfigure_compliance_param_node");
+
+ dynamic_server_compliance_param_ = std::make_unique<dynamic_reconfigure::Server<
+ franka_combined_example_controllers::dual_arm_compliance_paramConfig>>(
+ dynamic_reconfigure_compliance_param_node_);
+
+ dynamic_server_compliance_param_->setCallback(boost::bind(
+ &DualArmCartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
+
+ // Get the transformation from right_O_frame to left_O_frame
+ tf::StampedTransform transform;
+ tf::TransformListener listener;
+ try {
+ if (listener.waitForTransform(left_arm_id_ + "_link0", right_arm_id_ + "_link0", ros::Time(0),
+ ros::Duration(4.0))) {
+ listener.lookupTransform(left_arm_id_ + "_link0", right_arm_id_ + "_link0", ros::Time(0),
+ transform);
+ } else {
+ ROS_ERROR(
+ "DualArmCartesianImpedanceExampleController: Failed to read transform from %s to %s. "
+ "Aborting init!",
+ (right_arm_id_ + "_link0").c_str(), (left_arm_id_ + "_link0").c_str());
+ return false;
+ }
+ } catch (tf::TransformException& ex) {
+ ROS_ERROR("DualArmCartesianImpedanceExampleController: %s", ex.what());
+ return false;
+ }
+ tf::transformTFToEigen(transform, Ol_T_Or_); // NOLINT (readability-identifier-naming)
+
+ // Setup publisher for the centering frame.
+ publish_rate_ = franka_hw::TriggerRate(30.0);
+ center_frame_pub_.init(node_handle, "centering_frame", 1, true);
+
+ return left_success && right_success;
+}
+
+void DualArmCartesianImpedanceExampleController::startingArm(FrankaDataContainer& arm_data) {
+ // compute initial velocity with jacobian and set x_attractor and q_d_nullspace
+ // to initial configuration
+ franka::RobotState initial_state = arm_data.state_handle_->getRobotState();
+ // get jacobian
+ std::array<double, 42> jacobian_array =
+ arm_data.model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+ // convert to eigen
+ Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> dq_initial(initial_state.dq.data());
+ 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 target point to current state
+ arm_data.position_d_ = initial_transform.translation();
+ arm_data.orientation_d_ = Eigen::Quaterniond(initial_transform.rotation());
+ arm_data.position_d_target_ = initial_transform.translation();
+ arm_data.orientation_d_target_ = Eigen::Quaterniond(initial_transform.rotation());
+
+ // set nullspace target configuration to initial q
+ arm_data.q_d_nullspace_ = q_initial;
+}
+
+void DualArmCartesianImpedanceExampleController::starting(const ros::Time& /*time*/) {
+ for (auto& arm_data : arms_data_) {
+ startingArm(arm_data.second);
+ }
+ franka::RobotState robot_state_right =
+ arms_data_.at(right_arm_id_).state_handle_->getRobotState();
+ franka::RobotState robot_state_left = arms_data_.at(left_arm_id_).state_handle_->getRobotState();
+ Eigen::Affine3d Ol_T_EEl(Eigen::Matrix4d::Map( // NOLINT (readability-identifier-naming)
+ robot_state_left.O_T_EE.data())); // NOLINT (readability-identifier-naming)
+ Eigen::Affine3d Or_T_EEr(Eigen::Matrix4d::Map( // NOLINT (readability-identifier-naming)
+ robot_state_right.O_T_EE.data())); // NOLINT (readability-identifier-naming)
+ EEr_T_EEl_ =
+ Or_T_EEr.inverse() * Ol_T_Or_.inverse() * Ol_T_EEl; // NOLINT (readability-identifier-naming)
+ EEl_T_C_.setIdentity();
+ Eigen::Vector3d EEr_r_EEr_EEl = // NOLINT (readability-identifier-naming)
+ EEr_T_EEl_.translation(); // NOLINT (readability-identifier-naming)
+ EEl_T_C_.translation() = -0.5 * EEr_T_EEl_.inverse().rotation() * EEr_r_EEr_EEl;
+}
+
+void DualArmCartesianImpedanceExampleController::update(const ros::Time& /*time*/,
+ const ros::Duration& /*period*/) {
+ for (auto& arm_data : arms_data_) {
+ updateArm(arm_data.second); // NOLINT
+ }
+ if (publish_rate_()) {
+ publishCenteringPose();
+ }
+}
+
+void DualArmCartesianImpedanceExampleController::updateArm(FrankaDataContainer& arm_data) {
+ // get state variables
+ franka::RobotState robot_state = arm_data.state_handle_->getRobotState();
+ std::array<double, 49> inertia_array = arm_data.model_handle_->getMass();
+ std::array<double, 7> coriolis_array = arm_data.model_handle_->getCoriolis();
+ std::array<double, 42> jacobian_array =
+ arm_data.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());
+
+ // compute error to desired pose
+ // position error
+ Eigen::Matrix<double, 6, 1> error;
+ error.head(3) << position - arm_data.position_d_;
+
+ // orientation error
+ if (arm_data.orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) {
+ orientation.coeffs() << -orientation.coeffs();
+ }
+ // "difference" quaternion
+ Eigen::Quaterniond error_quaternion(orientation * arm_data.orientation_d_.inverse());
+ // convert to axis angle
+ Eigen::AngleAxisd error_quaternion_angle_axis(error_quaternion);
+ // compute "orientation error"
+ error.tail(3) << error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle();
+
+ // compute control
+ // allocate variables
+ Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7);
+
+ // pseudoinverse for nullspace handling
+ // kinematic pseuoinverse
+ Eigen::MatrixXd jacobian_transpose_pinv;
+ franka_example_controllers::pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv);
+
+ // Cartesian PD control with damping ratio = 1
+ tau_task << jacobian.transpose() * (-arm_data.cartesian_stiffness_ * error -
+ arm_data.cartesian_damping_ * (jacobian * dq));
+ // nullspace PD control with damping ratio = 1
+ tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) -
+ jacobian.transpose() * jacobian_transpose_pinv) *
+ (arm_data.nullspace_stiffness_ * (arm_data.q_d_nullspace_ - q) -
+ (2.0 * sqrt(arm_data.nullspace_stiffness_)) * dq);
+ // Desired torque
+ tau_d << tau_task + tau_nullspace + coriolis;
+ // Saturate torque rate to avoid discontinuities
+ tau_d << saturateTorqueRate(arm_data, tau_d, tau_J_d);
+ for (size_t i = 0; i < 7; ++i) {
+ arm_data.joint_handles_[i].setCommand(tau_d(i));
+ }
+
+ // update parameters changed online either through dynamic reconfigure or through the interactive
+ // target by filtering
+ arm_data.cartesian_stiffness_ = arm_data.filter_params_ * arm_data.cartesian_stiffness_target_ +
+ (1.0 - arm_data.filter_params_) * arm_data.cartesian_stiffness_;
+ arm_data.cartesian_damping_ = arm_data.filter_params_ * arm_data.cartesian_damping_target_ +
+ (1.0 - arm_data.filter_params_) * arm_data.cartesian_damping_;
+ arm_data.nullspace_stiffness_ = arm_data.filter_params_ * arm_data.nullspace_stiffness_target_ +
+ (1.0 - arm_data.filter_params_) * arm_data.nullspace_stiffness_;
+ arm_data.position_d_ = arm_data.filter_params_ * arm_data.position_d_target_ +
+ (1.0 - arm_data.filter_params_) * arm_data.position_d_;
+ arm_data.orientation_d_ =
+ arm_data.orientation_d_.slerp(arm_data.filter_params_, arm_data.orientation_d_target_);
+}
+
+Eigen::Matrix<double, 7, 1> DualArmCartesianImpedanceExampleController::saturateTorqueRate(
+ const FrankaDataContainer& arm_data,
+ 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, arm_data.delta_tau_max_),
+ -arm_data.delta_tau_max_);
+ }
+ return tau_d_saturated;
+}
+
+void DualArmCartesianImpedanceExampleController::complianceParamCallback(
+ franka_combined_example_controllers::dual_arm_compliance_paramConfig& config,
+ uint32_t /*level*/) {
+ auto& left_arm_data = arms_data_.at(left_arm_id_);
+ left_arm_data.cartesian_stiffness_target_.setIdentity();
+ left_arm_data.cartesian_stiffness_target_.topLeftCorner(3, 3)
+ << config.left_translational_stiffness * Eigen::Matrix3d::Identity();
+ left_arm_data.cartesian_stiffness_target_.bottomRightCorner(3, 3)
+ << config.left_rotational_stiffness * Eigen::Matrix3d::Identity();
+ left_arm_data.cartesian_damping_target_.setIdentity();
+
+ left_arm_data.cartesian_damping_target_.topLeftCorner(3, 3)
+ << 2 * sqrt(config.left_translational_stiffness) * Eigen::Matrix3d::Identity();
+ left_arm_data.cartesian_damping_target_.bottomRightCorner(3, 3)
+ << 2 * sqrt(config.left_rotational_stiffness) * Eigen::Matrix3d::Identity();
+ left_arm_data.nullspace_stiffness_target_ = config.left_nullspace_stiffness;
+
+ auto& right_arm_data = arms_data_.at(right_arm_id_);
+ right_arm_data.cartesian_stiffness_target_.setIdentity();
+ right_arm_data.cartesian_stiffness_target_.topLeftCorner(3, 3)
+ << config.right_translational_stiffness * Eigen::Matrix3d::Identity();
+ right_arm_data.cartesian_stiffness_target_.bottomRightCorner(3, 3)
+ << config.right_rotational_stiffness * Eigen::Matrix3d::Identity();
+ right_arm_data.cartesian_damping_target_.setIdentity();
+
+ right_arm_data.cartesian_damping_target_.topLeftCorner(3, 3)
+ << 2 * sqrt(config.right_translational_stiffness) * Eigen::Matrix3d::Identity();
+ right_arm_data.cartesian_damping_target_.bottomRightCorner(3, 3)
+ << 2 * sqrt(config.right_rotational_stiffness) * Eigen::Matrix3d::Identity();
+ right_arm_data.nullspace_stiffness_target_ = config.right_nullspace_stiffness;
+}
+
+void DualArmCartesianImpedanceExampleController::targetPoseCallback(
+ const geometry_msgs::PoseStamped::ConstPtr& msg) {
+ try {
+ if (msg->header.frame_id != left_arm_id_ + "_link0") { // NOLINT
+ ROS_ERROR_STREAM(
+ "DualArmCartesianImpedanceExampleController: Got pose target with invalid"
+ " frame_id "
+ << msg->header.frame_id << ". Expected " << left_arm_id_ + "_link0");
+ return;
+ }
+
+ // Set target for the left robot.
+ auto& left_arm_data = arms_data_.at(left_arm_id_);
+ Eigen::Affine3d Ol_T_C{}; // NOLINT (readability-identifier-naming)
+ tf::poseMsgToEigen(msg->pose, Ol_T_C);
+ Eigen::Affine3d Ol_T_EEl_d = // NOLINT (readability-identifier-naming)
+ Ol_T_C * EEl_T_C_.inverse(); // NOLINT (readability-identifier-naming)
+ left_arm_data.position_d_target_ = Ol_T_EEl_d.translation();
+ Eigen::Quaterniond last_orientation_d_target(left_arm_data.orientation_d_target_);
+ Eigen::Quaterniond new_orientation_target(Ol_T_EEl_d.rotation());
+ if (last_orientation_d_target.coeffs().dot(new_orientation_target.coeffs()) < 0.0) {
+ new_orientation_target.coeffs() << -new_orientation_target.coeffs();
+ }
+ Ol_T_EEl_d.linear() = new_orientation_target.matrix();
+ left_arm_data.orientation_d_target_ = Ol_T_EEl_d.rotation();
+
+ // Compute target for the right endeffector given the static desired transform from left to
+ // right endeffector.
+ Eigen::Affine3d Or_T_EEr_d = Ol_T_Or_.inverse() // NOLINT (readability-identifier-naming)
+ * Ol_T_EEl_d * // NOLINT (readability-identifier-naming)
+ EEr_T_EEl_.inverse(); // NOLINT (readability-identifier-naming)
+ auto& right_arm_data = arms_data_.at(right_arm_id_);
+ right_arm_data.position_d_target_ =
+ Or_T_EEr_d.translation(); // NOLINT (readability-identifier-naming)
+ last_orientation_d_target = Eigen::Quaterniond(right_arm_data.orientation_d_target_);
+ right_arm_data.orientation_d_target_ =
+ Or_T_EEr_d.rotation(); // NOLINT (readability-identifier-naming)
+ if (last_orientation_d_target.coeffs().dot(right_arm_data.orientation_d_target_.coeffs()) <
+ 0.0) {
+ right_arm_data.orientation_d_target_.coeffs()
+ << -right_arm_data.orientation_d_target_.coeffs();
+ }
+
+ } catch (std::out_of_range& ex) {
+ ROS_ERROR_STREAM("DualArmCartesianImpedanceExampleController: Exception setting target poses.");
+ }
+}
+
+void DualArmCartesianImpedanceExampleController::publishCenteringPose() {
+ if (center_frame_pub_.trylock()) {
+ franka::RobotState robot_state_left =
+ arms_data_.at(left_arm_id_).state_handle_->getRobotState();
+ Eigen::Affine3d Ol_T_EEl(Eigen::Matrix4d::Map( // NOLINT (readability-identifier-naming)
+ robot_state_left.O_T_EE.data())); // NOLINT (readability-identifier-naming)
+ Eigen::Affine3d Ol_T_C = Ol_T_EEl * EEl_T_C_; // NOLINT (readability-identifier-naming)
+ tf::poseEigenToMsg(Ol_T_C, center_frame_pub_.msg_.pose);
+ center_frame_pub_.msg_.header.frame_id = left_arm_id_ + "_link0";
+ center_frame_pub_.unlockAndPublish();
+ }
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::DualArmCartesianImpedanceExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/dual_arm_cartesian_impedance_example_controller.h b/fr3_impedance_controller_real/bak/dual_arm_cartesian_impedance_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..4247a47aa09c88690446d508662d8c8bf1da9677
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/dual_arm_cartesian_impedance_example_controller.h
@@ -0,0 +1,182 @@
+// Copyright (c) 2019 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <memory>
+#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 <realtime_tools/realtime_publisher.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+#include <Eigen/Dense>
+
+#include <franka_example_controllers/dual_arm_compliance_paramConfig.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+#include <franka_hw/trigger_rate.h>
+
+namespace franka_example_controllers {
+
+/**
+ * This container holds all data and parameters used to control one panda arm with a Cartesian
+ * impedance control law tracking a desired target pose.
+ */
+struct FrankaDataContainer {
+ std::unique_ptr<franka_hw::FrankaStateHandle>
+ state_handle_; ///< To read to complete robot state.
+ std::unique_ptr<franka_hw::FrankaModelHandle>
+ model_handle_; ///< To have access to e.g. jacobians.
+ std::vector<hardware_interface::JointHandle> joint_handles_; ///< To command joint torques.
+ double filter_params_{0.005}; ///< [-] PT1-Filter constant to smooth target values set
+ ///< by dynamic reconfigure servers (stiffness/damping)
+ ///< or interactive markers for the target poses.
+ double nullspace_stiffness_{20.0}; ///< [Nm/rad] To track the initial joint configuration in
+ ///< the nullspace of the Cartesian motion.
+ double nullspace_stiffness_target_{20.0}; ///< [Nm/rad] Unfiltered raw value.
+ const double delta_tau_max_{1.0}; ///< [Nm/ms] Maximum difference in joint-torque per
+ ///< timestep. Used to saturated torque rates to ensure
+ ///< feasible commands.
+ Eigen::Matrix<double, 6, 6> cartesian_stiffness_; ///< To track the target pose.
+ Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_; ///< Unfiltered raw value.
+ Eigen::Matrix<double, 6, 6> cartesian_damping_; ///< To damp cartesian motions.
+ Eigen::Matrix<double, 6, 6> cartesian_damping_target_; ///< Unfiltered raw value.
+ Eigen::Matrix<double, 7, 1> q_d_nullspace_; ///< Target joint pose for nullspace
+ ///< motion. For now we track the
+ ///< initial joint pose.
+ Eigen::Vector3d position_d_; ///< Target position of the end effector.
+ Eigen::Quaterniond orientation_d_; ///< Target orientation of the end effector.
+ Eigen::Vector3d position_d_target_; ///< Unfiltered raw value.
+ Eigen::Quaterniond orientation_d_target_; ///< Unfiltered raw value.
+};
+
+/**
+ * Controller class for ros_control that renders two decoupled Cartesian impedances for the
+ * tracking of two target poses for the two endeffectors. The controller can be reparameterized at
+ * runtime via dynamic reconfigure servers.
+ */
+class DualArmCartesianImpedanceExampleController
+ : public controller_interface::MultiInterfaceController<
+ franka_hw::FrankaModelInterface,
+ hardware_interface::EffortJointInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ /**
+ * Initializes the controller class to be ready to run.
+ *
+ * @param[in] robot_hw Pointer to a RobotHW class to get interfaces and resource handles.
+ * @param[in] node_handle Nodehandle that allows getting parameterizations from the server and
+ * starting subscribers.
+ * @return True if the controller was initialized successfully, false otherwise.
+ */
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
+
+ /**
+ * Prepares the controller for the real-time execution. This method is executed once every time
+ * the controller is started and runs in real-time.
+ */
+ void starting(const ros::Time&) override;
+
+ /**
+ * Computes the control-law and commands the resulting joint torques to the robot.
+ *
+ * @param[in] period The control period (here 0.001s).
+ */
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+ private:
+ std::map<std::string, FrankaDataContainer>
+ arms_data_; ///< Holds all relevant data for both arms.
+ std::string left_arm_id_; ///< Name of the left arm, retrieved from the parameter server.
+ std::string right_arm_id_; ///< Name of the right arm, retrieved from the parameter server.
+
+ ///< Transformation between base frames of the robots.
+ Eigen::Affine3d Ol_T_Or_; // NOLINT (readability-identifier-naming)
+ ///< Target transformation between the two endeffectors.
+ Eigen::Affine3d EEr_T_EEl_; // NOLINT (readability-identifier-naming)
+ ///< Transformation from the centering frame to the left end effector.
+ Eigen::Affine3d EEl_T_C_{};
+
+ ///< Publisher for the centering tracking frame of the coordinated motion.
+ realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> center_frame_pub_;
+ ///< Rate to trigger publishing the current pose of the centering frame.
+ franka_hw::TriggerRate publish_rate_;
+
+ /**
+ * Saturates torque commands to ensure feasibility.
+ *
+ * @param[in] arm_data The data container of the arm.
+ * @param[in] tau_d_calculated The raw command according to the control law.
+ * @param[in] tau_J_d The current desired torque, read from the robot state.
+ * @return The saturated torque command for the 7 joints of one arm.
+ */
+ Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+ const FrankaDataContainer& arm_data,
+ const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+ const Eigen::Matrix<double, 7, 1>& tau_J_d); // NOLINT (readability-identifier-naming)
+
+ /**
+ * Initializes a single Panda robot arm.
+ *
+ * @param[in] robot_hw A pointer the RobotHW class for getting interfaces and resource handles.
+ * @param[in] arm_id The name of the panda arm.
+ * @param[in] joint_names The names of all joints of the panda.
+ * @return True if successful, false otherwise.
+ */
+ bool initArm(hardware_interface::RobotHW* robot_hw,
+ const std::string& arm_id,
+ const std::vector<std::string>& joint_names);
+
+ /**
+ * Computes the decoupled controller update for a single arm.
+ *
+ * @param[in] arm_data The data container of the arm to control.
+ */
+ void updateArm(FrankaDataContainer& arm_data);
+
+ /**
+ * Prepares all internal states to be ready to run the real-time control for one arm.
+ *
+ * @param[in] arm_data The data container of the arm to prepare for the control loop.
+ */
+ void startingArm(FrankaDataContainer& arm_data);
+
+ ///< Dynamic reconfigure server
+ std::unique_ptr<dynamic_reconfigure::Server<
+ franka_combined_example_controllers::dual_arm_compliance_paramConfig>>
+ dynamic_server_compliance_param_;
+
+ ///< Nodehandle for the dynamic reconfigure namespace
+ ros::NodeHandle dynamic_reconfigure_compliance_param_node_;
+
+ /**
+ * Callback for updates on the parameterization of the controller in terms of stiffnesses.
+ *
+ * @param[in] config Data container for configuration updates.
+ */
+ void complianceParamCallback(
+ franka_combined_example_controllers::dual_arm_compliance_paramConfig& config,
+ uint32_t /*level*/);
+
+ ///< Target pose subscriber
+ ros::Subscriber sub_target_pose_left_;
+
+ /**
+ * Callback method that handles updates of the target poses.
+ *
+ * @param[in] msg New target pose.
+ */
+ void targetPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
+
+ /**
+ * Publishes a Pose Stamped for visualization of the current centering pose.
+ */
+ void publishCenteringPose();
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/dual_arm_cartesian_impedance_example_controller.launch b/fr3_impedance_controller_real/bak/dual_arm_cartesian_impedance_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..8320e60106a220904ec223138455f649a16ace2b
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/dual_arm_cartesian_impedance_example_controller.launch
@@ -0,0 +1,23 @@
+<?xml version="1.0" ?>
+<launch>
+ <!--Be sure to pass the IPs of your pandas like robot_ips:="{panda_1/robot_ip: <my_ip_1>, panda_2/robot_ip: <my_ip_2>}" -->
+ <arg name="robot_ips" />
+
+ <arg name="robot_id" default="panda_dual" />
+ <arg name="rviz" default="true" />
+ <arg name="rqt" default="true" />
+
+ <include file="$(find franka_control)/launch/franka_combined_control.launch" >
+ <arg name="robot_id" value="$(arg robot_id)" />
+ <arg name="robot_ips" value="$(arg robot_ips)" />
+ </include>
+
+ <group ns="$(arg robot_id)">
+ <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="dual_arm_cartesian_impedance_example_controller"/>
+ <node name="interactive_marker_left" pkg="franka_example_controllers" type="dual_arm_interactive_marker.py"
+ args="--right_arm_id panda_1 --left_arm_id panda_2" required="false" output="screen" />
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" if="$(arg rqt)"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_dual_description_with_marker.rviz" if="$(arg rviz)"/>
+ </group>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/dual_arm_compliance_param.cfg b/fr3_impedance_controller_real/bak/dual_arm_compliance_param.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..ef80c4138f5093d77a42ab4b2a45070ff045545d
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/dual_arm_compliance_param.cfg
@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+PACKAGE = "franka_combined_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("left_translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("left_rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 10, 0, 30)
+gen.add("left_nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0, 0, 100)
+
+gen.add("right_translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("right_rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 10, 0, 30)
+gen.add("right_nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0, 0, 100)
+
+exit(gen.generate(PACKAGE, "dynamic_compliance", "dual_arm_compliance_param"))
diff --git a/fr3_impedance_controller_real/bak/elbow_example_controller.cpp b/fr3_impedance_controller_real/bak/elbow_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c988cb502164eb85ef0a803f9347b58a28ab625a
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/elbow_example_controller.cpp
@@ -0,0 +1,86 @@
+// 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/elbow_example_controller.h>
+
+#include <cmath>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include <controller_interface/controller_base.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+namespace franka_example_controllers {
+
+bool ElbowExampleController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
+ if (cartesian_pose_interface_ == nullptr) {
+ ROS_ERROR("ElbowExampleController: Could not get Cartesian Pose interface from hardware");
+ return false;
+ }
+
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("ElbowExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ try {
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface_->getHandle(arm_id + "_robot"));
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM("ElbowExampleController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("ElbowExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ try {
+ auto state_handle = state_interface->getHandle(arm_id + "_robot");
+
+ std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "ElbowExampleController: Robot is not in the expected starting position for "
+ "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
+ "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ return false;
+ }
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM("ElbowExampleController: Exception getting state handle: " << e.what());
+ return false;
+ }
+
+ return true;
+}
+
+void ElbowExampleController::starting(const ros::Time& /* time */) {
+ initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ initial_elbow_ = cartesian_pose_handle_->getRobotState().elbow_d;
+ elapsed_time_ = ros::Duration(0.0);
+}
+
+void ElbowExampleController::update(const ros::Time& /* time */, const ros::Duration& period) {
+ elapsed_time_ += period;
+
+ double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_.toSec()));
+ auto elbow = initial_elbow_;
+ elbow[0] += angle;
+
+ cartesian_pose_handle_->setCommand(initial_pose_, elbow);
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ElbowExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/elbow_example_controller.h b/fr3_impedance_controller_real/bak/elbow_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..70a8cfb16cfa85ba695fc1393c9daaad6156ec04
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/elbow_example_controller.h
@@ -0,0 +1,35 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <array>
+#include <memory>
+#include <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_cartesian_command_interface.h>
+
+namespace franka_example_controllers {
+
+class ElbowExampleController
+ : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+ private:
+ franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
+ std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
+ ros::Duration elapsed_time_;
+ std::array<double, 16> initial_pose_{};
+ std::array<double, 2> initial_elbow_{};
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/elbow_example_controller.launch b/fr3_impedance_controller_real/bak/elbow_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..890f86545428747108774c36e87ef05984cb3008
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/elbow_example_controller.launch
@@ -0,0 +1,9 @@
+<?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)" />
+ <include file="$(find franka_control)/launch/franka_control.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="elbow_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/force_example_controller.cpp b/fr3_impedance_controller_real/bak/force_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2923ad58cfaa9a5e41f37469e812d1838172d517
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/force_example_controller.cpp
@@ -0,0 +1,152 @@
+// 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/force_example_controller.h>
+
+#include <cmath>
+#include <memory>
+
+#include <controller_interface/controller_base.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <franka/robot_state.h>
+
+namespace franka_example_controllers {
+
+bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle) {
+ std::vector<std::string> joint_names;
+ std::string arm_id;
+ ROS_WARN(
+ "ForceExampleController: Make sure your robot's endeffector is in contact "
+ "with a horizontal surface before starting the controller!");
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("ForceExampleController: Could not read parameter arm_id");
+ return false;
+ }
+ if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
+ ROS_ERROR(
+ "ForceExampleController: 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("ForceExampleController: 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(
+ "ForceExampleController: 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("ForceExampleController: 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(
+ "ForceExampleController: 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("ForceExampleController: 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("ForceExampleController: Exception getting joint handles: " << ex.what());
+ return false;
+ }
+ }
+
+ dynamic_reconfigure_desired_mass_param_node_ =
+ ros::NodeHandle("dynamic_reconfigure_desired_mass_param_node");
+ dynamic_server_desired_mass_param_ = std::make_unique<
+ dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>(
+
+ dynamic_reconfigure_desired_mass_param_node_);
+ dynamic_server_desired_mass_param_->setCallback(
+ boost::bind(&ForceExampleController::desiredMassParamCallback, this, _1, _2));
+
+ return true;
+}
+
+void ForceExampleController::starting(const ros::Time& /*time*/) {
+ franka::RobotState robot_state = state_handle_->getRobotState();
+ std::array<double, 7> gravity_array = model_handle_->getGravity();
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
+ // Bias correction for the current external torque
+ tau_ext_initial_ = tau_measured - gravity;
+ tau_error_.setZero();
+}
+
+void ForceExampleController::update(const ros::Time& /*time*/, const ros::Duration& period) {
+ franka::RobotState robot_state = state_handle_->getRobotState();
+ std::array<double, 42> jacobian_array =
+ model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+ std::array<double, 7> gravity_array = model_handle_->getGravity();
+ Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d( // NOLINT (readability-identifier-naming)
+ robot_state.tau_J_d.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
+
+ Eigen::Matrix<double, 7, 1> tau_d, tau_cmd, tau_ext;
+ Eigen::Matrix<double, 6, 1> desired_force_torque;
+ desired_force_torque.setZero();
+ desired_force_torque(2) = desired_mass_ * -9.81;
+ tau_ext = tau_measured - gravity - tau_ext_initial_;
+ tau_d = jacobian.transpose() * desired_force_torque;
+ tau_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext);
+ // FF + PI control (PI gains are initially all 0)
+ tau_cmd = tau_d + k_p_ * (tau_d - tau_ext) + k_i_ * tau_error_;
+ tau_cmd = saturateTorqueRate(tau_cmd, tau_J_d);
+
+ for (size_t i = 0; i < 7; ++i) {
+ joint_handles_[i].setCommand(tau_cmd(i));
+ }
+
+ // Update signals changed online through dynamic reconfigure
+ desired_mass_ = filter_gain_ * target_mass_ + (1 - filter_gain_) * desired_mass_;
+ k_p_ = filter_gain_ * target_k_p_ + (1 - filter_gain_) * k_p_;
+ k_i_ = filter_gain_ * target_k_i_ + (1 - filter_gain_) * k_i_;
+}
+
+void ForceExampleController::desiredMassParamCallback(
+ franka_example_controllers::desired_mass_paramConfig& config,
+ uint32_t /*level*/) {
+ target_mass_ = config.desired_mass;
+ target_k_p_ = config.k_p;
+ target_k_i_ = config.k_i;
+}
+
+Eigen::Matrix<double, 7, 1> ForceExampleController::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, kDeltaTauMax), -kDeltaTauMax);
+ }
+ return tau_d_saturated;
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ForceExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/force_example_controller.h b/fr3_impedance_controller_real/bak/force_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b7eb7d75906e673bbcce9f03694a971fa9bae9b
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/force_example_controller.h
@@ -0,0 +1,61 @@
+// 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 <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.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/Core>
+
+#include <franka_example_controllers/desired_mass_paramConfig.h>
+
+namespace franka_example_controllers {
+
+class ForceExampleController : 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 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::FrankaModelHandle> model_handle_;
+ std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
+ std::vector<hardware_interface::JointHandle> joint_handles_;
+
+ double desired_mass_{0.0};
+ double target_mass_{0.0};
+ double k_p_{0.0};
+ double k_i_{0.0};
+ double target_k_p_{0.0};
+ double target_k_i_{0.0};
+ double filter_gain_{0.001};
+ Eigen::Matrix<double, 7, 1> tau_ext_initial_;
+ Eigen::Matrix<double, 7, 1> tau_error_;
+ static constexpr double kDeltaTauMax{1.0};
+
+ // Dynamic reconfigure
+ std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>
+ dynamic_server_desired_mass_param_;
+ ros::NodeHandle dynamic_reconfigure_desired_mass_param_node_;
+ void desiredMassParamCallback(franka_example_controllers::desired_mass_paramConfig& config,
+ uint32_t level);
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/force_example_controller.launch b/fr3_impedance_controller_real/bak/force_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..58c873d96765e1253de439a3063c11de7b0d41b7
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/force_example_controller.launch
@@ -0,0 +1,10 @@
+<?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)" />
+ <include file="$(find franka_control)/launch/franka_control.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="force_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
+</launch>
diff --git a/fr3_impedance_controller_real/bak/joint_impedance_example_controller.cpp b/fr3_impedance_controller_real/bak/joint_impedance_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..607e87da9d919f03cef4925f46b92c8b9ab5e1da
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_impedance_example_controller.cpp
@@ -0,0 +1,214 @@
+// 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/joint_impedance_example_controller.h>
+
+#include <cmath>
+#include <memory>
+
+#include <controller_interface/controller_base.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <franka/robot_state.h>
+
+namespace franka_example_controllers {
+
+bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle) {
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("JointImpedanceExampleController: Could not read parameter arm_id");
+ return false;
+ }
+ if (!node_handle.getParam("radius", radius_)) {
+ ROS_INFO_STREAM(
+ "JointImpedanceExampleController: No parameter radius, defaulting to: " << radius_);
+ }
+ if (std::fabs(radius_) < 0.005) {
+ ROS_INFO_STREAM("JointImpedanceExampleController: Set radius to small, defaulting to: " << 0.1);
+ radius_ = 0.1;
+ }
+
+ if (!node_handle.getParam("vel_max", vel_max_)) {
+ ROS_INFO_STREAM(
+ "JointImpedanceExampleController: No parameter vel_max, defaulting to: " << vel_max_);
+ }
+ if (!node_handle.getParam("acceleration_time", acceleration_time_)) {
+ ROS_INFO_STREAM(
+ "JointImpedanceExampleController: No parameter acceleration_time, defaulting to: "
+ << acceleration_time_);
+ }
+
+ std::vector<std::string> joint_names;
+ if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
+ ROS_ERROR(
+ "JointImpedanceExampleController: Invalid or no joint_names parameters provided, aborting "
+ "controller init!");
+ return false;
+ }
+
+ if (!node_handle.getParam("k_gains", k_gains_) || k_gains_.size() != 7) {
+ ROS_ERROR(
+ "JointImpedanceExampleController: Invalid or no k_gain parameters provided, aborting "
+ "controller init!");
+ return false;
+ }
+
+ if (!node_handle.getParam("d_gains", d_gains_) || d_gains_.size() != 7) {
+ ROS_ERROR(
+ "JointImpedanceExampleController: Invalid or no d_gain parameters provided, aborting "
+ "controller init!");
+ return false;
+ }
+
+ double publish_rate(30.0);
+ if (!node_handle.getParam("publish_rate", publish_rate)) {
+ ROS_INFO_STREAM("JointImpedanceExampleController: publish_rate not found. Defaulting to "
+ << publish_rate);
+ }
+ rate_trigger_ = franka_hw::TriggerRate(publish_rate);
+
+ if (!node_handle.getParam("coriolis_factor", coriolis_factor_)) {
+ ROS_INFO_STREAM("JointImpedanceExampleController: coriolis_factor not found. Defaulting to "
+ << coriolis_factor_);
+ }
+
+ auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
+ if (model_interface == nullptr) {
+ ROS_ERROR_STREAM(
+ "JointImpedanceExampleController: 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(
+ "JointImpedanceExampleController: Exception getting model handle from interface: "
+ << ex.what());
+ return false;
+ }
+
+ auto* cartesian_pose_interface = robot_hw->get<franka_hw::FrankaPoseCartesianInterface>();
+ if (cartesian_pose_interface == nullptr) {
+ ROS_ERROR_STREAM(
+ "JointImpedanceExampleController: Error getting cartesian pose interface from hardware");
+ return false;
+ }
+ try {
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface->getHandle(arm_id + "_robot"));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "JointImpedanceExampleController: Exception getting cartesian pose 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(
+ "JointImpedanceExampleController: 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(
+ "JointImpedanceExampleController: Exception getting joint handles: " << ex.what());
+ return false;
+ }
+ }
+ torques_publisher_.init(node_handle, "torque_comparison", 1);
+
+ std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0);
+
+ return true;
+}
+
+void JointImpedanceExampleController::starting(const ros::Time& /*time*/) {
+ initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+}
+
+void JointImpedanceExampleController::update(const ros::Time& /*time*/,
+ const ros::Duration& period) {
+ if (vel_current_ < vel_max_) {
+ vel_current_ += period.toSec() * std::fabs(vel_max_ / acceleration_time_);
+ }
+ vel_current_ = std::fmin(vel_current_, vel_max_);
+
+ angle_ += period.toSec() * vel_current_ / std::fabs(radius_);
+ if (angle_ > 2 * M_PI) {
+ angle_ -= 2 * M_PI;
+ }
+
+ double delta_y = radius_ * (1 - std::cos(angle_));
+ double delta_z = radius_ * std::sin(angle_);
+
+ std::array<double, 16> pose_desired = initial_pose_;
+ pose_desired[13] += delta_y;
+ pose_desired[14] += delta_z;
+ cartesian_pose_handle_->setCommand(pose_desired);
+
+ franka::RobotState robot_state = cartesian_pose_handle_->getRobotState();
+ std::array<double, 7> coriolis = model_handle_->getCoriolis();
+ std::array<double, 7> gravity = model_handle_->getGravity();
+
+ double alpha = 0.99;
+ for (size_t i = 0; i < 7; i++) {
+ dq_filtered_[i] = (1 - alpha) * dq_filtered_[i] + alpha * robot_state.dq[i];
+ }
+
+ std::array<double, 7> tau_d_calculated;
+ for (size_t i = 0; i < 7; ++i) {
+ tau_d_calculated[i] = coriolis_factor_ * coriolis[i] +
+ k_gains_[i] * (robot_state.q_d[i] - robot_state.q[i]) +
+ d_gains_[i] * (robot_state.dq_d[i] - dq_filtered_[i]);
+ }
+
+ // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is
+ // 1000 * (1 / sampling_time).
+ std::array<double, 7> tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d);
+
+ for (size_t i = 0; i < 7; ++i) {
+ joint_handles_[i].setCommand(tau_d_saturated[i]);
+ }
+
+ if (rate_trigger_() && torques_publisher_.trylock()) {
+ std::array<double, 7> tau_j = robot_state.tau_J;
+ std::array<double, 7> tau_error;
+ double error_rms(0.0);
+ for (size_t i = 0; i < 7; ++i) {
+ tau_error[i] = last_tau_d_[i] - tau_j[i];
+ error_rms += std::sqrt(std::pow(tau_error[i], 2.0)) / 7.0;
+ }
+ torques_publisher_.msg_.root_mean_square_error = error_rms;
+ for (size_t i = 0; i < 7; ++i) {
+ torques_publisher_.msg_.tau_commanded[i] = last_tau_d_[i];
+ torques_publisher_.msg_.tau_error[i] = tau_error[i];
+ torques_publisher_.msg_.tau_measured[i] = tau_j[i];
+ }
+ torques_publisher_.unlockAndPublish();
+ }
+
+ for (size_t i = 0; i < 7; ++i) {
+ last_tau_d_[i] = tau_d_saturated[i] + gravity[i];
+ }
+}
+
+std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate(
+ const std::array<double, 7>& tau_d_calculated,
+ const std::array<double, 7>& tau_J_d) { // NOLINT (readability-identifier-naming)
+ std::array<double, 7> 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, kDeltaTauMax), -kDeltaTauMax);
+ }
+ return tau_d_saturated;
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointImpedanceExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/joint_impedance_example_controller.h b/fr3_impedance_controller_real/bak/joint_impedance_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..526629a463dfbd43e530978543afe166976b202f
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_impedance_example_controller.h
@@ -0,0 +1,60 @@
+// 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 <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_example_controllers/JointTorqueComparison.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/trigger_rate.h>
+
+namespace franka_example_controllers {
+
+class JointImpedanceExampleController : public controller_interface::MultiInterfaceController<
+ franka_hw::FrankaModelInterface,
+ hardware_interface::EffortJointInterface,
+ franka_hw::FrankaPoseCartesianInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+ private:
+ // Saturation
+ std::array<double, 7> saturateTorqueRate(
+ const std::array<double, 7>& tau_d_calculated,
+ const std::array<double, 7>& tau_J_d); // NOLINT (readability-identifier-naming)
+
+ std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
+ std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+ std::vector<hardware_interface::JointHandle> joint_handles_;
+
+ static constexpr double kDeltaTauMax{1.0};
+ double radius_{0.1};
+ double acceleration_time_{2.0};
+ double vel_max_{0.05};
+ double angle_{0.0};
+ double vel_current_{0.0};
+
+ std::vector<double> k_gains_;
+ std::vector<double> d_gains_;
+ double coriolis_factor_{1.0};
+ std::array<double, 7> dq_filtered_;
+ std::array<double, 16> initial_pose_;
+
+ franka_hw::TriggerRate rate_trigger_{1.0};
+ std::array<double, 7> last_tau_d_{};
+ realtime_tools::RealtimePublisher<JointTorqueComparison> torques_publisher_;
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/joint_impedance_example_controller.launch b/fr3_impedance_controller_real/bak/joint_impedance_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..c624359a9ee8a112e23ad5efc420e62c30c8f429
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_impedance_example_controller.launch
@@ -0,0 +1,9 @@
+<?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)" />
+ <include file="$(find franka_control)/launch/franka_control.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="joint_impedance_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/joint_position_example_controller.cpp b/fr3_impedance_controller_real/bak/joint_position_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3ba12b8d80072bc1de24d83c334ed3d23115507f
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_position_example_controller.cpp
@@ -0,0 +1,81 @@
+// 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/joint_position_example_controller.h>
+
+#include <cmath>
+
+#include <controller_interface/controller_base.h>
+#include <hardware_interface/hardware_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+namespace franka_example_controllers {
+
+bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ position_joint_interface_ = robot_hardware->get<hardware_interface::PositionJointInterface>();
+ if (position_joint_interface_ == nullptr) {
+ ROS_ERROR(
+ "JointPositionExampleController: Error getting position joint interface from hardware!");
+ return false;
+ }
+ std::vector<std::string> joint_names;
+ if (!node_handle.getParam("joint_names", joint_names)) {
+ ROS_ERROR("JointPositionExampleController: Could not parse joint names");
+ }
+ if (joint_names.size() != 7) {
+ ROS_ERROR_STREAM("JointPositionExampleController: Wrong number of joint names, got "
+ << joint_names.size() << " instead of 7 names!");
+ return false;
+ }
+ position_joint_handles_.resize(7);
+ for (size_t i = 0; i < 7; ++i) {
+ try {
+ position_joint_handles_[i] = position_joint_interface_->getHandle(joint_names[i]);
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "JointPositionExampleController: Exception getting joint handles: " << e.what());
+ return false;
+ }
+ }
+
+ std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(position_joint_handles_[i].getPosition() - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "JointPositionExampleController: Robot is not in the expected starting position for "
+ "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
+ "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ return false;
+ }
+ }
+
+ return true;
+}
+
+void JointPositionExampleController::starting(const ros::Time& /* time */) {
+ for (size_t i = 0; i < 7; ++i) {
+ initial_pose_[i] = position_joint_handles_[i].getPosition();
+ }
+ elapsed_time_ = ros::Duration(0.0);
+}
+
+void JointPositionExampleController::update(const ros::Time& /*time*/,
+ const ros::Duration& period) {
+ elapsed_time_ += period;
+
+ double delta_angle = M_PI / 16 * (1 - std::cos(M_PI / 5.0 * elapsed_time_.toSec())) * 0.2;
+ for (size_t i = 0; i < 7; ++i) {
+ if (i == 4) {
+ position_joint_handles_[i].setCommand(initial_pose_[i] - delta_angle);
+ } else {
+ position_joint_handles_[i].setCommand(initial_pose_[i] + delta_angle);
+ }
+ }
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointPositionExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/joint_position_example_controller.h b/fr3_impedance_controller_real/bak/joint_position_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..414885106b372223a8b43b951668c5d47efc3271
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_position_example_controller.h
@@ -0,0 +1,31 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <array>
+#include <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+namespace franka_example_controllers {
+
+class JointPositionExampleController : public controller_interface::MultiInterfaceController<
+ hardware_interface::PositionJointInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+ private:
+ hardware_interface::PositionJointInterface* position_joint_interface_;
+ std::vector<hardware_interface::JointHandle> position_joint_handles_;
+ ros::Duration elapsed_time_;
+ std::array<double, 7> initial_pose_{};
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/joint_position_example_controller.launch b/fr3_impedance_controller_real/bak/joint_position_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..565fc885e25aa9c3e33e77ab026bf90df8cae51d
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_position_example_controller.launch
@@ -0,0 +1,8 @@
+<?xml version="1.0" ?>
+<launch>
+ <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/>
+ <arg name="arm_id" default="fr3"/>
+ <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="joint_position_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/joint_velocity_example_controller.cpp b/fr3_impedance_controller_real/bak/joint_velocity_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0d9274c8b7cc0037049f29b661beb776d59d8fa2
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_velocity_example_controller.cpp
@@ -0,0 +1,108 @@
+// 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/joint_velocity_example_controller.h>
+
+#include <cmath>
+
+#include <controller_interface/controller_base.h>
+#include <hardware_interface/hardware_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+namespace franka_example_controllers {
+
+bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ velocity_joint_interface_ = robot_hardware->get<hardware_interface::VelocityJointInterface>();
+ if (velocity_joint_interface_ == nullptr) {
+ ROS_ERROR(
+ "JointVelocityExampleController: Error getting velocity joint interface from hardware!");
+ return false;
+ }
+
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("JointVelocityExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ std::vector<std::string> joint_names;
+ if (!node_handle.getParam("joint_names", joint_names)) {
+ ROS_ERROR("JointVelocityExampleController: Could not parse joint names");
+ }
+ if (joint_names.size() != 7) {
+ ROS_ERROR_STREAM("JointVelocityExampleController: Wrong number of joint names, got "
+ << joint_names.size() << " instead of 7 names!");
+ return false;
+ }
+ velocity_joint_handles_.resize(7);
+ for (size_t i = 0; i < 7; ++i) {
+ try {
+ velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_names[i]);
+ } catch (const hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "JointVelocityExampleController: Exception getting joint handles: " << ex.what());
+ return false;
+ }
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("JointVelocityExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ try {
+ auto state_handle = state_interface->getHandle(arm_id + "_robot");
+
+ std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "JointVelocityExampleController: Robot is not in the expected starting position for "
+ "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
+ "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ return false;
+ }
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "JointVelocityExampleController: Exception getting state handle: " << e.what());
+ return false;
+ }
+
+ return true;
+}
+
+void JointVelocityExampleController::starting(const ros::Time& /* time */) {
+ elapsed_time_ = ros::Duration(0.0);
+}
+
+void JointVelocityExampleController::update(const ros::Time& /* time */,
+ const ros::Duration& period) {
+ elapsed_time_ += period;
+
+ ros::Duration time_max(8.0);
+ double omega_max = 0.1;
+ double cycle = std::floor(
+ std::pow(-1.0, (elapsed_time_.toSec() - std::fmod(elapsed_time_.toSec(), time_max.toSec())) /
+ time_max.toSec()));
+ double omega = cycle * omega_max / 2.0 *
+ (1.0 - std::cos(2.0 * M_PI / time_max.toSec() * elapsed_time_.toSec()));
+
+ for (auto joint_handle : velocity_joint_handles_) {
+ joint_handle.setCommand(omega);
+ }
+}
+
+void JointVelocityExampleController::stopping(const ros::Time& /*time*/) {
+ // WARNING: DO NOT SEND ZERO VELOCITIES HERE AS IN CASE OF ABORTING DURING MOTION
+ // A JUMP TO ZERO WILL BE COMMANDED PUTTING HIGH LOADS ON THE ROBOT. LET THE DEFAULT
+ // BUILT-IN STOPPING BEHAVIOR SLOW DOWN THE ROBOT.
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointVelocityExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/joint_velocity_example_controller.h b/fr3_impedance_controller_real/bak/joint_velocity_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c20999d8578a5bf1b1d073bedf24d95d3bbf08e
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_velocity_example_controller.h
@@ -0,0 +1,32 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+namespace franka_example_controllers {
+
+class JointVelocityExampleController : public controller_interface::MultiInterfaceController<
+ hardware_interface::VelocityJointInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+ void starting(const ros::Time&) override;
+ void stopping(const ros::Time&) override;
+
+ private:
+ hardware_interface::VelocityJointInterface* velocity_joint_interface_;
+ std::vector<hardware_interface::JointHandle> velocity_joint_handles_;
+ ros::Duration elapsed_time_;
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/joint_velocity_example_controller.launch b/fr3_impedance_controller_real/bak/joint_velocity_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..ff17fdd2c3238ae98a82e1c129794bb55fd2b01d
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_velocity_example_controller.launch
@@ -0,0 +1,9 @@
+<?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)" />
+ <include file="$(find franka_control)/launch/franka_control.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="joint_velocity_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/joint_wall.cpp b/fr3_impedance_controller_real/bak/joint_wall.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d2138bf778a2daf2394ba68992c1d54196775297
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_wall.cpp
@@ -0,0 +1,155 @@
+// Copyright (c) 2022 Franka Emika GmbH
+#include <franka_example_controllers/joint_wall.h>
+
+#include <ros/ros.h>
+
+#include <cmath>
+#include <iostream>
+#include <sstream>
+
+namespace franka_example_controllers {
+
+JointWall::JointWall(const double soft_upper_joint_position_limit,
+ const double soft_lower_joint_position_limit,
+ const double PD_zone_width, // NOLINT (readability-identifier-naming)
+ const double D_zone_width, // NOLINT (readability-identifier-naming)
+ const double PD_zone_stiffness, // NOLINT (readability-identifier-naming)
+ const double PD_zone_damping, // NOLINT (readability-identifier-naming)
+ const double D_zone_damping) // NOLINT (readability-identifier-naming)
+ : soft_upper_joint_position_limit_(soft_upper_joint_position_limit),
+ soft_lower_joint_position_limit_(soft_lower_joint_position_limit),
+ PD_zone_width_(PD_zone_width),
+ D_zone_width_(D_zone_width),
+ PD_zone_stiffness_(PD_zone_stiffness),
+ PD_zone_damping_(PD_zone_damping),
+ D_zone_damping_(D_zone_damping) {
+ PD_zone_width_ = positiveCheck(PD_zone_width);
+ D_zone_width_ = positiveCheck(D_zone_width);
+ PD_zone_stiffness_ = positiveCheck(PD_zone_stiffness);
+ PD_zone_damping_ = positiveCheck(PD_zone_damping);
+ D_zone_damping_ = positiveCheck(D_zone_damping);
+};
+
+double JointWall::computeTorque(const double q, const double dq) {
+ init(q, dq);
+ adjustMovingWall(q, dq);
+
+ double D_zone_boundary_max = // NOLINT (readability-identifier-naming)
+ soft_upper_joint_position_limit_ - zone_width_scale_ * (PD_zone_width_ + D_zone_width_);
+ double PD_zone_boundary_max = // NOLINT (readability-identifier-naming)
+ soft_upper_joint_position_limit_ - zone_width_scale_ * PD_zone_width_;
+ double D_zone_boundary_min = // NOLINT (readability-identifier-naming)
+ soft_lower_joint_position_limit_ + zone_width_scale_ * (PD_zone_width_ + D_zone_width_);
+ double PD_zone_boundary_min = // NOLINT (readability-identifier-naming)
+ soft_lower_joint_position_limit_ + zone_width_scale_ * PD_zone_width_;
+
+ double torque = 0;
+ if (inRange(D_zone_boundary_max, PD_zone_boundary_max, q) ||
+ inRange(PD_zone_boundary_min, D_zone_boundary_min, q)) {
+ // In D zone
+ torque = -D_zone_damping_ * dq;
+ } else if (q > PD_zone_boundary_max) {
+ // In PD zone max
+ torque = -PD_zone_damping_ * dq + PD_zone_stiffness_ * (PD_zone_boundary_max - q);
+ } else if (q < PD_zone_boundary_min) {
+ // In PD zone min
+ torque = -PD_zone_damping_ * dq + PD_zone_stiffness_ * (PD_zone_boundary_min - q);
+ }
+
+ return torque;
+}
+
+void JointWall::reset() {
+ initialized_ = false;
+}
+
+bool JointWall::inRange(double low, double high, double x) {
+ return (low <= x && x <= high);
+};
+
+void JointWall::init(const double q, const double dq) {
+ if (initialized_) {
+ return;
+ }
+
+ if (q < soft_lower_joint_position_limit_ || q > soft_upper_joint_position_limit_) {
+ std::stringstream ss;
+ ss << "q " << q << " is beyond the joint wall: [" << soft_lower_joint_position_limit_ << ", "
+ << soft_upper_joint_position_limit_ << "]";
+ throw std::runtime_error(ss.str().c_str());
+ }
+
+ switch (getMotionInWall(q, dq)) {
+ case MotionInWall::EnteringNormal:
+ moving_wall_ = false;
+ break;
+ case MotionInWall::PenetratingLowerLimit:
+ case MotionInWall::LeavingLowerLimit:
+ zone_width_scale_ =
+ fabs(q - soft_lower_joint_position_limit_) / (PD_zone_width_ + D_zone_width_);
+ moving_wall_ = true;
+ break;
+ case MotionInWall::PenetratingUpperLimit:
+ case MotionInWall::LeavingUpperLimit:
+ zone_width_scale_ =
+ fabs(q - soft_upper_joint_position_limit_) / (PD_zone_width_ + D_zone_width_);
+ moving_wall_ = true;
+ break;
+ }
+ initialized_ = true;
+}
+
+void JointWall::adjustMovingWall(const double q, const double dq) {
+ if (!moving_wall_) {
+ return;
+ }
+ double new_scale;
+ switch (getMotionInWall(q, dq)) {
+ case MotionInWall::EnteringNormal:
+ moving_wall_ = false;
+ zone_width_scale_ = 1;
+ break;
+ case MotionInWall::LeavingLowerLimit:
+ new_scale = fabs(q - soft_lower_joint_position_limit_) / (PD_zone_width_ + D_zone_width_);
+ zone_width_scale_ = fmax(zone_width_scale_, new_scale);
+ break;
+ case MotionInWall::LeavingUpperLimit:
+ new_scale = fabs(q - soft_upper_joint_position_limit_) / (PD_zone_width_ + D_zone_width_);
+ zone_width_scale_ = fmax(zone_width_scale_, new_scale);
+ break;
+ case MotionInWall::PenetratingLowerLimit:
+ case MotionInWall::PenetratingUpperLimit:
+ break;
+ }
+}
+
+double JointWall::positiveCheck(double value) {
+ if (value < 0) {
+ ROS_WARN_THROTTLE(
+ 1, "JointWall expects positive parameters, but got negative. Using its absolute value.");
+ value = fabs(value);
+ }
+ return value;
+}
+
+JointWall::MotionInWall JointWall::getMotionInWall(const double q, const double dq) const {
+ double D_zone_boundary_max = // NOLINT (readability-identifier-naming)
+ soft_upper_joint_position_limit_ - PD_zone_width_ - D_zone_width_;
+ double D_zone_boundary_min = // NOLINT (readability-identifier-naming)
+ soft_lower_joint_position_limit_ + PD_zone_width_ + D_zone_width_;
+ if (q < D_zone_boundary_min) {
+ if (dq <= 0) {
+ return MotionInWall::PenetratingLowerLimit;
+ }
+ return MotionInWall::LeavingLowerLimit;
+ }
+ if (q > D_zone_boundary_max) {
+ if (dq >= 0) {
+ return MotionInWall::PenetratingUpperLimit;
+ }
+ return MotionInWall::LeavingUpperLimit;
+ }
+ return MotionInWall::EnteringNormal;
+}
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/joint_wall.h b/fr3_impedance_controller_real/bak/joint_wall.h
new file mode 100644
index 0000000000000000000000000000000000000000..753c81bcdbbc056a848a6e9fc0e08061d8765b21
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/joint_wall.h
@@ -0,0 +1,199 @@
+// Copyright (c) 2022 Franka Emika GmbH
+#pragma once
+
+#include <array>
+#include <memory>
+
+// clang-format off
+/**
+ * @file joint_wall.h
+ * Contains functions for calculating torques generated by virtual walls.
+ |<---- PD_zone_width ------>|<---D_zone_width--->| |<---D_zone_width--->|<--- PD_zone_width --->|
+ |:::::::::::::::::::::::::::|- - - - - - - - - - | | - - - - - - - - - -|:::::::::::::::::::::::|
+ ^ soft_lower_joint_position_limit normal ^ range soft_upper_joint_position_limit ^
+ PD_zone_boundary_min ^ ^ PD_zone_boundary_max
+ D_zone_boundary_min ^ ^ D_zone_boundary_max
+*/
+// clang-format on
+
+namespace franka_example_controllers {
+
+/// A class that offers an implementation of a virtual wall for a single joint.
+class JointWall {
+ public:
+ /**
+ * Creates a JointWall instance with default params.
+ */
+ JointWall() = delete;
+
+ /**
+ * Creates a JointWall instance with configurable parameters.
+ * @param[in] soft_upper_joint_position_limit (rad)
+ * @param[in] soft_lower_joint_position_limit (rad)
+ * @param[in] PD_zone_width (meter)
+ * @param[in] D_zone_width (meter)
+ * @param[in] PD_zone_stiffness (N*meter/rad)
+ * @param[in] PD_zone_damping (N*meter*s/rad)
+ * @param[in] D_zone_damping (N*meter*s/rad)
+ */
+ JointWall(double soft_upper_joint_position_limit,
+ double soft_lower_joint_position_limit,
+ double PD_zone_width,
+ double D_zone_width,
+ double PD_zone_stiffness,
+ double PD_zone_damping,
+ double D_zone_damping);
+
+ /**
+ * Computes the torque with given q and dq. Be aware that the torque is also affected by previous
+ * states.
+ * @param[in] q current joint position.
+ * @param[in] dq the current joint velocity.
+ * @return the resulting torque
+ */
+ double computeTorque(double q, double dq);
+
+ /**
+ * Resets the initialized flag to false. After calling reset, the next call to computeTorque
+ * resets the internal states for joint position and velocity (q, dq). Call this method before
+ * restarting your controller, e.g. when the position might have changed during control pauses due
+ * to guiding.
+ */
+ void reset();
+
+ private:
+ /**
+ * Indicates the status of moving wall, which occurs after initializing the state inside a wall.
+ */
+ enum class MotionInWall {
+ EnteringNormal,
+ PenetratingLowerLimit,
+ LeavingLowerLimit,
+ PenetratingUpperLimit,
+ LeavingUpperLimit
+ };
+
+ // Joint wall parameters
+ double soft_upper_joint_position_limit_{0};
+ double soft_lower_joint_position_limit_{0};
+ double PD_zone_width_{0};
+ double D_zone_width_{0};
+ double PD_zone_stiffness_{0};
+ double PD_zone_damping_{0};
+ double D_zone_damping_{0};
+
+ bool initialized_{false}; // Internal states q, dq will be reset to the current states in
+ // computeTorque if initialized_ = false;
+
+ // Indicates whether the joint wall is still moving, which occurs after initializing the state
+ // inside joint wall.
+ bool moving_wall_{false};
+
+ double zone_width_scale_{1};
+
+ /**
+ * Checks if x is in range [low, high]
+ * @param[in] low lower limit of the range.
+ * @param[in] high upper limit of the range.
+ * @param[in] x value to check.
+ * @return true if in range, false otherwise.
+ */
+ static bool inRange(double low, double high, double x);
+
+ /**
+ * Check if the input is positive number, if not print error and return its abs
+ * @param[in] value The value to check.
+ * @return the absolute value of the value.
+ */
+ static double positiveCheck(double value);
+
+ /**
+ * Initializes the joint wall computation with initial states.
+ * @param[in] q the current joint position.
+ * @param[in] dq the current joint velocity.
+ */
+ void init(double q, double dq);
+
+ /**
+ * Moves the wall with given state if the state is initialized inside the wall
+ * @param[in] q the current joint position.
+ * @param[in] dq the current joint velocity.
+ */
+ void adjustMovingWall(double q, double dq);
+
+ /**
+ * Checks the motion type in joint wall
+ * @param[in] q the current joint position.
+ * @param[in] dq the current joint velocity.
+ */
+ MotionInWall getMotionInWall(double q, double dq) const;
+};
+
+/**
+ * A class that organizes multiple virtual joint walls.
+ * @tparam num_dof the number of joints to create virtual walls for.
+ */
+template <size_t num_dof>
+class JointWallContainer {
+ public:
+ /**
+ * Creates a new virtual joint position wall instance. (see joint_wall.h for more details on joint
+ * wall parameters.)
+ *
+ * @param[in] soft_upper_joint_position_limits
+ * @param[in] soft_lower_joint_position_limits
+ * @param[in] PD_zone_widths
+ * @param[in] D_zone_widths
+ * @param[in] PD_zone_stiffnesses
+ * @param[in] PD_zone_dampings
+ * @param[in] D_zone_dampings
+ */
+ JointWallContainer(const std::array<double, num_dof>& soft_upper_joint_position_limits,
+ const std::array<double, num_dof>& soft_lower_joint_position_limits,
+ const std::array<double, num_dof>& PD_zone_widths,
+ const std::array<double, num_dof>& D_zone_widths,
+ const std::array<double, num_dof>& PD_zone_stiffnesses,
+ const std::array<double, num_dof>& PD_zone_dampings,
+ const std::array<double, num_dof>& D_zone_dampings) {
+ for (size_t i = 0; i < num_dof; i++) {
+ joint_walls_.at(i) = std::make_unique<JointWall>(
+ soft_upper_joint_position_limits[i], soft_lower_joint_position_limits[i],
+ PD_zone_widths[i], D_zone_widths[i], PD_zone_stiffnesses[i], PD_zone_dampings[i],
+ D_zone_dampings[i]);
+ }
+ }
+
+ JointWallContainer() = delete;
+
+ /**
+ * Computes the torques generated by virtual walls.
+ * @param[in] q the current joint positions.
+ * @param[in] dq the current joint velocities.
+ * @return the repelling torques/efforts from the virtual wall.
+ */
+ std::array<double, num_dof> computeTorque(const std::array<double, num_dof>& q,
+ const std::array<double, num_dof>& dq) {
+ std::array<double, num_dof> torque;
+ for (size_t i = 0; i < num_dof; i++) {
+ torque[i] = joint_walls_[i]->computeTorque(q[i], dq[i]);
+ };
+ return torque;
+ }
+
+ /**
+ * Resets the initialized flags of all walls to false. After calling reset, the next call to
+ * computeTorque resets the internal states for joint position and velocity (q, dq). Call this
+ * method before restarting your controller, e.g. when the position might have changed during
+ * control pauses due to guiding.
+ */
+ void reset() {
+ for (auto& jw : joint_walls_) {
+ jw->reset();
+ }
+ }
+
+ private:
+ std::array<std::unique_ptr<JointWall>, num_dof> joint_walls_;
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/mainpage.dox b/fr3_impedance_controller_real/bak/mainpage.dox
new file mode 100644
index 0000000000000000000000000000000000000000..941d0bf97714038b58c39c7dcd816e901e22c023
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/mainpage.dox
@@ -0,0 +1,6 @@
+/**
+ * @mainpage
+ * @htmlinclude "manifest.html"
+ *
+ * Overview page for Franka Emika research robots: https://frankaemika.github.io
+ */
diff --git a/fr3_impedance_controller_real/bak/model_example_controller.cpp b/fr3_impedance_controller_real/bak/model_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b67f2416e7473626a91515f551c81c5d8209bb82
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/model_example_controller.cpp
@@ -0,0 +1,95 @@
+// 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/model_example_controller.h>
+
+#include <algorithm>
+#include <array>
+#include <cstring>
+#include <iterator>
+#include <memory>
+
+#include <controller_interface/controller_base.h>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+
+namespace {
+template <class T, size_t N>
+std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
+ ostream << "[";
+ std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
+ std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
+ ostream << "]";
+ return ostream;
+}
+} // anonymous namespace
+
+namespace franka_example_controllers {
+
+bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle) {
+ franka_state_interface_ = robot_hw->get<franka_hw::FrankaStateInterface>();
+ if (franka_state_interface_ == nullptr) {
+ ROS_ERROR("ModelExampleController: Could not get Franka state interface from hardware");
+ return false;
+ }
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("ModelExampleController: Could not read parameter arm_id");
+ return false;
+ }
+ model_interface_ = robot_hw->get<franka_hw::FrankaModelInterface>();
+ if (model_interface_ == nullptr) {
+ ROS_ERROR_STREAM("ModelExampleController: Error getting model interface from hardware");
+ return false;
+ }
+
+ try {
+ franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+ franka_state_interface_->getHandle(arm_id + "_robot"));
+ } catch (const hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "ModelExampleController: Exception getting franka state handle: " << ex.what());
+ 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(
+ "ModelExampleController: Exception getting model handle from interface: " << ex.what());
+ return false;
+ }
+ return true;
+}
+
+void ModelExampleController::update(const ros::Time& /*time*/, const ros::Duration& /*period*/) {
+ if (rate_trigger_()) {
+ std::array<double, 49> mass = model_handle_->getMass();
+ std::array<double, 7> coriolis = model_handle_->getCoriolis();
+ std::array<double, 7> gravity = model_handle_->getGravity();
+ std::array<double, 16> pose = model_handle_->getPose(franka::Frame::kJoint4);
+ std::array<double, 42> joint4_body_jacobian =
+ model_handle_->getBodyJacobian(franka::Frame::kJoint4);
+ std::array<double, 42> endeffector_zero_jacobian =
+ model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+
+ ROS_INFO("--------------------------------------------------");
+ ROS_INFO_STREAM("mass :" << mass);
+ ROS_INFO_STREAM("coriolis: " << coriolis);
+ ROS_INFO_STREAM("gravity :" << gravity);
+ ROS_INFO_STREAM("joint_pose :" << pose);
+ ROS_INFO_STREAM("joint4_body_jacobian :" << joint4_body_jacobian);
+ ROS_INFO_STREAM("joint_zero_jacobian :" << endeffector_zero_jacobian);
+ }
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ModelExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/model_example_controller.h b/fr3_impedance_controller_real/bak/model_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..e1b4aef2dd18c45fe7388db300f6a5c5f78704fb
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/model_example_controller.h
@@ -0,0 +1,34 @@
+// 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 <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+#include <franka_hw/trigger_rate.h>
+
+namespace franka_example_controllers {
+
+class ModelExampleController
+ : public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
+ void update(const ros::Time&, const ros::Duration&) override;
+
+ private:
+ franka_hw::FrankaStateInterface* franka_state_interface_;
+ std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_;
+ franka_hw::FrankaModelInterface* model_interface_;
+ std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+ franka_hw::TriggerRate rate_trigger_{1.0};
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/model_example_controller.launch b/fr3_impedance_controller_real/bak/model_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..61ea5ef7f8825b7bc021b24ffaec8ca7a0e138a4
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/model_example_controller.launch
@@ -0,0 +1,9 @@
+<?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)" />
+ <include file="$(find franka_control)/launch/franka_control.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="model_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/move_to_start.launch b/fr3_impedance_controller_real/bak/move_to_start.launch
new file mode 100644
index 0000000000000000000000000000000000000000..fd9ce503201cc9a59385506eca825abf3deee93d
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/move_to_start.launch
@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot_ip" />
+ <arg name="arm_id" default="panda" />
+ <arg name="transmission" default="effort" doc="The type of position control to use (either 'position' or 'effort')" />
+
+ <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true">
+ <arg name="load_gripper" value="false" />
+ </include>
+
+ <node name="controller_spawner"
+ pkg="controller_manager"
+ type="spawner"
+ respawn="false"
+ output="screen"
+ args="$(arg transmission)_joint_trajectory_controller">
+ </node>
+
+ <node name="move_to_start"
+ pkg="franka_example_controllers"
+ type="move_to_start.py"
+ output="screen"
+ required="true">
+ <rosparam file="$(find franka_control)/config/start_pose.yaml" subst_value="true" />
+ <remap from="~follow_joint_trajectory" to="$(arg transmission)_joint_trajectory_controller/follow_joint_trajectory" />
+ <remap from="~joint_states" to="franka_state_controller/joint_states" />
+ <param name="max_dq" value="0.2" /> <!-- [rad/s] -->
+ </node>
+
+</launch>
diff --git a/fr3_impedance_controller_real/bak/msg/JointTorqueComparison.msg b/fr3_impedance_controller_real/bak/msg/JointTorqueComparison.msg
new file mode 100644
index 0000000000000000000000000000000000000000..c5197324ed2d16df92985ebe664fe9cc96da1d00
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/msg/JointTorqueComparison.msg
@@ -0,0 +1,4 @@
+float64[7] tau_error
+float64[7] tau_commanded
+float64[7] tau_measured
+float64 root_mean_square_error
diff --git a/fr3_impedance_controller_real/bak/package_bak.xml b/fr3_impedance_controller_real/bak/package_bak.xml
new file mode 100644
index 0000000000000000000000000000000000000000..297f2003c5617cd900dc32956aec47fee2b122c8
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/package_bak.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/bak/robot.rviz b/fr3_impedance_controller_real/bak/robot.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..f4f6170c8eb3e76c333cc2b6cd87abaf7fea25b5
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/robot.rviz
@@ -0,0 +1,266 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 89
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /RobotModel1
+ Splitter Ratio: 0.5
+ Tree Height: 278
+ - 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: Image
+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: 0.20000000298023224
+ 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
+ fr3_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_hand_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_hand_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ fr3_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link0_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link1_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link2_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link3_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link4_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link5_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link6_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link7_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fr3_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ fr3_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/MarkerArray
+ Enabled: true
+ Marker Topic: visualization_marker_array
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /camera/color/image_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: fr3_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: 0.6237555146217346
+ 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.363277792930603
+ Y: 0.005851757247000933
+ Z: 0.04891345649957657
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.16539791226387024
+ Target Frame: <Fixed Frame>
+ Yaw: 1.2904071807861328
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1163
+ Hide Left Dock: false
+ Hide Right Dock: true
+ Image:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd000000040000000000000334000003edfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ac000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001ef0000023b0000001600ffffff000000010000010f000003edfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003ed000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000446000003ed00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1920
+ X: 2560
+ Y: 240
diff --git a/fr3_impedance_controller_real/bak/rosdoc.yaml b/fr3_impedance_controller_real/bak/rosdoc.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..96ee597ef5cacd0f223f676c738f396f0810b78c
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/rosdoc.yaml
@@ -0,0 +1,2 @@
+- builder: doxygen
+ javadoc_autobrief: YES
diff --git a/fr3_impedance_controller_real/launch/rviz/franka_description_with_marker.rviz b/fr3_impedance_controller_real/bak/rviz/franka_description_with_marker.rviz
similarity index 100%
rename from fr3_impedance_controller_real/launch/rviz/franka_description_with_marker.rviz
rename to fr3_impedance_controller_real/bak/rviz/franka_description_with_marker.rviz
diff --git a/fr3_impedance_controller_real/bak/rviz/franka_dual_description_with_marker.rviz b/fr3_impedance_controller_real/bak/rviz/franka_dual_description_with_marker.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..bdd4b593a530fb3c89e7423473e06f7e7635fa04
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/rviz/franka_dual_description_with_marker.rviz
@@ -0,0 +1,281 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /RobotModel1
+ - /InteractiveMarkers1
+ Splitter Ratio: 0.655555546
+ Tree Height: 820
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679016
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+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.0299999993
+ 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
+ - Class: rviz/TF
+ Enabled: false
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ {}
+ Update Interval: 0
+ Value: false
+ - 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
+ base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: panda_dual/robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/InteractiveMarkers
+ Enable Transparency: true
+ Enabled: true
+ Name: InteractiveMarkers
+ Show Axes: true
+ Show Descriptions: true
+ Show Visual Aids: true
+ Update Topic: /panda_dual/target_pose_marker/update
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: base
+ 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
+ Topic: /initialpose
+ - 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: 2.63399172
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0.42157951
+ Y: -0.0423400775
+ Z: 1.508255
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.00999999978
+ Pitch: 0.429795891
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 6.16679144
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1020
+ Hide Left Dock: false
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a00000375fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000375000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001bc000001e20000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ce000001d10000000000000000fb0000000a0049006d00610067006500000001a9000001f50000000000000000000000010000010f00000365fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f00000365000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003bfc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1855
+ X: 65
+ Y: 28
diff --git a/fr3_impedance_controller_real/bak/rviz/teleop_joint_pd_example.rviz b/fr3_impedance_controller_real/bak/rviz/teleop_joint_pd_example.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..4fc200b9bfa79f776f2378c41504d0d81103db43
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/rviz/teleop_joint_pd_example.rviz
@@ -0,0 +1,396 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /RobotModel1
+ - /WrenchStampedSlave1
+ - /MarkerArray1
+ Splitter Ratio: 0.4892857074737549
+ Tree Height: 809
+ - 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
+ Experimental: false
+ 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
+ - Class: rviz/TF
+ Enabled: false
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ {}
+ Update Interval: 0
+ Value: false
+ - 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
+ base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_hand_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_hand_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ follower_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link0_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link1_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link2_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link3_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link4_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link5_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link6_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link7_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ follower_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ follower_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_hand_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_hand_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ leader_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link0_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link1_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link2_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link3_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link4_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link5_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link6_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link7_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ leader_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ leader_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: panda_teleop/robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Alpha: 1
+ Arrow Width: 0.20000000298023224
+ Class: rviz/WrenchStamped
+ Enabled: true
+ Force Arrow Scale: 0.05000000074505806
+ Force Color: 204; 51; 51
+ Hide Small Values: true
+ History Length: 1
+ Name: WrenchStampedSlave
+ Topic: /panda_teleop/follower_state_controller/F_ext
+ Torque Arrow Scale: 0.20000000298023224
+ Torque Color: 204; 204; 51
+ Unreliable: false
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /panda_teleop/teleop_joint_pd_example_controller/marker_labels
+ Name: MarkerArray
+ Namespaces:
+ basic_shapes: true
+ Queue Size: 100
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: base
+ 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: 4.382266998291016
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: -0.43465375900268555
+ Y: 0.19732384383678436
+ Z: 0.8964318633079529
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.3903988003730774
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 6.2585835456848145
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: true
+ Height: 1025
+ Hide Left Dock: true
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd0000000400000000000003fd00000366fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000366000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001bc000001e20000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ce000001d10000000000000000fb0000000a0049006d00610067006500000001a9000001f50000000000000000000000010000010f00000365fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f00000365000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003bfc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000073d0000036600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1853
+ X: 67
+ Y: 27
diff --git a/fr3_impedance_controller_real/bak/scripts/dual_arm_interactive_marker.py b/fr3_impedance_controller_real/bak/scripts/dual_arm_interactive_marker.py
new file mode 100755
index 0000000000000000000000000000000000000000..a26c2d9094eb142c5ca2280d80587cf48c4370ee
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/scripts/dual_arm_interactive_marker.py
@@ -0,0 +1,229 @@
+#!/usr/bin/env python
+""" This simple script creates an interactive marker for changing desired centering pose of
+ two the dual_panda_cartesian_impedance_example_controller. It features also resetting the
+ marker to current centering pose between the left and the right endeffector.
+"""
+
+import rospy
+import argparse
+
+from interactive_markers.interactive_marker_server import \
+ InteractiveMarkerServer, InteractiveMarkerFeedback
+from visualization_msgs.msg import InteractiveMarker, \
+ InteractiveMarkerControl, Marker
+from franka_msgs.msg import FrankaState
+from geometry_msgs.msg import PoseStamped
+
+marker_pose = PoseStamped()
+
+has_error = False
+left_has_error = False
+right_has_error = False
+
+pose_pub = None
+
+
+def make_sphere(scale=0.3):
+ """
+ This function returns sphere marker for 3D translational movements.
+ :param scale: scales the size of the sphere
+ :return: sphere marker
+ """
+ marker = Marker()
+ marker.type = Marker.SPHERE
+ marker.scale.x = scale * 0.45
+ marker.scale.y = scale * 0.45
+ marker.scale.z = scale * 0.45
+ marker.color.r = 0.5
+ marker.color.g = 0.5
+ marker.color.b = 0.5
+ marker.color.a = 1.0
+ return marker
+
+
+def publish_target_pose():
+ """
+ This function publishes desired centering pose which the controller will subscribe to.
+ :return: None
+ """
+ marker_pose.header.stamp = rospy.Time(0)
+ pose_pub.publish(marker_pose)
+
+
+def left_franka_state_callback(msg):
+ """
+ This callback function set `has_error` variable to True if the left arm is having an error.
+ :param msg: FrankaState msg data
+ :return: None
+ """
+ global has_error, left_has_error
+ if msg.robot_mode == FrankaState.ROBOT_MODE_MOVE:
+ left_has_error = False
+ else:
+ left_has_error = True
+ has_error = left_has_error or right_has_error
+
+
+def right_franka_state_callback(msg):
+ """
+ This callback function set `has_error` variable to True if the right arm is having an error.
+ :param msg: FrankaState msg data
+ :return: None
+ """
+ global has_error, right_has_error
+ if msg.robot_mode == FrankaState.ROBOT_MODE_MOVE:
+ right_has_error = False
+ else:
+ right_has_error = True
+ has_error = left_has_error or right_has_error
+
+
+def reset_marker_pose_blocking():
+ """
+ This function resets the marker pose to current "middle pose" of left and right arm EEs.
+ :return: None
+ """
+
+ global marker_pose
+ marker_pose = rospy.wait_for_message(
+ "dual_arm_cartesian_impedance_example_controller/centering_frame", PoseStamped)
+
+
+def process_feedback(feedback):
+ """
+ This callback function clips the marker_pose inside a predefined box to prevent misuse of the
+ marker.
+ :param feedback: feedback data of interactive marker
+ :return: None
+ """
+ global marker_pose
+ if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
+ marker_pose.pose.position.x = feedback.pose.position.x
+ marker_pose.pose.position.y = feedback.pose.position.y
+ marker_pose.pose.position.z = feedback.pose.position.z
+ marker_pose.pose.orientation = feedback.pose.orientation
+ server.applyChanges()
+
+
+if __name__ == "__main__":
+ rospy.init_node("target_pose_node")
+
+ parser = argparse.ArgumentParser("dual_panda_interactive_marker.py")
+ parser.add_argument("--left_arm_id",
+ help="The id of the left arm.",
+ required=True)
+ parser.add_argument("--right_arm_id",
+ help="The id of the right arm.",
+ required=True)
+ parser.add_argument('args', nargs=argparse.REMAINDER)
+ args = parser.parse_args()
+
+ # Arm IDs of left and right arms
+ left_arm_id = args.left_arm_id
+ right_arm_id = args.right_arm_id
+
+ # Initialize subscribers for error states of the arms
+ left_state_sub = rospy.Subscriber(left_arm_id + "_state_controller/franka_states",
+ FrankaState, left_franka_state_callback)
+
+ right_state_sub = rospy.Subscriber(right_arm_id + "_state_controller/franka_states",
+ FrankaState, right_franka_state_callback)
+
+ # Set marker pose to be the current "middle pose" of both EEs
+ reset_marker_pose_blocking()
+
+ # Initialize publisher for publishing desired centering pose
+ pose_pub = rospy.Publisher(
+ "dual_arm_cartesian_impedance_example_controller/centering_frame_target_pose",
+ PoseStamped,
+ queue_size=1)
+
+ # Interactive marker settings
+ server = InteractiveMarkerServer("target_pose_marker")
+ int_marker = InteractiveMarker()
+ int_marker.header.frame_id = marker_pose.header.frame_id
+ int_marker.scale = 0.3
+ int_marker.name = "centering_frame_pose"
+ int_marker.description = ("Target centering pose\n"
+ "BE CAREFUL! \n"
+ "If you move the target marker\n"
+ "both robots will follow it \n"
+ "as the center between the two\n"
+ "endeffectors. Be aware of\n"
+ "potential collisions!")
+ int_marker.pose = marker_pose.pose
+
+ # insert a box
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 0
+ control.orientation.z = 0
+ control.name = "rotate_x"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 0
+ control.orientation.z = 0
+ control.name = "move_x"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 1
+ control.orientation.z = 0
+ control.name = "rotate_y"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 1
+ control.orientation.z = 0
+ control.name = "move_y"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 0
+ control.orientation.z = 1
+ control.name = "rotate_z"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 0
+ control.orientation.z = 1
+ control.name = "move_z"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 1
+ control.orientation.z = 1
+ control.name = "move_3D"
+ control.always_visible = True
+ control.markers.append(make_sphere())
+ control.interaction_mode = InteractiveMarkerControl.MOVE_3D
+ int_marker.controls.append(control)
+
+ server.insert(int_marker, process_feedback)
+ server.applyChanges()
+
+ # main loop
+ while not rospy.is_shutdown():
+ publish_target_pose()
+ if has_error:
+ reset_marker_pose_blocking()
+ publish_target_pose()
+ server.setPose("centering_frame_pose", marker_pose.pose)
+ server.applyChanges()
+ rospy.sleep(0.5)
+ rospy.sleep(0.1)
diff --git a/fr3_impedance_controller_real/bak/scripts/interactive_marker.py b/fr3_impedance_controller_real/bak/scripts/interactive_marker.py
new file mode 100755
index 0000000000000000000000000000000000000000..1893debeb98abc733fe20d07cf0a60a001400cba
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/scripts/interactive_marker.py
@@ -0,0 +1,137 @@
+#!/usr/bin/env python
+
+import rospy
+import tf.transformations
+import numpy as np
+
+from interactive_markers.interactive_marker_server import \
+ InteractiveMarkerServer, InteractiveMarkerFeedback
+from visualization_msgs.msg import InteractiveMarker, \
+ InteractiveMarkerControl
+from geometry_msgs.msg import PoseStamped
+from franka_msgs.msg import FrankaState
+
+marker_pose = PoseStamped()
+pose_pub = None
+# [[min_x, max_x], [min_y, max_y], [min_z, max_z]]
+position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]]
+
+
+def publisher_callback(msg, link_name):
+ marker_pose.header.frame_id = link_name
+ marker_pose.header.stamp = rospy.Time(0)
+ pose_pub.publish(marker_pose)
+
+
+def process_feedback(feedback):
+ if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
+ marker_pose.pose.position.x = max([min([feedback.pose.position.x,
+ position_limits[0][1]]),
+ position_limits[0][0]])
+ marker_pose.pose.position.y = max([min([feedback.pose.position.y,
+ position_limits[1][1]]),
+ position_limits[1][0]])
+ marker_pose.pose.position.z = max([min([feedback.pose.position.z,
+ position_limits[2][1]]),
+ position_limits[2][0]])
+ marker_pose.pose.orientation = feedback.pose.orientation
+ server.applyChanges()
+
+
+def wait_for_initial_pose():
+ msg = rospy.wait_for_message("franka_state_controller/franka_states",
+ FrankaState) # type: FrankaState
+
+ initial_quaternion = \
+ tf.transformations.quaternion_from_matrix(
+ np.transpose(np.reshape(msg.O_T_EE,
+ (4, 4))))
+ initial_quaternion = initial_quaternion / \
+ np.linalg.norm(initial_quaternion)
+ marker_pose.pose.orientation.x = initial_quaternion[0]
+ marker_pose.pose.orientation.y = initial_quaternion[1]
+ marker_pose.pose.orientation.z = initial_quaternion[2]
+ marker_pose.pose.orientation.w = initial_quaternion[3]
+ marker_pose.pose.position.x = msg.O_T_EE[12]
+ marker_pose.pose.position.y = msg.O_T_EE[13]
+ marker_pose.pose.position.z = msg.O_T_EE[14]
+
+
+if __name__ == "__main__":
+ rospy.init_node("equilibrium_pose_node")
+ listener = tf.TransformListener()
+ link_name = rospy.get_param("~link_name")
+
+ wait_for_initial_pose()
+
+ pose_pub = rospy.Publisher(
+ "equilibrium_pose", PoseStamped, queue_size=10)
+ server = InteractiveMarkerServer("equilibrium_pose_marker")
+ int_marker = InteractiveMarker()
+ int_marker.header.frame_id = link_name
+ int_marker.scale = 0.3
+ int_marker.name = "equilibrium_pose"
+ int_marker.description = ("Equilibrium Pose\nBE CAREFUL! "
+ "If you move the \nequilibrium "
+ "pose the robot will follow it\n"
+ "so be aware of potential collisions")
+ int_marker.pose = marker_pose.pose
+ # run pose publisher
+ rospy.Timer(rospy.Duration(0.005),
+ lambda msg: publisher_callback(msg, link_name))
+
+ # insert a box
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 0
+ control.orientation.z = 0
+ control.name = "rotate_x"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 0
+ control.orientation.z = 0
+ control.name = "move_x"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 1
+ control.orientation.z = 0
+ control.name = "rotate_y"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 1
+ control.orientation.z = 0
+ control.name = "move_y"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 0
+ control.orientation.z = 1
+ control.name = "rotate_z"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 0
+ control.orientation.z = 1
+ control.name = "move_z"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ server.insert(int_marker, process_feedback)
+
+ server.applyChanges()
+
+ rospy.spin()
diff --git a/fr3_impedance_controller_real/bak/scripts/move_to_start.py b/fr3_impedance_controller_real/bak/scripts/move_to_start.py
new file mode 100755
index 0000000000000000000000000000000000000000..0e75e3dde5b1156647922d573d03ad40b5daacdc
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/scripts/move_to_start.py
@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+
+import sys
+import rospy as ros
+
+from actionlib import SimpleActionClient
+from sensor_msgs.msg import JointState
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from control_msgs.msg import FollowJointTrajectoryAction, \
+ FollowJointTrajectoryGoal, FollowJointTrajectoryResult
+
+ros.init_node('move_to_start')
+
+action = ros.resolve_name('~follow_joint_trajectory')
+client = SimpleActionClient(action, FollowJointTrajectoryAction)
+ros.loginfo("move_to_start: Waiting for '" + action + "' action to come up")
+client.wait_for_server()
+
+param = ros.resolve_name('~joint_pose')
+pose = ros.get_param(param, None)
+if pose is None:
+ ros.logerr('move_to_start: Could not find required parameter "' + param + '"')
+ sys.exit(1)
+
+topic = ros.resolve_name('~joint_states')
+ros.loginfo("move_to_start: Waiting for message on topic '" + topic + "'")
+joint_state = ros.wait_for_message(topic, JointState)
+initial_pose = dict(zip(joint_state.name, joint_state.position))
+
+max_movement = max(abs(pose[joint] - initial_pose[joint]) for joint in pose)
+
+point = JointTrajectoryPoint()
+point.time_from_start = ros.Duration.from_sec(
+ # Use either the time to move the furthest joint with 'max_dq' or 500ms,
+ # whatever is greater
+ max(max_movement / ros.get_param('~max_dq', 0.5), 0.5)
+)
+goal = FollowJointTrajectoryGoal()
+
+goal.trajectory.joint_names, point.positions = [list(x) for x in zip(*pose.items())]
+point.velocities = [0] * len(pose)
+
+goal.trajectory.points.append(point)
+goal.goal_time_tolerance = ros.Duration.from_sec(0.5)
+
+ros.loginfo('Sending trajectory Goal to move into initial config')
+client.send_goal_and_wait(goal)
+
+result = client.get_result()
+if result.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
+ ros.logerr('move_to_start: Movement was not successful: ' + {
+ FollowJointTrajectoryResult.INVALID_GOAL:
+ """
+ The joint pose you want to move to is invalid (e.g. unreachable, singularity...).
+ Is the 'joint_pose' reachable?
+ """,
+
+ FollowJointTrajectoryResult.INVALID_JOINTS:
+ """
+ The joint pose you specified is for different joints than the joint trajectory controller
+ is claiming. Does you 'joint_pose' include all 7 joints of the robot?
+ """,
+
+ FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED:
+ """
+ During the motion the robot deviated from the planned path too much. Is something blocking
+ the robot?
+ """,
+
+ FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED:
+ """
+ After the motion the robot deviated from the desired goal pose too much. Probably the robot
+ didn't reach the joint_pose properly
+ """,
+ }[result.error_code])
+
+else:
+ ros.loginfo('move_to_start: Successfully moved into start pose')
diff --git a/fr3_impedance_controller_real/bak/spacenav_teleop_twist_controller.cpp b/fr3_impedance_controller_real/bak/spacenav_teleop_twist_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..680cb97212a10731376076ae3563e49c10b632f8
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/spacenav_teleop_twist_controller.cpp
@@ -0,0 +1,214 @@
+// 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/spacenav_teleop_twist_controller.h>
+
+#include <array>
+#include <cmath>
+#include <memory>
+#include <string>
+
+#include <actionlib/client/simple_action_client.h>
+#include <franka_gripper/MoveAction.h>
+#include <franka_gripper/StopAction.h>
+#include <franka_gripper/franka_gripper.h>
+#include <controller_interface/controller_base.h>
+#include <hardware_interface/hardware_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+namespace franka_example_controllers {
+
+bool SpacenavTeleopTwistController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("SpacenavTeleopTwistController: Could not get parameter arm_id");
+ return false;
+ }
+
+ velocity_cartesian_interface_ =
+ robot_hardware->get<franka_hw::FrankaVelocityCartesianInterface>();
+ if (velocity_cartesian_interface_ == nullptr) {
+ ROS_ERROR(
+ "SpacenavTeleopTwistController: Could not get Cartesian velocity interface from "
+ "hardware");
+ return false;
+ }
+ try {
+ velocity_cartesian_handle_ = std::make_unique<franka_hw::FrankaCartesianVelocityHandle>(
+ velocity_cartesian_interface_->getHandle(arm_id + "_robot"));
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "SpacenavTeleopTwistController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("SpacenavTeleopTwistController: Could not get state interface from hardware");
+ return false;
+ }
+
+ if (!node_handle.getParam("linear_gain", linear_gain_)) {
+ ROS_ERROR("SpacenavTeleopTwistController: Could not get parameter linear_gain");
+ return false;
+ }
+ ROS_INFO_STREAM("SpacenavTeleopTwistController: Using linear_gain " << linear_gain_);
+ if (!node_handle.getParam("angular_gain", angular_gain_)) {
+ ROS_ERROR("SpacenavTeleopTwistController: Could not get parameter angular_gain");
+ return false;
+ }
+ ROS_INFO_STREAM("SpacenavTeleopTwistController: Using angular_gain " << angular_gain_);
+
+ ac_move = new actionlib::SimpleActionClient<franka_gripper::MoveAction>("franka_gripper/move", false);
+ ac_stop = new actionlib::SimpleActionClient<franka_gripper::StopAction>("franka_gripper/stop", false);
+ ROS_INFO("Waiting for MoveAction server to start.");
+ ac_move->waitForServer();
+ //Subscribing to relevant twist topic
+ twist_sub_ = node_handle.subscribe<geometry_msgs::Twist>("/spacenav/twist", 1, &SpacenavTeleopTwistController::twistCallback, this);
+ //Subscribing to joy topic for button information
+ joy_sub_ = node_handle.subscribe<sensor_msgs::Joy>("/spacenav/joy", 1, &SpacenavTeleopTwistController::joyCallback, this);
+ //initializing variables
+ jerk_limit = 75;
+
+ /**
+ try {
+ auto state_handle = state_interface->getHandle(arm_id + "_robot");
+
+ std::array<double, 7> q_start = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "CartesianVelocityExampleController: Robot is not in the expected starting position "
+ "for running this example. Run `roslaunch franka_example_controllers "
+ "move_to_start.launch robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` "
+ "first.");
+ return false;
+ }
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianVelocityExampleController: Exception getting state handle: " << e.what());
+ return false;
+ }
+ **/
+
+ return true;
+}
+
+void SpacenavTeleopTwistController::starting(const ros::Time& /* time */) {
+ elapsed_time_ = ros::Duration(0.0);
+ std::array<double, 6> command = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+ std::array<double, 6> command_prev = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+ std::array<double, 6> accel = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+ std::array<double, 6> accel_prev = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+ velocity_cartesian_handle_->setCommand(command);
+}
+
+void SpacenavTeleopTwistController::update(const ros::Time& /* time */,
+ const ros::Duration& period) {
+
+ double steptime = period.toSec();
+ // ROS_INFO("Steptime: %f", steptime);
+ std::array<double, 6> command = {{twist_command.linear.x,
+ twist_command.linear.y,
+ twist_command.linear.z,
+ twist_command.angular.x,
+ twist_command.angular.y,
+ twist_command.angular.z}};
+ std::array<double, 6> accel = {{(command[0]-command_prev[0])/steptime,
+ (command[1]-command_prev[1])/steptime,
+ (command[2]-command_prev[2])/steptime,
+ (command[3]-command_prev[3])/steptime,
+ (command[4]-command_prev[4])/steptime,
+ (command[5]-command_prev[5])/steptime}};
+ std::array<double, 6> jerk = {{(accel[0]-accel_prev[0])/steptime,
+ (accel[1]-accel_prev[1])/steptime,
+ (accel[2]-accel_prev[2])/steptime,
+ (accel[3]-accel_prev[3])/steptime,
+ (accel[4]-accel_prev[4])/steptime,
+ (accel[5]-accel_prev[5])/steptime}};
+
+
+ //scale down acc and then command to be
+ for (int i = 0; i < 6; i++)
+ {
+ if (jerk[i]>= jerk_limit)
+ {
+ accel[i] = (jerk_limit * steptime) + accel_prev[i];
+ command[i] = (accel[i] * steptime) + command_prev[i];
+ }
+ else if (jerk[i]<= -jerk_limit)
+ {
+ accel[i] = (-jerk_limit * steptime) + accel_prev[i];
+ command[i] = (accel[i] * steptime) + command_prev[i];
+ }
+ }
+
+ command_prev = {{command[0],
+ command[1],
+ command[2],
+ command[3],
+ command[4],
+ command[5]}};
+ accel_prev = {{accel[0],
+ accel[1],
+ accel[2],
+ accel[3],
+ accel[4],
+ accel[5]}};
+ velocity_cartesian_handle_->setCommand(command);
+ //Gripper logic
+ int idx = -1;
+ int *foo = std::find(std::begin(button_command), std::end(button_command), 1);
+ actionlib::SimpleClientGoalState state = ac_move->getState();
+ //ROS_INFO("Current state: %s", state.toString().c_str());
+ // When the element is not found, std::find returns the end of the range
+ if (foo != std::end(button_command)) {
+ idx = std::distance(std::begin(button_command), foo);
+ if (state.toString() != "ACTIVE") {
+ if (idx == 0) {
+ move_goal.width = 0.1;
+ move_goal.speed = 0.01;
+ ac_move->sendGoal(move_goal);
+ }
+ else if (idx == 1) {
+ move_goal.width = 0.0;
+ move_goal.speed = 0.01;
+ ac_move->sendGoal(move_goal);
+ }
+ }
+ } else {
+ if (state.toString() == "ACTIVE") {
+ ac_move->cancelAllGoals();
+ ac_stop->sendGoal(stop_goal);
+ }
+ }
+
+}
+
+void SpacenavTeleopTwistController::stopping(const ros::Time& /*time*/) {
+ // WARNING: DO NOT SEND ZERO VELOCITIES HERE AS IN CASE OF ABORTING DURING MOTION
+ // A JUMP TO ZERO WILL BE COMMANDED PUTTING HIGH LOADS ON THE ROBOT. LET THE DEFAULT
+ // BUILT-IN STOPPING BEHAVIOR SLOW DOWN THE ROBOT.
+}
+
+void SpacenavTeleopTwistController::twistCallback(const geometry_msgs::TwistConstPtr& msg) {
+ twist_command.linear.x = linear_gain_ * msg->linear.x;
+ twist_command.linear.y = linear_gain_ * msg->linear.y;
+ twist_command.linear.z = linear_gain_ * msg->linear.z;
+ twist_command.angular.x = angular_gain_ * msg->angular.x;
+ twist_command.angular.y = angular_gain_ * msg->angular.y;
+ twist_command.angular.z = angular_gain_ * msg->angular.z;
+}
+
+void SpacenavTeleopTwistController::joyCallback(const sensor_msgs::JoyConstPtr& msg) {
+ button_command[0] = msg->buttons[0];
+ button_command[1] = msg->buttons[1];
+}
+
+}// namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::SpacenavTeleopTwistController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/spacenav_teleop_twist_controller.h b/fr3_impedance_controller_real/bak/spacenav_teleop_twist_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..97beea742510a21d1f81fc4fd7c7fc7e8b42fec1
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/spacenav_teleop_twist_controller.h
@@ -0,0 +1,52 @@
+// 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 <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <franka_hw/franka_state_interface.h>
+#include <actionlib/client/simple_action_client.h>
+#include <franka_gripper/MoveAction.h>
+#include <franka_gripper/StopAction.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/Joy.h>
+
+namespace franka_example_controllers {
+
+class SpacenavTeleopTwistController : public controller_interface::MultiInterfaceController<
+ franka_hw::FrankaVelocityCartesianInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+ void starting(const ros::Time&) override;
+ void stopping(const ros::Time&) override;
+
+ private:
+ franka_hw::FrankaVelocityCartesianInterface* velocity_cartesian_interface_;
+ std::unique_ptr<franka_hw::FrankaCartesianVelocityHandle> velocity_cartesian_handle_;
+ ros::Duration elapsed_time_;
+ ros::Subscriber twist_sub_;
+ ros::Subscriber joy_sub_;
+ actionlib::SimpleActionClient<franka_gripper::MoveAction> *ac_move;
+ actionlib::SimpleActionClient<franka_gripper::StopAction> *ac_stop;
+ double linear_gain_;
+ double angular_gain_;
+ void twistCallback(const geometry_msgs::TwistConstPtr& msg);
+ void joyCallback(const sensor_msgs::JoyConstPtr& msg);
+ geometry_msgs::Twist twist_command;
+ std::array<int, 2> button_command;
+ std::array<double, 6> command_prev;
+ std::array<double, 6> accel_prev;
+ double jerk_limit;
+ franka_gripper::MoveGoal move_goal;
+ franka_gripper::StopGoal stop_goal;
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/spacenav_teleop_twist_controller.launch b/fr3_impedance_controller_real/bak/spacenav_teleop_twist_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..73815da630499d86d61dc12f3feb5758b29662d7
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/spacenav_teleop_twist_controller.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
+ <arg name="arm_id" default="$(arg robot)" />
+ <arg name="linear_gain" default="0.01" />
+ <arg name="angular_gain" default="0.01" />
+ <include file="$(find spacenav_node)/launch/classic.launch" pass_all_args="true"/>
+ <include file="$(find franka_control)/launch/franka_control.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="spacenav_teleop_twist_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/teleop_gripper.launch b/fr3_impedance_controller_real/bak/teleop_gripper.launch
new file mode 100644
index 0000000000000000000000000000000000000000..ddb758977c7180aafe6a165f6f159a0ee05d33e8
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/teleop_gripper.launch
@@ -0,0 +1,35 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot_id" />
+ <arg name="leader_ip" />
+ <arg name="follower_ip" />
+ <arg name="leader_id" default="panda_1" />
+ <arg name="follower_id" default="panda_2" />
+ <arg name="gripper_homed" default="false" />
+
+ <group ns="$(arg leader_id)">
+ <include file="$(find franka_gripper)/launch/franka_gripper.launch" >
+ <arg name="robot_ip" value="$(arg leader_ip)" />
+ <arg name="stop_at_shutdown" value="true" />
+ <arg name="arm_id" value="$(arg leader_id)" />
+ </include>
+ </group>
+
+ <group ns="$(arg follower_id)">
+ <include file="$(find franka_gripper)/launch/franka_gripper.launch" >
+ <arg name="robot_ip" value="$(arg follower_ip)" />
+ <arg name="stop_at_shutdown" value="true" />
+ <arg name="arm_id" value="$(arg follower_id)" />
+ </include>
+ </group>
+
+ <node name="teleop_gripper_node" pkg="franka_example_controllers" type="teleop_gripper_node" respawn="false" output="screen" >
+ <rosparam param="gripper_homed" subst_value="true">$(arg gripper_homed)</rosparam>
+ <remap from="~leader/joint_states" to="$(arg leader_id)/franka_gripper/joint_states" />
+ <remap from="leader/homing" to="$(arg leader_id)/franka_gripper/homing" />
+ <remap from="follower/homing" to="$(arg follower_id)/franka_gripper/homing" />
+ <remap from="follower/grasp" to="$(arg follower_id)/franka_gripper/grasp" />
+ <remap from="follower/move" to="$(arg follower_id)/franka_gripper/move" />
+ <remap from="follower/stop" to="$(arg follower_id)/franka_gripper/stop" />
+ </node>
+</launch>
diff --git a/fr3_impedance_controller_real/bak/teleop_gripper_node.cpp b/fr3_impedance_controller_real/bak/teleop_gripper_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..231805ff3185b84a3e9e038feaf9d607cd15f229
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/teleop_gripper_node.cpp
@@ -0,0 +1,190 @@
+// Copyright (c) 2020 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka/gripper.h>
+#include <franka_example_controllers/teleop_gripper_paramConfig.h>
+#include <franka_gripper/GraspAction.h>
+#include <franka_gripper/HomingAction.h>
+#include <franka_gripper/MoveAction.h>
+#include <franka_gripper/StopAction.h>
+
+#include <actionlib/client/simple_action_client.h>
+#include <dynamic_reconfigure/server.h>
+#include <ros/init.h>
+#include <ros/node_handle.h>
+#include <sensor_msgs/JointState.h>
+
+#include <functional>
+#include <memory>
+#include <mutex>
+
+using franka_gripper::GraspAction;
+using franka_gripper::HomingAction;
+using franka_gripper::MoveAction;
+using franka_gripper::StopAction;
+using GraspClient = actionlib::SimpleActionClient<GraspAction>;
+using HomingClient = actionlib::SimpleActionClient<HomingAction>;
+using MoveClient = actionlib::SimpleActionClient<MoveAction>;
+using StopClient = actionlib::SimpleActionClient<StopAction>;
+
+/**
+ * Client class for teleoperating a follower gripper from a leader gripper.
+ * By closing the leader gripper manually, exceeding a defined width threshold, a ROS
+ * action is called and the follower gripper will grasp an object with a configurable force.
+ * When opening the leader gripper, the follower gripper will also open.
+ */
+class TeleopGripperClient {
+ public:
+ TeleopGripperClient()
+ : leader_homing_client_("leader/homing", true),
+ follower_homing_client_("follower/homing", true),
+ grasp_client_("follower/grasp", true),
+ move_client_("follower/move", true),
+ stop_client_("follower/stop", true){};
+
+ bool init(const std::shared_ptr<ros::NodeHandle>& pnh) {
+ grasping_ = false;
+ gripper_homed_ = false;
+ if (!pnh->getParam("gripper_homed", gripper_homed_)) {
+ ROS_INFO_STREAM(
+ "teleop_gripper_node: Could not read parameter gripper_homed. "
+ "Defaulting to "
+ << std::boolalpha << gripper_homed_);
+ }
+
+ // Init for dynamic reconfigure
+ dynamic_reconfigure_teleop_gripper_param_node_ =
+ ros::NodeHandle("dyn_reconf_teleop_gripper_param_node");
+ dynamic_server_teleop_gripper_param_ = std::make_unique<
+ dynamic_reconfigure::Server<franka_example_controllers::teleop_gripper_paramConfig>>(
+ dynamic_reconfigure_teleop_gripper_param_node_);
+ dynamic_server_teleop_gripper_param_->setCallback(
+ boost::bind(&TeleopGripperClient::teleopGripperParamCallback, this, _1, _2));
+
+ bool homing_success(false);
+ if (!gripper_homed_) {
+ ROS_INFO("teleop_gripper_node: Homing Gripper.");
+ homing_success = homingGripper();
+ }
+
+ if (gripper_homed_ || homing_success) {
+ // Start action servers and subscriber for gripper
+ ros::Duration timeout(2.0);
+ if (grasp_client_.waitForServer(timeout) && move_client_.waitForServer(timeout) &&
+ stop_client_.waitForServer(timeout)) {
+ leader_sub_ = pnh->subscribe("leader/joint_states", 1,
+ &TeleopGripperClient::subscriberCallback, this);
+ } else {
+ ROS_ERROR(
+ "teleop_gripper_node: Action Server could not be started. Shutting "
+ "down node.");
+ return false;
+ }
+ } else {
+ return false;
+ }
+ return true;
+ };
+
+ private:
+ double max_width_{0.07}; // Default value. It will be reset when gripper is homed [m]
+ bool grasping_;
+ bool gripper_homed_;
+
+ double grasp_force_; // [N]
+ double grasp_epsilon_inner_{0.001}; // [m]
+ double grasp_epsilon_outer_scaling_{100};
+ double move_speed_; // [m/s]
+
+ double start_pos_grasping_{0.5}; // Threshold position of leader gripper where to start grasping.
+ double start_pos_opening_{0.6}; // Threshold position of leader gripper where to open.
+
+ HomingClient follower_homing_client_;
+ HomingClient leader_homing_client_;
+ GraspClient grasp_client_;
+ MoveClient move_client_;
+ StopClient stop_client_;
+
+ std::mutex subscriber_mutex_;
+ ros::Subscriber leader_sub_;
+
+ std::mutex dynamic_reconfigure_mutex_;
+ ros::NodeHandle dynamic_reconfigure_teleop_gripper_param_node_;
+ std::unique_ptr<
+ dynamic_reconfigure::Server<franka_example_controllers::teleop_gripper_paramConfig>>
+ dynamic_server_teleop_gripper_param_;
+
+ void teleopGripperParamCallback(
+ const franka_example_controllers::teleop_gripper_paramConfig& config,
+ uint32_t /*level*/) {
+ if (dynamic_reconfigure_mutex_.try_lock()) {
+ grasp_force_ = config.grasp_force;
+ move_speed_ = config.move_speed;
+ ROS_INFO_STREAM("Dynamic Reconfigure: Gripper params set: grasp_force = "
+ << grasp_force_ << " N ; move_speed = " << move_speed_ << " m/s");
+ }
+ dynamic_reconfigure_mutex_.unlock();
+ };
+
+ bool homingGripper() {
+ if (follower_homing_client_.waitForServer(ros::Duration(2.0)) &&
+ leader_homing_client_.waitForServer(ros::Duration(2.0))) {
+ leader_homing_client_.sendGoal(franka_gripper::HomingGoal());
+ follower_homing_client_.sendGoal(franka_gripper::HomingGoal());
+
+ if (leader_homing_client_.waitForResult(ros::Duration(10.0)) &&
+ follower_homing_client_.waitForResult(ros::Duration(10.0))) {
+ return true;
+ }
+ }
+ ROS_ERROR("teleop_gripper_node: HomingAction has timed out.");
+ return false;
+ }
+
+ void subscriberCallback(const sensor_msgs::JointState& msg) {
+ std::lock_guard<std::mutex> _(subscriber_mutex_);
+ if (!gripper_homed_) {
+ // If gripper had to be homed, reset max_width_.
+ max_width_ = 2 * msg.position[0];
+ gripper_homed_ = true;
+ }
+ double gripper_width = 2 * msg.position[0];
+ if (gripper_width < start_pos_grasping_ * max_width_ && !grasping_) {
+ // Grasp object
+ franka_gripper::GraspGoal grasp_goal;
+ grasp_goal.force = grasp_force_;
+ grasp_goal.speed = move_speed_;
+ grasp_goal.epsilon.inner = grasp_epsilon_inner_;
+ grasp_goal.epsilon.outer = grasp_epsilon_outer_scaling_ * max_width_;
+
+ grasp_client_.sendGoal(grasp_goal);
+ if (grasp_client_.waitForResult(ros::Duration(5.0))) {
+ grasping_ = true;
+ } else {
+ ROS_INFO("teleop_gripper_node: GraspAction was not successful.");
+ stop_client_.sendGoal(franka_gripper::StopGoal());
+ }
+ } else if (gripper_width > start_pos_opening_ * max_width_ && grasping_) {
+ // Open gripper
+ franka_gripper::MoveGoal move_goal;
+ move_goal.speed = move_speed_;
+ move_goal.width = max_width_;
+ move_client_.sendGoal(move_goal);
+ if (move_client_.waitForResult(ros::Duration(5.0))) {
+ grasping_ = false;
+ } else {
+ ROS_ERROR("teleop_gripper_node: MoveAction was not successful.");
+ stop_client_.sendGoal(franka_gripper::StopGoal());
+ }
+ }
+ }
+};
+
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "teleop_gripper_node");
+ auto pnh = std::make_shared<ros::NodeHandle>("~");
+ TeleopGripperClient teleop_gripper_client;
+ if (teleop_gripper_client.init(pnh)) {
+ ros::spin();
+ }
+ return 0;
+}
diff --git a/fr3_impedance_controller_real/bak/teleop_gripper_param.cfg b/fr3_impedance_controller_real/bak/teleop_gripper_param.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..5f0d9e14310d5012f5655c2e2459b1a1c08218d0
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/teleop_gripper_param.cfg
@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gripper_param = gen.add_group("GripperParameter", type="apply")
+
+gripper_param.add("grasp_force", double_t, 0,
+ "Grasping force to be applied on an object. [N]", 40.0, 1.0,
+ 60.0)
+gripper_param.add("move_speed", double_t, 0,
+ "Speed of the follower gripper when opening [m/s]", 0.3,
+ 0.01, 0.4)
+
+exit(gen.generate(PACKAGE, "teleop_gripper_param", "teleop_gripper_param"))
diff --git a/fr3_impedance_controller_real/bak/teleop_joint_pd_example_control_node.yaml b/fr3_impedance_controller_real/bak/teleop_joint_pd_example_control_node.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5c5a66a4221669259527eb99b5daa0a4f34b17b4
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/teleop_joint_pd_example_control_node.yaml
@@ -0,0 +1,69 @@
+robot_hardware:
+ - leader
+ - follower
+
+leader:
+ type: franka_hw/FrankaCombinableHW
+ arm_id: leader
+ joint_names:
+ - leader_joint1
+ - leader_joint2
+ - leader_joint3
+ - leader_joint4
+ - leader_joint5
+ - leader_joint6
+ - leader_joint7
+ # Configure the threshold angle for printing joint limit warnings.
+ joint_limit_warning_threshold: 0.1 # [rad]
+ # Activate rate limiter? [true|false]
+ rate_limiting: true
+ # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+ cutoff_frequency: 1000
+ # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+ internal_controller: joint_impedance
+ # Used to decide whether to enforce realtime mode [enforce|ignore]
+ realtime_config: enforce
+ # Configure collision behavior reflexes.
+ # The upper torque and upper force thresholds are set to higher values compared to the default ones.
+ collision_config:
+ lower_torque_thresholds_acceleration: [25.0, 25.0, 23.0, 23.0, 20.0, 17.5, 15.0] # [Nm]
+ upper_torque_thresholds_acceleration: [25.0, 25.0, 23.0, 23.0, 20.0, 17.5, 15.0] # [Nm]
+ lower_torque_thresholds_nominal: [25.0, 25.0, 23.0, 23.0, 20.0, 17.5, 15.0] # [Nm]
+ upper_torque_thresholds_nominal: [25.0, 25.0, 23.0, 23.0, 20.0, 17.5, 15.0] # [Nm]
+ lower_force_thresholds_acceleration: [40.0, 40.0, 40.0, 40.0, 40.0, 40.0] # [N, N, N, Nm, Nm, Nm]
+ upper_force_thresholds_acceleration: [40.0, 40.0, 40.0, 40.0, 40.0, 40.0] # [N, N, N, Nm, Nm, Nm]
+ lower_force_thresholds_nominal: [40.0, 40.0, 40.0, 40.0, 40.0, 40.0] # [N, N, N, Nm, Nm, Nm]
+ upper_force_thresholds_nominal: [40.0, 40.0, 40.0, 40.0, 40.0, 40.0] # [N, N, N, Nm, Nm, Nm]
+
+follower:
+ type: franka_hw/FrankaCombinableHW
+ arm_id: follower
+ joint_names:
+ - follower_joint1
+ - follower_joint2
+ - follower_joint3
+ - follower_joint4
+ - follower_joint5
+ - follower_joint6
+ - follower_joint7
+ # Configure the threshold angle for printing joint limit warnings.
+ joint_limit_warning_threshold: 0.1 # [rad]
+ # Activate rate limiter? [true|false]
+ rate_limiting: true
+ # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+ cutoff_frequency: 1000
+ # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+ internal_controller: joint_impedance
+ # Used to decide whether to enforce realtime mode [enforce|ignore]
+ realtime_config: enforce
+ # Configure collision behavior reflexes.
+ # The upper torque and upper force thresholds are set to higher values compared to the default ones.
+ collision_config:
+ lower_torque_thresholds_acceleration: [25.0, 25.0, 23.0, 23.0, 20.0, 17.5, 15.0] # [Nm]
+ upper_torque_thresholds_acceleration: [25.0, 25.0, 23.0, 23.0, 20.0, 17.5, 15.0] # [Nm]
+ lower_torque_thresholds_nominal: [25.0, 25.0, 23.0, 23.0, 20.0, 17.5, 15.0] # [Nm]
+ upper_torque_thresholds_nominal: [25.0, 25.0, 23.0, 23.0, 20.0, 17.5, 15.0] # [Nm]
+ lower_force_thresholds_acceleration: [40.0, 40.0, 40.0, 40.0, 40.0, 40.0] # [N, N, N, Nm, Nm, Nm]
+ upper_force_thresholds_acceleration: [40.0, 40.0, 40.0, 40.0, 40.0, 40.0] # [N, N, N, Nm, Nm, Nm]
+ lower_force_thresholds_nominal: [40.0, 40.0, 40.0, 40.0, 40.0, 40.0] # [N, N, N, Nm, Nm, Nm]
+ upper_force_thresholds_nominal: [40.0, 40.0, 40.0, 40.0, 40.0, 40.0] # [N, N, N, Nm, Nm, Nm]
diff --git a/fr3_impedance_controller_real/bak/teleop_joint_pd_example_controller.cpp b/fr3_impedance_controller_real/bak/teleop_joint_pd_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..027202f9534456dffbccfa261da4e48ba38ea73a
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/teleop_joint_pd_example_controller.cpp
@@ -0,0 +1,537 @@
+// Copyright (c) 2020 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/joint_wall.h>
+#include <franka_example_controllers/teleop_joint_pd_example_controller.h>
+
+#include <hardware_interface/hardware_interface.h>
+#include <joint_limits_interface/joint_limits_urdf.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+#include <urdf/model.h>
+
+#include <Eigen/Dense>
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <string>
+#include <vector>
+
+using Vector7d = Eigen::Matrix<double, 7, 1>;
+
+const std::string kControllerName = "TeleopJointPDExampleController";
+
+namespace franka_example_controllers {
+
+bool TeleopJointPDExampleController::init(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle) {
+ std::string leader_arm_id;
+ std::string follower_arm_id;
+
+ std::vector<std::string> leader_joint_names;
+ std::vector<std::string> follower_joint_names;
+
+ try {
+ k_d_leader_lower_ = get7dParam("leader/d_gains_lower", node_handle);
+ k_d_leader_upper_ = get7dParam("leader/d_gains_upper", node_handle);
+ dq_max_leader_lower_ = get7dParam("leader/dq_max_lower", node_handle);
+ dq_max_leader_upper_ = get7dParam("leader/dq_max_upper", node_handle);
+
+ k_p_follower_ = get7dParam("follower/p_gains", node_handle);
+ k_d_follower_ = get7dParam("follower/d_gains", node_handle);
+ k_dq_ = get7dParam("follower/drift_comp_gains", node_handle);
+ dq_max_lower_ = get7dParam("follower/dq_max_lower", node_handle);
+ dq_max_upper_ = get7dParam("follower/dq_max_upper", node_handle);
+ ddq_max_lower_ = get7dParam("follower/ddq_max_lower", node_handle);
+ ddq_max_upper_ = get7dParam("follower/ddq_max_upper", node_handle);
+
+ leader_data_.contact_force_threshold =
+ get1dParam<double>("leader/contact_force_threshold", node_handle);
+ follower_data_.contact_force_threshold =
+ get1dParam<double>("follower/contact_force_threshold", node_handle);
+
+ leader_arm_id = get1dParam<std::string>("leader/arm_id", node_handle);
+ follower_arm_id = get1dParam<std::string>("follower/arm_id", node_handle);
+
+ leader_joint_names = getJointParams<std::string>("leader/joint_names", node_handle);
+ follower_joint_names = getJointParams<std::string>("follower/joint_names", node_handle);
+
+ if (!node_handle.getParam("debug", debug_)) {
+ ROS_INFO_STREAM_NAMED(kControllerName, "Could not find parameter debug. Defaulting to "
+ << std::boolalpha << debug_);
+ }
+
+ // Init for each arm
+ initArm(robot_hw, node_handle, leader_data_, leader_arm_id, leader_joint_names);
+ initArm(robot_hw, node_handle, follower_data_, follower_arm_id, follower_joint_names);
+ } catch (const std::invalid_argument& ex) {
+ ROS_ERROR_NAMED(kControllerName, "%s", ex.what());
+ return false;
+ }
+
+ if (debug_) {
+ // Init for dynamic reconfigure
+ dynamic_reconfigure_teleop_param_node_ = ros::NodeHandle("dyn_reconf_teleop_param_node");
+ dynamic_server_teleop_param_ = std::make_unique<
+ dynamic_reconfigure::Server<franka_example_controllers::teleop_paramConfig>>(
+ dynamic_reconfigure_teleop_param_node_);
+ dynamic_server_teleop_param_->setCallback(
+ boost::bind(&TeleopJointPDExampleController::teleopParamCallback, this, _1, _2));
+
+ // Init for publishers
+ auto init_publisher = [&node_handle](auto& publisher, const auto& topic) {
+ publisher.init(node_handle, topic, 1);
+ publisher.lock();
+ publisher.msg_.name.resize(7);
+ publisher.msg_.position.resize(7);
+ publisher.msg_.velocity.resize(7);
+ publisher.msg_.effort.resize(7);
+ publisher.unlock();
+ };
+
+ init_publisher(leader_target_pub_, "leader_target");
+ init_publisher(follower_target_pub_, "follower_target");
+ leader_contact_pub_.init(node_handle, "leader_contact", 1);
+ follower_contact_pub_.init(node_handle, "follower_contact", 1);
+ marker_pub_.init(node_handle, "marker_labels", 1, true);
+
+ auto get_marker = [](const std::string& arm_id, int32_t id, const std::string& text) {
+ visualization_msgs::Marker marker;
+ marker.header.frame_id = arm_id + "_link0";
+ marker.header.stamp = ros::Time::now();
+ marker.ns = "basic_shapes";
+ marker.id = id;
+ marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+ marker.action = visualization_msgs::Marker::ADD;
+
+ marker.pose.position.x = 0.0;
+ marker.pose.position.y = 0.0;
+ marker.pose.position.z = 1.0;
+ marker.pose.orientation.x = 0.0;
+ marker.pose.orientation.y = 0.0;
+ marker.pose.orientation.z = 0.0;
+ marker.pose.orientation.w = 1.0;
+
+ marker.text = text;
+
+ marker.scale.x = 0.3;
+ marker.scale.y = 0.3;
+ marker.scale.z = 0.1;
+
+ marker.color.r = 0.0f;
+ marker.color.g = 1.0f;
+ marker.color.b = 0.0f;
+ marker.color.a = 1.0;
+ return marker;
+ };
+
+ {
+ std::lock_guard<realtime_tools::RealtimePublisher<visualization_msgs::MarkerArray>> lock(
+ marker_pub_);
+ marker_pub_.msg_.markers.push_back(get_marker(leader_arm_id, 1, "leader"));
+ marker_pub_.msg_.markers.push_back(get_marker(follower_arm_id, 2, "follower"));
+ }
+ publishMarkers();
+ }
+
+ return true;
+}
+
+void TeleopJointPDExampleController::initArm(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle,
+ FrankaDataContainer& arm_data,
+ const std::string& arm_id,
+ const std::vector<std::string>& joint_names) {
+ auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
+ if (not effort_joint_interface) {
+ throw std::invalid_argument(kControllerName +
+ ": Error getting effort joint interface from hardware of " +
+ arm_id + ".");
+ }
+
+ arm_data.joint_handles.clear();
+ for (const auto& name : joint_names) {
+ try {
+ arm_data.joint_handles.push_back(effort_joint_interface->getHandle(name));
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ throw std::invalid_argument(kControllerName +
+ ": Exception getting joint handle: " + std::string(e.what()));
+ }
+ }
+
+ // Get state interface.
+ auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
+ if (not state_interface) {
+ throw std::invalid_argument(kControllerName + ": Error getting state interface from hardware.");
+ }
+
+ try {
+ arm_data.state_handle = std::make_unique<franka_hw::FrankaStateHandle>(
+ state_interface->getHandle(arm_id + "_robot"));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ throw std::invalid_argument(
+ kControllerName +
+ ": Exception getting state handle from interface: " + std::string(ex.what()));
+ }
+
+ // Setup joint walls
+ // Virtual joint position wall parameters
+ const std::array<double, 7> kPDZoneWidth = {{0.12, 0.09, 0.09, 0.09, 0.0349, 0.0349, 0.0349}};
+ const std::array<double, 7> kDZoneWidth = {{0.12, 0.09, 0.09, 0.09, 0.0349, 0.0349, 0.0349}};
+ const std::array<double, 7> kPDZoneStiffness = {
+ {2000.0, 2000.0, 1000.0, 1000.0, 500.0, 200.0, 200.0}};
+ const std::array<double, 7> kPDZoneDamping = {{30.0, 30.0, 30.0, 10.0, 5.0, 5.0, 5.0}};
+ const std::array<double, 7> kDZoneDamping = {{30.0, 30.0, 30.0, 10.0, 5.0, 5.0, 5.0}};
+
+ std::array<double, 7> upper_joint_soft_limit;
+ std::array<double, 7> lower_joint_soft_limit;
+ getJointLimits(node_handle, joint_names, upper_joint_soft_limit, lower_joint_soft_limit);
+
+ arm_data.virtual_joint_wall = std::make_unique<JointWallContainer<7>>(
+ upper_joint_soft_limit, lower_joint_soft_limit, kPDZoneWidth, kDZoneWidth, kPDZoneStiffness,
+ kPDZoneDamping, kDZoneDamping);
+}
+
+void TeleopJointPDExampleController::starting(const ros::Time& /*time*/) {
+ // Reset joint walls to start from the current q, dq
+ leader_data_.virtual_joint_wall->reset();
+ follower_data_.virtual_joint_wall->reset();
+
+ // Reset stored states to the current states.
+ franka::RobotState follower_robot_state = follower_data_.state_handle->getRobotState();
+ q_target_last_ = Eigen::Map<Vector7d>(follower_robot_state.q.data());
+ dq_target_last_.setZero();
+ leader_data_.tau_target_last.setZero();
+ follower_data_.tau_target_last.setZero();
+
+ // Store alignment position from leader
+ franka::RobotState leader_robot_state = leader_data_.state_handle->getRobotState();
+ init_leader_q_ = Eigen::Map<Vector7d>(leader_robot_state.q.data());
+ current_state_ = TeleopStateMachine::ALIGN;
+ if (debug_) {
+ publishMarkers();
+ }
+}
+
+void TeleopJointPDExampleController::update(const ros::Time& /*time*/,
+ const ros::Duration& period) {
+ franka::RobotState leader_robot_state = leader_data_.state_handle->getRobotState();
+ franka::RobotState follower_robot_state = follower_data_.state_handle->getRobotState();
+ leader_data_.q = Eigen::Map<Vector7d>(leader_robot_state.q.data());
+ leader_data_.dq = Eigen::Map<Vector7d>(leader_robot_state.dq.data());
+ follower_data_.q = Eigen::Map<Vector7d>(follower_robot_state.q.data());
+ follower_data_.dq = Eigen::Map<Vector7d>(follower_robot_state.dq.data());
+
+ if (current_state_ == TeleopStateMachine::ALIGN) {
+ // Check coefficient-wise if the two robots are aligned
+ const auto kNorm = (leader_data_.q - follower_data_.q).cwiseAbs().array();
+ if ((kNorm < kAlignmentTolerance_).all()) {
+ current_state_ = TeleopStateMachine::TRACK;
+ ROS_INFO_STREAM_NAMED(kControllerName, "Leader and follower are aligned");
+ }
+ }
+
+ // Determine contact scaling factor depending on the external cartesian forces applied on the
+ // endeffector.
+ Vector6d leader_f_ext_hat = Eigen::Map<Vector6d>(leader_robot_state.K_F_ext_hat_K.data());
+ leader_data_.f_ext_norm = leader_f_ext_hat.head(3).norm();
+ leader_data_.contact =
+ rampParameter(leader_data_.f_ext_norm, 1.0, 0.0, leader_data_.contact_force_threshold,
+ leader_data_.contact_ramp_increase);
+
+ Vector6d follower_f_ext_hat = Eigen::Map<Vector6d>(follower_robot_state.K_F_ext_hat_K.data());
+ follower_data_.f_ext_norm = follower_f_ext_hat.head(3).norm();
+ follower_data_.contact =
+ rampParameter(follower_data_.f_ext_norm, 1.0, 0.0, follower_data_.contact_force_threshold,
+ follower_data_.contact_ramp_increase);
+
+ // Determine max velocities and accelerations of follower arm depending on tracking errors to
+ // avoid jumps and high velocities when starting example.
+ Vector7d q_deviation = (q_target_last_ - leader_data_.q).cwiseAbs();
+ Vector7d dq_max;
+ Vector7d ddq_max;
+
+ if (current_state_ == TeleopStateMachine::ALIGN) {
+ dq_max = dq_max_align_;
+ ddq_max = ddq_max_align_;
+ prev_alignment_error_ = alignment_error_;
+ alignment_error_ = (init_leader_q_ - follower_data_.q);
+ Vector7d dalignment_error = (alignment_error_ - prev_alignment_error_) / period.toSec();
+
+ dq_unsaturated_ = k_p_follower_align_.asDiagonal() * alignment_error_ +
+ k_d_follower_align_.asDiagonal() * dalignment_error;
+ } else {
+ for (size_t i = 0; i < 7; ++i) {
+ dq_max[i] = rampParameter(q_deviation[i], dq_max_lower_[i], dq_max_upper_[i],
+ velocity_ramp_shift_, velocity_ramp_increase_);
+ ddq_max[i] = rampParameter(q_deviation[i], ddq_max_lower_[i], ddq_max_upper_[i],
+ velocity_ramp_shift_, velocity_ramp_increase_);
+ }
+ dq_unsaturated_ = k_dq_.asDiagonal() * (leader_data_.q - q_target_last_) + leader_data_.dq;
+ }
+
+ // Calculate target postions and velocities for follower arm
+ dq_target_ = saturateAndLimit(dq_unsaturated_, dq_target_last_, dq_max, ddq_max, period.toSec());
+ dq_target_last_ = dq_target_;
+ q_target_ = q_target_last_ + (dq_target_ * period.toSec());
+ q_target_last_ = q_target_;
+
+ if (!leader_robot_state.current_errors && !follower_robot_state.current_errors) {
+ if (current_state_ == TeleopStateMachine::ALIGN) {
+ // Compute P control for the leader arm to stay in position.
+ leader_data_.tau_target = k_p_follower_.asDiagonal() * (init_leader_q_ - leader_data_.q);
+ } else {
+ // Compute force-feedback for the leader arm to render the haptic interaction of the follower
+ // robot. Add a slight damping to reduce vibrations.
+ // The force feedback is applied when the external forces on the follower arm exceed a
+ // threshold. While the leader arm is unguided (not in contact), the force-feedback is
+ // reduced. When the leader robot exceeds the soft limit velocities dq_max_leader_lower
+ // damping is increased gradually until it saturates when reaching dq_max_leader_upper to
+ // maximum damping.
+
+ Vector7d follower_tau_ext_hat =
+ Eigen::Map<Vector7d>(follower_robot_state.tau_ext_hat_filtered.data());
+ Vector7d leader_damping_torque =
+ leader_damping_scaling_ * leaderDamping(leader_data_.dq).asDiagonal() * leader_data_.dq;
+ Vector7d leader_force_feedback =
+ follower_data_.contact *
+ (force_feedback_idle_ +
+ leader_data_.contact * (force_feedback_guiding_ - force_feedback_idle_)) *
+ (-follower_tau_ext_hat);
+
+ leader_data_.tau_target = leader_force_feedback - leader_damping_torque;
+ }
+
+ // Compute PD control for the follower arm to track the leader's motions.
+ follower_data_.tau_target =
+ follower_stiffness_scaling_ * k_p_follower_.asDiagonal() * (q_target_ - follower_data_.q) +
+ sqrt(follower_stiffness_scaling_) * k_d_follower_.asDiagonal() *
+ (dq_target_ - follower_data_.dq);
+ } else {
+ // Control target torques to zero if any arm is in error state.
+ leader_data_.tau_target = decrease_factor_ * leader_data_.tau_target_last;
+ follower_data_.tau_target = decrease_factor_ * follower_data_.tau_target_last;
+ }
+
+ // Add torques from joint walls to the torque commands.
+ auto to_eigen = [](const std::array<double, 7>& data) { return Vector7d(data.data()); };
+ auto from_eigen = [](const Vector7d& data) {
+ return std::array<double, 7>{data(0), data(1), data(2), data(3), data(4), data(5), data(6)};
+ };
+ std::array<double, 7> virtual_wall_tau_leader = leader_data_.virtual_joint_wall->computeTorque(
+ from_eigen(leader_data_.q), from_eigen(leader_data_.dq));
+
+ std::array<double, 7> virtual_wall_tau_follower =
+ follower_data_.virtual_joint_wall->computeTorque(from_eigen(follower_data_.q),
+ from_eigen(follower_data_.dq));
+
+ leader_data_.tau_target += to_eigen(virtual_wall_tau_leader);
+ follower_data_.tau_target += to_eigen(virtual_wall_tau_follower);
+
+ // Store torques for next time step
+ leader_data_.tau_target_last = leader_data_.tau_target;
+ follower_data_.tau_target_last = follower_data_.tau_target;
+
+ updateArm(leader_data_);
+ updateArm(follower_data_);
+
+ if (debug_ && publish_rate_()) {
+ publishLeaderTarget();
+ publishFollowerTarget();
+ publishLeaderContact();
+ publishFollowerContact();
+ }
+}
+
+void TeleopJointPDExampleController::updateArm(FrankaDataContainer& arm_data) {
+ for (size_t i = 0; i < 7; ++i) {
+ arm_data.joint_handles[i].setCommand(arm_data.tau_target[i]);
+ }
+}
+
+Eigen::Matrix<double, 7, 1> TeleopJointPDExampleController::saturateAndLimit(const Vector7d& x_calc,
+ const Vector7d& x_last,
+ const Vector7d& x_max,
+ const Vector7d& dx_max,
+ const double delta_t) {
+ Vector7d x_limited;
+ for (size_t i = 0; i < 7; i++) {
+ double delta_x_max = dx_max[i] * delta_t;
+ double diff = x_calc[i] - x_last[i];
+ double x_saturated = x_last[i] + std::max(std::min(diff, delta_x_max), -delta_x_max);
+ x_limited[i] = std::max(std::min(x_saturated, x_max[i]), -x_max[i]);
+ }
+ return x_limited;
+}
+
+double TeleopJointPDExampleController::rampParameter(const double x,
+ const double neg_x_asymptote,
+ const double pos_x_asymptote,
+ const double shift_along_x,
+ const double increase_factor) {
+ double ramp =
+ 0.5 * (pos_x_asymptote + neg_x_asymptote -
+ (pos_x_asymptote - neg_x_asymptote) * tanh(increase_factor * (x - shift_along_x)));
+ return ramp;
+}
+
+void TeleopJointPDExampleController::teleopParamCallback(
+ franka_example_controllers::teleop_paramConfig& config,
+ uint32_t /*level*/) {
+ if (dynamic_reconfigure_mutex_.try_lock()) {
+ leader_damping_scaling_ = config.leader_damping_scaling;
+ follower_stiffness_scaling_ = config.follower_stiffness_scaling;
+ force_feedback_guiding_ = config.force_feedback_guiding;
+ force_feedback_idle_ = config.force_feedback_idle;
+ follower_data_.contact_force_threshold = config.follower_contact_force_threshold;
+ leader_data_.contact_force_threshold = config.leader_contact_force_threshold;
+
+ dq_max_lower_[0] = config.dq_l_1;
+ dq_max_lower_[1] = config.dq_l_2;
+ dq_max_lower_[2] = config.dq_l_3;
+ dq_max_lower_[3] = config.dq_l_4;
+ dq_max_lower_[4] = config.dq_l_5;
+ dq_max_lower_[5] = config.dq_l_6;
+ dq_max_lower_[6] = config.dq_l_7;
+
+ dq_max_upper_[0] = config.dq_u_1;
+ dq_max_upper_[1] = config.dq_u_2;
+ dq_max_upper_[2] = config.dq_u_3;
+ dq_max_upper_[3] = config.dq_u_4;
+ dq_max_upper_[4] = config.dq_u_5;
+ dq_max_upper_[5] = config.dq_u_6;
+ dq_max_upper_[6] = config.dq_u_7;
+
+ ddq_max_lower_[0] = config.ddq_l_1;
+ ddq_max_lower_[1] = config.ddq_l_2;
+ ddq_max_lower_[2] = config.ddq_l_3;
+ ddq_max_lower_[3] = config.ddq_l_4;
+ ddq_max_lower_[4] = config.ddq_l_5;
+ ddq_max_lower_[5] = config.ddq_l_6;
+ ddq_max_lower_[6] = config.ddq_l_7;
+
+ ddq_max_upper_[0] = config.ddq_u_1;
+ ddq_max_upper_[1] = config.ddq_u_2;
+ ddq_max_upper_[2] = config.ddq_u_3;
+ ddq_max_upper_[3] = config.ddq_u_4;
+ ddq_max_upper_[4] = config.ddq_u_5;
+ ddq_max_upper_[5] = config.ddq_u_6;
+ ddq_max_upper_[6] = config.ddq_u_7;
+
+ ROS_INFO_NAMED(kControllerName, "Dynamic reconfigure: Controller params set.");
+ }
+ dynamic_reconfigure_mutex_.unlock();
+}
+
+void TeleopJointPDExampleController::getJointLimits(ros::NodeHandle& nh,
+ const std::vector<std::string>& joint_names,
+ std::array<double, 7>& upper_joint_soft_limit,
+ std::array<double, 7>& lower_joint_soft_limit) {
+ const std::string& node_namespace = nh.getNamespace();
+ std::size_t found = node_namespace.find_last_of('/');
+ std::string parent_namespace = node_namespace.substr(0, found);
+
+ if (!nh.hasParam(parent_namespace + "/robot_description")) {
+ throw std::invalid_argument(kControllerName + ": No parameter robot_description (namespace: " +
+ parent_namespace + ")found to set joint limits!");
+ }
+
+ urdf::Model urdf_model;
+ if (!urdf_model.initParamWithNodeHandle(parent_namespace + "/robot_description", nh)) {
+ throw std::invalid_argument(kControllerName +
+ ": Could not initialize urdf model from robot_description "
+ "(namespace: " +
+ parent_namespace + ").");
+ }
+ joint_limits_interface::SoftJointLimits soft_limits;
+ for (size_t i = 0; i < joint_names.size(); ++i) {
+ const std::string& joint_name = joint_names.at(i);
+ auto urdf_joint = urdf_model.getJoint(joint_name);
+ if (!urdf_joint) {
+ ROS_ERROR_STREAM_NAMED(kControllerName,
+ ": Could not get joint " << joint_name << " from urdf");
+ }
+ if (!urdf_joint->safety) {
+ ROS_ERROR_STREAM_NAMED(kControllerName, ": Joint " << joint_name << " has no limits");
+ }
+ if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) {
+ upper_joint_soft_limit[i] = soft_limits.max_position;
+ lower_joint_soft_limit[i] = soft_limits.min_position;
+ } else {
+ ROS_ERROR_STREAM_NAMED(kControllerName, ": Could not parse joint limit for joint "
+ << joint_name << " for joint limit interfaces");
+ }
+ }
+}
+
+void TeleopJointPDExampleController::publishLeaderTarget() {
+ if (leader_target_pub_.trylock()) {
+ for (size_t i = 0; i < 7; ++i) {
+ leader_target_pub_.msg_.name[i] = "panda_joint" + std::to_string(i + 1);
+ leader_target_pub_.msg_.position[i] = 0.0;
+ leader_target_pub_.msg_.velocity[i] = 0.0;
+ leader_target_pub_.msg_.effort[i] = leader_data_.tau_target[i];
+ }
+ leader_target_pub_.unlockAndPublish();
+ }
+}
+
+void TeleopJointPDExampleController::publishFollowerTarget() {
+ if (follower_target_pub_.trylock()) {
+ for (size_t i = 0; i < 7; ++i) {
+ follower_target_pub_.msg_.name[i] = "panda_joint" + std::to_string(i + 1);
+ follower_target_pub_.msg_.position[i] = q_target_[i];
+ follower_target_pub_.msg_.velocity[i] = dq_target_[i];
+ follower_target_pub_.msg_.effort[i] = follower_data_.tau_target[i];
+ }
+ follower_target_pub_.unlockAndPublish();
+ }
+}
+
+void TeleopJointPDExampleController::publishLeaderContact() {
+ if (leader_contact_pub_.trylock()) {
+ leader_contact_pub_.msg_.data = leader_data_.contact;
+ leader_contact_pub_.unlockAndPublish();
+ }
+}
+
+void TeleopJointPDExampleController::publishFollowerContact() {
+ if (follower_contact_pub_.trylock()) {
+ follower_contact_pub_.msg_.data = follower_data_.contact;
+ follower_contact_pub_.unlockAndPublish();
+ }
+}
+
+Vector7d TeleopJointPDExampleController::get7dParam(const std::string& param_name,
+ ros::NodeHandle& nh) {
+ auto buffer = getJointParams<double>(param_name, nh);
+ return Vector7d(Eigen::Map<Vector7d>(buffer.data()));
+}
+
+Vector7d TeleopJointPDExampleController::leaderDamping(const Vector7d& dq) {
+ auto simple_ramp = [](const double min, const double max, const double value) -> double {
+ if (value >= max) {
+ return 1.0;
+ }
+ if (value <= min) {
+ return 0.0;
+ }
+ return (value - min) / (max - min);
+ };
+ Vector7d damping;
+ for (size_t i = 0; i < 7; ++i) {
+ damping(i) = k_d_leader_lower_(i) +
+ simple_ramp(dq_max_leader_lower_(i), dq_max_leader_upper_(i), std::abs(dq(i))) *
+ (k_d_leader_upper_(i) - k_d_leader_lower_(i));
+ }
+ return damping;
+}
+
+void TeleopJointPDExampleController::publishMarkers() {
+ marker_pub_.lock();
+ marker_pub_.unlockAndPublish();
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::TeleopJointPDExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/bak/teleop_joint_pd_example_controller.h b/fr3_impedance_controller_real/bak/teleop_joint_pd_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..4ddb658b2f75e44a096974f07b1945597465a851
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/teleop_joint_pd_example_controller.h
@@ -0,0 +1,222 @@
+// Copyright (c) 2020 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <franka_example_controllers/joint_wall.h>
+#include <franka_example_controllers/teleop_paramConfig.h>
+#include <franka_hw/franka_state_interface.h>
+#include <franka_hw/trigger_rate.h>
+
+#include <control_msgs/GripperCommandAction.h>
+#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+#include <std_msgs/Float64.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <Eigen/Dense>
+#include <memory>
+#include <mutex>
+#include <vector>
+
+namespace franka_example_controllers {
+/**
+ * Finite state machine that defines the states of the teleoperation phases.
+ * ALIGN is the initial phase, when the leader and follower align. During this phase the leader
+ * cannot be moved.
+ * TRACK is the tracking phase.
+ */
+enum TeleopStateMachine {
+ ALIGN,
+ TRACK,
+};
+
+/**
+ * Controller class for ros_control that allows force-feedback teleoperation of a follower arm from
+ * a leader arm. Smooth tracking is implemented by integrating a velocity signal, which is
+ * calculated by limiting and saturating the velocity of the leader arm and a drift compensation.
+ * The torque control of the follower arm is implemented by a simple PD-controller.
+ * The leader arm is slightly damped to reduce vibrations.
+ * Force-feedback is applied to the leader arm when the external forces on the follower arm exceed a
+ * configured threshold.
+ * While the leader arm is unguided (not in contact), the applied force-feedback will be reduced.
+ */
+class TeleopJointPDExampleController : public controller_interface::MultiInterfaceController<
+ hardware_interface::EffortJointInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ /**
+ * Initializes the controller class to be ready to run.
+ *
+ * @param[in] robot_hw Pointer to a RobotHW class to get interfaces and resource handles.
+ * @param[in] node_handle Nodehandle that allows getting parameterizations from the server and
+ * starting publisher
+ */
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
+
+ /**
+ * Prepares the controller for the real-time execution. This method is executed once every time
+ * the controller is started and runs in real-time
+ */
+ void starting(const ros::Time& /*time*/) override;
+
+ /**
+ * Computes the control-law and commands the resulting joint torques to the robots.
+ *
+ * @param[in] period The control period (here 0.001s)
+ */
+ void update(const ros::Time& /*time*/, const ros::Duration& period) override;
+
+ private:
+ using Vector6d = Eigen::Matrix<double, 6, 1>;
+ using Vector7d = Eigen::Matrix<double, 7, 1>;
+ using Matrix7d = Eigen::Matrix<double, 7, 7>;
+
+ struct FrankaDataContainer {
+ std::unique_ptr<franka_hw::FrankaStateHandle> state_handle;
+ std::vector<hardware_interface::JointHandle> joint_handles;
+
+ // A virtual wall to avoid joint limits.
+ std::unique_ptr<JointWallContainer<7>> virtual_joint_wall;
+
+ Vector7d tau_target; // Target effort of each joint [Nm, Nm, Nm, Nm, Nm, Nm, Nm]
+ Vector7d tau_target_last; // Last target effort of each joint [Nm, ...]
+ Vector7d q; // Measured position of each joint [rad, ...]
+ Vector7d dq; // Measured velocity of each joint [rad/s, ...]
+
+ double f_ext_norm; // Norm of the external (cartesian) forces vector at the EE [N]
+ double contact; // Contact scaling factor (values between 0 and 1)
+ double contact_ramp_increase{0.3}; // Parameter for contact scaling factor
+ double contact_force_threshold; // Parameter for contact scaling factor [N]
+ };
+
+ FrankaDataContainer leader_data_; // Container for data of the leader arm
+ FrankaDataContainer follower_data_; // Container for data of the follower arm
+
+ Vector7d q_target_; // Target positions of the follower arm [rad, rad, rad, rad, rad, rad, rad]
+ Vector7d q_target_last_; // Last target positions of the follower arm [rad, ...]
+ Vector7d dq_unsaturated_; // Unsaturated target velocities of the follower arm [rad/s, ...]
+ Vector7d dq_target_; // Target velocities of the follower arm [rad/s, ...]
+ Vector7d dq_target_last_; // Last target velocities of the follower arm [rad/s, ...]
+
+ Vector7d init_leader_q_; // Measured q of leader arm during alignment [rad]
+
+ Vector7d dq_max_lower_; // Lower max velocities of the follower arm [rad/s, ...]
+ Vector7d dq_max_upper_; // Upper max velocities of the follower arm [rad/s, ...]
+ Vector7d ddq_max_lower_; // Lower max accelerations of the follower arm [rad/s², ...]
+ Vector7d ddq_max_upper_; // Upper max accelerations of the follower arm [rad/s², ...]
+ double velocity_ramp_shift_{0.25}; // parameter for ramping dq_max and ddq_max [rad]
+ double velocity_ramp_increase_{20}; // parameter for ramping dq_max and ddq_max
+
+ // Max velocities of the follower arm during alignment [rad/s, ...]
+ Vector7d dq_max_align_{(Vector7d() << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()};
+ // Max accelerations of the follower arm during alignment [rad/s², ...]
+ Vector7d ddq_max_align_{(Vector7d() << 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5).finished()};
+
+ Vector7d alignment_error_; // Diff between follower and leader q during alignment [rad/s, ...]
+ Vector7d prev_alignment_error_; // alignment_error_ in previous control loop [rad/s, ...]
+
+ Vector7d k_p_follower_; // p-gain for follower arm
+ Vector7d k_d_follower_; // d-gain for follower arm
+ // p-gain for follower arm during alignment
+ Vector7d k_p_follower_align_{(Vector7d() << 45.0, 45.0, 45.0, 45.0, 18.0, 11.0, 5.0).finished()};
+ // d-gain for follower arm during alignment
+ Vector7d k_d_follower_align_{(Vector7d() << 4.5, 4.5, 4.5, 4.5, 1.5, 1.5, 1.0).finished()};
+
+ Vector7d dq_max_leader_lower_; // Soft max velocities of the leader arm [rad/s, ...]
+ Vector7d dq_max_leader_upper_; // Hard max velocities of the leader arm [rad/s, ...]
+ Vector7d k_d_leader_lower_; // d-gain for leader arm when under soft-limit
+ Vector7d k_d_leader_upper_; // d-gain for leader arm when hard limit is reached
+
+ Vector7d k_dq_; // gain for drift compensation in follower arm
+
+ double force_feedback_idle_{0.5}; // Applied force-feedback, when leader arm is not guided
+ double force_feedback_guiding_{0.95}; // Applied force-feeback, when leader arm is guided
+
+ double decrease_factor_{0.95}; // Param, used when (in error state) controlling torques to zero
+
+ TeleopStateMachine current_state_{TeleopStateMachine::ALIGN}; // Current state in teleoperation
+
+ const double kAlignmentTolerance_{1e-2}; // Tolerance to consider a joint aligned [rad]
+
+ void initArm(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle,
+ FrankaDataContainer& arm_data,
+ const std::string& arm_id,
+ const std::vector<std::string>& joint_names);
+
+ void updateArm(FrankaDataContainer& arm_data);
+
+ Vector7d saturateAndLimit(const Vector7d& x_calc,
+ const Vector7d& x_last,
+ const Vector7d& x_max,
+ const Vector7d& dx_max,
+ double delta_t);
+
+ double rampParameter(double x,
+ double neg_x_asymptote,
+ double pos_x_asymptote,
+ double shift_along_x,
+ double increase_factor);
+
+ template <typename T>
+ std::vector<T> getJointParams(const std::string& param_name, ros::NodeHandle& nh) {
+ std::vector<T> vec;
+ if (!nh.getParam(param_name, vec) || vec.size() != 7) {
+ throw std::invalid_argument("TeleopJointPDExampleController: Invalid or no parameter " +
+ nh.getNamespace() + "/" + param_name +
+ " provided, aborting controller init!");
+ }
+ return vec;
+ }
+
+ Vector7d get7dParam(const std::string& param_name, ros::NodeHandle& nh);
+
+ template <typename T>
+ T get1dParam(const std::string& param_name, ros::NodeHandle& nh) {
+ T out;
+ if (!nh.getParam(param_name, out)) {
+ throw std::invalid_argument("TeleopJointPDExampleController: Invalid or no parameter " +
+ nh.getNamespace() + "/" + param_name +
+ " provided, "
+ "aborting controller init!");
+ }
+ return out;
+ }
+
+ static void getJointLimits(ros::NodeHandle& nh,
+ const std::vector<std::string>& joint_names,
+ std::array<double, 7>& upper_joint_soft_limit,
+ std::array<double, 7>& lower_joint_soft_limit);
+
+ Vector7d leaderDamping(const Vector7d& dq);
+
+ // Debug tool
+ bool debug_{false};
+ std::mutex dynamic_reconfigure_mutex_;
+ double leader_damping_scaling_{1.0};
+ double follower_stiffness_scaling_{1.0};
+
+ ros::NodeHandle dynamic_reconfigure_teleop_param_node_;
+ std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::teleop_paramConfig>>
+ dynamic_server_teleop_param_;
+ void teleopParamCallback(franka_example_controllers::teleop_paramConfig& config, uint32_t level);
+
+ franka_hw::TriggerRate publish_rate_{60.0};
+ realtime_tools::RealtimePublisher<sensor_msgs::JointState> leader_target_pub_;
+ realtime_tools::RealtimePublisher<sensor_msgs::JointState> follower_target_pub_;
+ realtime_tools::RealtimePublisher<std_msgs::Float64> leader_contact_pub_;
+ realtime_tools::RealtimePublisher<std_msgs::Float64> follower_contact_pub_;
+ realtime_tools::RealtimePublisher<visualization_msgs::MarkerArray> marker_pub_;
+
+ void publishLeaderTarget();
+ void publishFollowerTarget();
+ void publishLeaderContact();
+ void publishFollowerContact();
+ void publishMarkers();
+};
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/bak/teleop_joint_pd_example_controller.launch b/fr3_impedance_controller_real/bak/teleop_joint_pd_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..6406d28d1b4d29daf1ef5b1f8e2af578a519abcc
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/teleop_joint_pd_example_controller.launch
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<launch>
+ <arg name="leader_ip" doc="left robot from user persepective standing in front of the robots for a robot setup according to $(find franka_description)/robots/dual_panda/dual_panda_example.urdf.xacro" />
+ <arg name="follower_ip" doc="right robot from user persepective standing in front of the robots for a robot setup according to $(find franka_description)/robots/dual_panda/dual_panda_example.urdf.xacro" />
+
+ <arg name="robot_id" default="panda_teleop" doc="name of the 2-arm robot" />
+ <arg name="debug" default="false" doc="publish debug information and enable dynamic parameter tuning" />
+ <arg name="rviz" default="true" doc="open rviz on launch to show the virtual robot setup" />
+ <arg name="load_gripper" default="false" doc="start teleoperation for the grippers (full open/close is mirrored to follower)" />
+ <arg name="gripper_homed" default="false" doc="is the gripper already homed? If not the gripper will perform an initial homing motion" />
+
+ <remap from="dyn_reconf_teleop_param_node" to="$(arg robot_id)/dyn_reconf_teleop_param_node" />
+ <remap from="$(arg robot_id)/dyn_reconf_teleop_param_node/leader_contact_force_threshold" to="$(arg robot_id)/teleop_joint_pd_example_controller/leader/contact_force_threshold" />
+ <remap from="$(arg robot_id)/dyn_reconf_teleop_param_node/follower_contact_force_threshold" to="$(arg robot_id)/teleop_joint_pd_example_controller/follower/contact_force_threshold" />
+
+ <include file="$(find franka_control)/launch/franka_combined_control.launch">
+ <arg name="robot_id" value="$(arg robot_id)" />
+ <arg name="robot_ips" value="{leader/robot_ip: $(arg leader_ip), follower/robot_ip: $(arg follower_ip)}" />
+ <arg name="hw_config_file" value="$(find franka_example_controllers)/config/teleop_joint_pd_example_control_node.yaml" />
+ <arg name="robot" value="$(find franka_description)/robots/dual_panda/dual_panda_example.urdf.xacro" />
+ <arg name="args" value="arm_id_1:=leader arm_id_2:=follower" />
+ <arg name="controllers_file" value="$(find franka_example_controllers)/config/teleop_state_controllers.yaml" />
+ <arg name="controllers_to_start" value="leader_state_controller follower_state_controller" />
+ <arg name="joint_states_source_list" value="[leader_state_controller/joint_states, follower_state_controller/joint_states, leader/franka_gripper/joint_states, follower/franka_gripper/joint_states]" />
+ </include>
+
+ <group ns="$(arg robot_id)">
+ <include file="$(find franka_example_controllers)/launch/teleop_gripper.launch" if="$(arg load_gripper)">
+ <arg name="leader_ip" value="$(arg leader_ip)" />
+ <arg name="follower_ip" value="$(arg follower_ip)" />
+ <arg name="leader_id" value="leader" />
+ <arg name="follower_id" value="follower" />
+ <arg name="gripper_homed" value="$(arg gripper_homed)" />
+ </include>
+
+ <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
+ <rosparam param="teleop_joint_pd_example_controller/debug" subst_value="true">$(arg debug)</rosparam>
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="teleop_joint_pd_example_controller" />
+
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" if="$(eval arg('debug') or arg('load_gripper'))" />
+
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/teleop_joint_pd_example.rviz" if="$(arg rviz)" />
+ </group>
+</launch>
\ No newline at end of file
diff --git a/fr3_impedance_controller_real/bak/teleop_param.cfg b/fr3_impedance_controller_real/bak/teleop_param.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..26166baeb929c7cdaea87fcadba2e240146436ef
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/teleop_param.cfg
@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+controller_gains = gen.add_group("Controller_Gains", type="apply")
+controller_gains.add("leader_damping_scaling", double_t, 0,
+ "Factor to be multiplied with initial leader d_gains",
+ 1.0, 0.0, 8.0)
+controller_gains.add(
+ "follower_stiffness_scaling", double_t, 0,
+ "Factor to be multiplied with initial p-gains of follower arm, damping will also be adapted",
+ 1.0, 0.1, 1.3)
+
+force_feedback = gen.add_group("Force_Feedback")
+force_feedback.add("force_feedback_idle", double_t, 0,
+ "Applied feedback force when leader arm is idle", 0.5, 0.0,
+ 1.0)
+force_feedback.add("force_feedback_guiding", double_t, 0,
+ "Applied feedback force when leader arm is guided", 0.98,
+ 0.0, 1.0)
+
+contact_param = gen.add_group("Cartesian_Contact")
+contact_param.add(
+ "leader_contact_force_threshold", double_t, 0,
+ "Force threshold at leader endeffector to determine whether the arm is in contact.",
+ 4.0, 0.1, 10.0)
+contact_param.add(
+ "follower_contact_force_threshold", double_t, 0,
+ "Force threshold at follower endeffector to determine whether the arm is in contact.",
+ 5.0, 0.1, 10.0)
+
+dq_max = gen.add_group("Max_Velocities")
+dq_max_lower_tab = dq_max.add_group("Dq_MaxLower", type="tab")
+dq_max_lower = dq_max_lower_tab.add_group("dq_max_lower", type="apply")
+
+dq_max_lower.add("dq_l_1", double_t, 0, "Description", 0.8, 0.1, 2.17)
+dq_max_lower.add("dq_l_2", double_t, 0, "Description", 0.8, 0.1, 2.17)
+dq_max_lower.add("dq_l_3", double_t, 0, "Description", 0.8, 0.1, 2.17)
+dq_max_lower.add("dq_l_4", double_t, 0, "Description", 0.8, 0.1, 2.17)
+dq_max_lower.add("dq_l_5", double_t, 0, "Description", 2.5, 0.1, 2.6)
+dq_max_lower.add("dq_l_6", double_t, 0, "Description", 2.5, 0.1, 2.6)
+dq_max_lower.add("dq_l_7", double_t, 0, "Description", 2.5, 0.1, 2.6)
+
+dq_max_upper_tab = dq_max.add_group("Dq_MaxUpper", type="tab")
+dq_max_upper = dq_max_upper_tab.add_group("dq_max_upper", type="apply")
+dq_max_upper.add("dq_u_1", double_t, 0, "Description", 2.0, 0.1, 2.17)
+dq_max_upper.add("dq_u_2", double_t, 0, "Description", 2.0, 0.1, 2.17)
+dq_max_upper.add("dq_u_3", double_t, 0, "Description", 2.0, 0.1, 2.17)
+dq_max_upper.add("dq_u_4", double_t, 0, "Description", 2.0, 0.1, 2.17)
+dq_max_upper.add("dq_u_5", double_t, 0, "Description", 2.5, 0.1, 2.6)
+dq_max_upper.add("dq_u_6", double_t, 0, "Description", 2.5, 0.1, 2.6)
+dq_max_upper.add("dq_u_7", double_t, 0, "Description", 2.5, 0.1, 2.6)
+
+ddq_max = gen.add_group("Max_Acceleration")
+ddq_max_lower_tab = ddq_max.add_group("Ddq_MaxLower", type="tab")
+ddq_max_lower = ddq_max_lower_tab.add_group("ddq_max_lower", type="apply")
+ddq_max_lower.add("ddq_l_1", double_t, 0, "Description", 0.8, 0.1, 15.0)
+ddq_max_lower.add("ddq_l_2", double_t, 0, "Description", 0.8, 0.1, 7.5)
+ddq_max_lower.add("ddq_l_3", double_t, 0, "Description", 0.8, 0.1, 10.0)
+ddq_max_lower.add("ddq_l_4", double_t, 0, "Description", 0.8, 0.1, 12.5)
+ddq_max_lower.add("ddq_l_5", double_t, 0, "Description", 10.0, 0.1, 15.0)
+ddq_max_lower.add("ddq_l_6", double_t, 0, "Description", 10.0, 0.1, 20.0)
+ddq_max_lower.add("ddq_l_7", double_t, 0, "Description", 10.0, 0.1, 20.0)
+
+ddq_max_upper_tab = ddq_max.add_group("Ddq_MaxUpper", type="tab")
+ddq_max_upper = ddq_max_upper_tab.add_group("ddq_max_upper", type="apply")
+ddq_max_upper.add("ddq_u_1", double_t, 0, "Description", 6.0, 0.1, 15.0)
+ddq_max_upper.add("ddq_u_2", double_t, 0, "Description", 6.0, 0.1, 7.5)
+ddq_max_upper.add("ddq_u_3", double_t, 0, "Description", 6.0, 0.1, 10.0)
+ddq_max_upper.add("ddq_u_4", double_t, 0, "Description", 6.0, 0.1, 12.5)
+ddq_max_upper.add("ddq_u_5", double_t, 0, "Description", 15.0, 0.1, 15.0)
+ddq_max_upper.add("ddq_u_6", double_t, 0, "Description", 20.0, 0.1, 20.0)
+ddq_max_upper.add("ddq_u_7", double_t, 0, "Description", 20.0, 0.1, 20.0)
+
+exit(gen.generate(PACKAGE, "teleop_param", "teleop_param"))
diff --git a/fr3_impedance_controller_real/bak/teleop_state_controllers.yaml b/fr3_impedance_controller_real/bak/teleop_state_controllers.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..79944ed842e5033e85e786acdd5058b38fc06f96
--- /dev/null
+++ b/fr3_impedance_controller_real/bak/teleop_state_controllers.yaml
@@ -0,0 +1,25 @@
+leader_state_controller:
+ type: franka_control/FrankaStateController
+ arm_id: leader
+ joint_names:
+ - leader_joint1
+ - leader_joint2
+ - leader_joint3
+ - leader_joint4
+ - leader_joint5
+ - leader_joint6
+ - leader_joint7
+ publish_rate: 30 # [Hz]
+
+follower_state_controller:
+ type: franka_control/FrankaStateController
+ arm_id: follower
+ joint_names:
+ - follower_joint1
+ - follower_joint2
+ - follower_joint3
+ - follower_joint4
+ - follower_joint5
+ - follower_joint6
+ - follower_joint7
+ publish_rate: 30 # [Hz]
diff --git a/fr3_impedance_controller_real/cfg/controller_param.cfg b/fr3_impedance_controller_real/cfg/controller_param.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..cb9abf01e186898a869402021e8aa64a2f370e7a
--- /dev/null
+++ b/fr3_impedance_controller_real/cfg/controller_param.cfg
@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("K_task_x", double_t, 0, "Cartesian translational-x stiffness", 200, 0, 400)
+gen.add("K_task_y", double_t, 0, "Cartesian translational-y stiffness", 200, 0, 400)
+gen.add("K_task_z", double_t, 0, "Cartesian translational-z stiffness", 200, 0, 400)
+gen.add("K_task_rot", double_t, 0, "Cartesian rotational stiffness", 10, 0, 30)
+gen.add("K_null", double_t, 0, "Null-space joint stiffness", 0.5, 0, 100)
+
+gen.add("fq", double_t, 0, "DOB cutoff frequency", 15, 0, 75)
+gen.add("s1", double_t, 0, "SOSML s1 gain", 10, 0, 2000)
+gen.add("s2", double_t, 0, "SOSML s2 gain", 200, 0, 2000)
+gen.add("r_th", double_t, 0, "MOB residual threshold", 0, 0, 100)
+gen.add("Km", double_t, 0, "MOB low-pass filter gain", 2.5, 0, 75)
+
+exit(gen.generate(PACKAGE, "dynamic_compliance", "controller_param"))
+
diff --git a/fr3_impedance_controller_real/config/franka_example_controllers.yaml b/fr3_impedance_controller_real/config/franka_example_controllers.yaml
index 4cb8fa5665b64f40f8a8628dcae2b52fe9b3b169..e4127e9575ced925bd48f6b83ee4df24bfe605bf 100644
--- a/fr3_impedance_controller_real/config/franka_example_controllers.yaml
+++ b/fr3_impedance_controller_real/config/franka_example_controllers.yaml
@@ -1,91 +1,3 @@
-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)
@@ -98,82 +10,170 @@ cartesian_impedance_example_controller:
- $(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)
+# 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
+
+# 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
index 8c7ab949d7951526a050262d6bb70bba529b0e1d..4d5adaa20529f95f86d6718a1563432b6f46be5a 100644
--- a/fr3_impedance_controller_real/default.nix
+++ b/fr3_impedance_controller_real/default.nix
@@ -12,21 +12,25 @@
, franka-description
, franka-gripper
, franka-hw
-, franka-example-controllers
, libfranka
, hardware-interface
, joint-limits-interface
, message-generation
, message-runtime
, geometry-msgs
+, tf2-geometry-msgs
, visualization-msgs
, roscpp
, rospy
, tf
, tf-conversions
, urdf
+, spacenav-node
+, rqt-reconfigure
+#, rviz
, common # shared library from the project
+, fr3-custom-msgs
}:
buildRosPackage {
@@ -46,7 +50,6 @@ buildRosPackage {
franka-gripper
franka-hw
libfranka
- franka-example-controllers
geometry-msgs
hardware-interface
joint-limits-interface
@@ -57,14 +60,18 @@ buildRosPackage {
roscpp rospy
tf
tf-conversions
+ tf2-geometry-msgs
urdf
visualization-msgs
+ spacenav-node
+ rqt-reconfigure
+ #rviz
common
+ fr3-custom-msgs
];
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
deleted file mode 100644
index 1bec1f5d6871804f5fca75b9f2cf7b1d414278dd..0000000000000000000000000000000000000000
--- a/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml
+++ /dev/null
@@ -1,84 +0,0 @@
-<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/franka_example_controllers_plugin.xml b/fr3_impedance_controller_real/franka_example_controllers_plugin.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ba2baaedf3be020c491682815892aded108636ff
--- /dev/null
+++ b/fr3_impedance_controller_real/franka_example_controllers_plugin.xml
@@ -0,0 +1,83 @@
+<library path="lib/libfranka_example_controllers">
+ <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/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> -->
+ <!-- <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/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
deleted file mode 100644
index bfe738d9b80449658c32b9823d5dfe913f019099..0000000000000000000000000000000000000000
--- a/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
+++ /dev/null
@@ -1,116 +0,0 @@
-// 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/include/franka_example_controllers/cartesian_impedance_example_controller.h b/fr3_impedance_controller_real/include/franka_example_controllers/cartesian_impedance_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..024d62d53695b06276835ebbf935fde6b3485fc1
--- /dev/null
+++ b/fr3_impedance_controller_real/include/franka_example_controllers/cartesian_impedance_example_controller.h
@@ -0,0 +1,202 @@
+// 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>
+
+/* ROS packages */
+#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <geometry_msgs/PoseStamped.h> // interactive marker
+#include <geometry_msgs/Twist.h> // spacenave teleop
+#include <sensor_msgs/Joy.h> // joystick
+#include "fr3_custom_msgs/ControllerState.h" // custom message
+
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+
+// #include <franka_example_controllers/compliance_paramConfig.h>
+#include "franka_example_controllers/controller_paramConfig.h"
+
+/* Dependencies */
+#include "pinocchio/algorithm/crba.hpp" // joint-space inertia (upper-triangle)
+#include "pinocchio/algorithm/frames.hpp" //
+#include "pinocchio/algorithm/jacobian.hpp" //
+#include "pinocchio/algorithm/kinematics.hpp" //
+#include "pinocchio/algorithm/rnea.hpp" // Coriolis, gravity, joint torque
+#include "pinocchio/autodiff/casadi.hpp" //
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+
+/* Custom libraries */
+#include "DisturbanceObserver.hpp"
+#include "ImpedanceController.hpp"
+#include "MomentumObserver.hpp"
+#include "SlidingModeObserver.hpp"
+
+#include "eigen_types.hpp"
+#include "Timer.hpp"
+
+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 update(const ros::Time &, const ros::Duration & period) override;
+ // void stopping(const ros::Time &) override;
+
+private:
+ std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
+ std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+ std::vector<hardware_interface::JointHandle> joint_handles_;
+
+ Timer loop_timer_;
+ double max_exec_time_ = 0.0;
+ // int tick_ = 0; // loop iteration
+ double loop_time_ = 0.0; // loop elapsed time
+
+ /* Robot Model */
+ // NOTE: you need to specify absolute path (not relative) for URDF file.
+ std::string repo_path_ = "/home/ipk410/nix-project/impedance_controller";
+ std::string urdf_filename_ = std::string(repo_path_ + "/assets/model/fr3_franka_hand.urdf");
+ pinocchio::Model model_;
+ pinocchio::Data data_;
+ int ee_id_;
+
+ //* ----- Constants ------------------------------------------------------------------------------
+ const double delta_tau_max_{ 1.0 };
+ const double twist_scale_[6] = { 0.15, 0.15, 0.15, 0.8, 0.8, 0.8 };
+ const double alpha_ = 0.005; // targeting filter coefficient
+ // const double spacenav_cmd_max_{ 0.6834 };
+
+ // TODO: make it as rosparam (optional)
+ const Vec6<double> fr3_vel_limit_ = // [m(rad)/s]
+ (Vec6<double>() << 3.0, 3.0, 3.0, 2.5, 2.5, 2.5).finished();
+ const Vec6<double> fr3_acc_limit_ = // [m(rad)/s^2]
+ (Vec6<double>() << 9.0, 9.0, 9.0, 17.0, 17.0, 17.0).finished();
+ const Vec6<double> fr3_jrk_limit_ = // [m(rad)/s^3]
+ (Vec6<double>() << 4500.0, 4500.0, 4500.0, 8500.0, 8500.0, 8500.0).finished();
+
+ //* ----- Booleans to control --------------------------------------------------------------------
+ bool b_count_loop_time_ = false;
+
+ bool b_dob_applied_ = false;
+ bool b_sosml_applied_ = false;
+ bool b_mob_applied_ = false;
+
+ //* ----- Publisher & Subscriber -----------------------------------------------------------------
+ /* Controller state publisher */
+ ros::Publisher pub_ctrl_state_;
+ fr3_custom_msgs::ControllerState msg_ctrl_state_;
+
+ /* Target EE Twist & Gripper command from Spacenav (or PS3) Joystick */
+ ros::Subscriber sub_target_twist_;
+ ros::Subscriber sub_joy_;
+ // std::array<int, 2> btn_cmd_;
+ Vec6<double> ee_vel_world_target_;
+
+ /* Target EE Pose command from Interactive Marker */
+ // ros::Subscriber sub_ee_pose_;
+ // std::mutex target_ee_pose_mutex_;
+ // Vec3<double> ee_pos_world_target_;
+ // Quat<double> ee_quat_world_target_;
+ // bool b_is_marker_mode_ = false;
+
+ //* ----- Control Parameters ---------------------------------------------------------------------
+ /* Dynamic Reconfigure Server */
+ std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::controller_paramConfig>>
+ ctrl_param_server_;
+ ros::NodeHandle ctrl_param_node_;
+
+ Mat6<double> K_task_, K_task_target_; // cartesian stiffness [N/m, Nm/rad]
+ Mat6<double> D_task_, D_task_target_; // cartesian damping [Ns/m, Nms/rad]
+ Mat7<double> K_null_, K_null_target_; // null-sapce stiffness [Nm/rad]
+ Mat7<double> D_null_, D_null_target_; // null-space damping [Nms/rad]
+
+ Vec6<double> fq_, fq_target_; // DOB low-pass filter cutoff-frequency [Hz]
+ Vec7<double> s1_, s1_target_, s2_, s2_target_; // SOSML tuning parameters [Hz^2]
+ Vec7<double> t1_, t1_target_, t2_, t2_target_; // SOSML tuning parameters [Hz]
+ Vec7<double> r_th_, r_th_target_; // MOB residual threshold [Nm]
+ Vec7<double> km_, km_target_; // MOB low-pass filter gain [rad/s]
+
+ //* ----- Control States -------------------------------------------------------------------------
+ /* Joint states */
+ Vec7<double> q_mes_;
+ Vec7<double> q_null_des_;
+ Vec7<double> dq_mes_, dq_mes_prev_;
+ Vec7<double> ddq_eul_, ddq_eul_prev_; // joint accel using backward Euler method
+
+ /* Desired EE states */
+ Vec3<double> ee_pos_world_des_;
+ Quat<double> ee_quat_world_des_;
+ Vec3<double> ee_rpy_world_des_, ee_rpy_world_des_prev_;
+ Vec6<double> ee_vel_world_des_, ee_vel_world_des_prev_;
+ Vec6<double> ee_acc_world_des_, ee_acc_world_des_prev_;
+
+ /* Measured EE states */
+ Vec3<double> ee_pos_world_mes_;
+ Quat<double> ee_quat_world_mes_;
+ Vec3<double> ee_rpy_world_mes_, ee_rpy_world_mes_prev_;
+ Vec6<double> ee_vel_world_mes_, ee_vel_world_mes_prev_; // v = J * dq
+ Vec6<double> ee_acc_world_eul_, ee_acc_world_eul_prev_; // task accel by backward Euler method
+
+ /* Controllers */
+ std::unique_ptr<ImpedanceController<double>> imp_;
+ std::unique_ptr<DisturbanceObserver<double>> dob_;
+ Vec7<double> tau_d_hat_dob_;
+ Vec6<double> Fd_hat_dob_;
+
+ std::unique_ptr<SlidingModeObserver<double>> sosml_;
+ Vec7<double> ddq_hat_sosml_;
+ Vec7<double> tau_d_hat_sosml_;
+ Vec6<double> ee_acc_world_hat_sosml_;
+ Vec6<double> Fd_hat_sosml_;
+
+ std::unique_ptr<MomentumObserver<double>> mob_;
+ Vec7<double> h_mes_, h_mes_prev_; // General Momentum
+ Vec7<double> ddq_hat_mob_;
+ Vec6<double> ee_acc_world_hat_mob_;
+ Vec7<double> tau_d_hat_mob_;
+ Vec6<double> Fd_hat_mob_;
+
+ Vec6<double> Fd_hat_fr_; // franka's external force estimation
+
+public:
+ //* ----- SUBSCRIBE CALLBACK ---------------------------------------------------------------------
+ void subscribe_target_twist(const geometry_msgs::TwistConstPtr & msg);
+ void subscribe_joy_command(const sensor_msgs::JoyConstPtr & msg);
+
+ //* ----- PARAMETER SERVER CALLBACK --------------------------------------------------------------
+ void request_controller_parameters(franka_example_controllers::controller_paramConfig & config,
+ uint32_t level);
+
+ //* ----- METHODS --------------------------------------------------------------------------------
+ void init_states();
+ void update_previous_states();
+ void publish_controller_state();
+
+ /* joint torque 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)
+
+ //* ----- PRINTER --------------------------------------------------------------------------------
+ void print_end_effector_states();
+ void print_dynamics();
+};
+
+} // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/include/franka_example_controllers/pseudo_inversion.h b/fr3_impedance_controller_real/include/franka_example_controllers/pseudo_inversion.h
new file mode 100644
index 0000000000000000000000000000000000000000..c820927483012b31290f30226806b70a3c917937
--- /dev/null
+++ b/fr3_impedance_controller_real/include/franka_example_controllers/pseudo_inversion.h
@@ -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
diff --git a/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch b/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch
index 682f04cfcd180840857522b592d624f7b1c2eae4..c62f6e71d5dabaffcefc2269b335461bcbd0464b 100644
--- a/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch
+++ b/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch
@@ -1,20 +1,20 @@
<?xml version="1.0" ?>
<launch>
- <arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
+ <arg name="robot" default="fr3" 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"/>
+ <!-- <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">
+ <!-- <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> -->
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
</launch>
diff --git a/fr3_impedance_controller_real/package.xml b/fr3_impedance_controller_real/package.xml
index 297f2003c5617cd900dc32956aec47fee2b122c8..8530cf3f9da06361fbc5f890784d86356471bfe3 100644
--- a/fr3_impedance_controller_real/package.xml
+++ b/fr3_impedance_controller_real/package.xml
@@ -1,11 +1,15 @@
<?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>
+ <name>franka_example_controllers</name>
+ <version>0.10.1</version>
+ <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros_control</description>
+ <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
<license>Apache 2.0</license>
+ <url type="website">http://wiki.ros.org/franka_example_controllers</url>
+ <url type="repository">https://github.com/frankaemika/franka_ros</url>
+ <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
+ <author>Franka Emika GmbH</author>
<buildtool_depend>catkin</buildtool_depend>
@@ -19,7 +23,6 @@
<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>
@@ -30,7 +33,6 @@
<depend>realtime_tools</depend>
<depend>roscpp</depend>
<depend>urdf</depend>
- <depend>sensor_msgs</depend>
<depend>visualization_msgs</depend>
<exec_depend>franka_control</exec_depend>
@@ -39,6 +41,6 @@
<exec_depend>rospy</exec_depend>
<export>
- <controller_interface plugin="${prefix}/fr3_impedance_controller_real_plugin.xml" />
+ <controller_interface plugin="${prefix}/franka_example_controllers_plugin.xml" />
</export>
</package>
diff --git a/fr3_impedance_controller_real/src/cartesian_impedance_example_controller.cpp b/fr3_impedance_controller_real/src/cartesian_impedance_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..59dc9f652ff84164c8d71856a1bee76e010dd44a
--- /dev/null
+++ b/fr3_impedance_controller_real/src/cartesian_impedance_example_controller.cpp
@@ -0,0 +1,620 @@
+// 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>
+
+/* Custom libraries */
+#include "orientation_utilities.hpp"
+#include "robotics_utilities.hpp"
+
+namespace pin = pinocchio;
+namespace ori = orient_utils;
+namespace robo = robot_utils;
+
+using namespace std;
+// using pin::ReferenceFrame;
+
+namespace franka_example_controllers
+{
+bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW * robot_hw,
+ ros::NodeHandle & node_handle)
+{
+ //* Load URDF model and data *//
+ if (urdf_filename_.empty())
+ {
+ throw std::runtime_error("URDF filename is invalid");
+ }
+ pin::urdf::buildModel(urdf_filename_, model_);
+ data_ = pin::Data(model_);
+ ee_id_ = model_.getFrameId("fr3_hand_tcp", (pin::FrameType::BODY));
+
+ //* Set Publisher *//
+ pub_ctrl_state_ = node_handle.advertise<fr3_custom_msgs::ControllerState>("controller_state", 20);
+
+ //* Set Subscriber *//
+ // NOTE: you need to change topic name as `/spacenav/joy` if you want to command gripper
+ sub_joy_ = node_handle.subscribe<sensor_msgs::Joy>(
+ "/joy", 1, &CartesianImpedanceExampleController::subscribe_joy_command, this);
+
+ // FIXME: spacenav is not operating !!
+ // sub_target_twist_ = node_handle.subscribe<geometry_msgs::Twist>(
+ // "/spacenav/twist", 1, &CartesianImpedanceExampleController::subscribe_target_twist, this);
+
+ //* Set HW interface *//
+ 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;
+ }
+ }
+
+ //* Initialize states *//
+ this->init_states(); // NOTE: this should be executed aforehand of parameter callback
+
+ //* Parameter server declaration *//
+ ctrl_param_node_ = ros::NodeHandle(node_handle.getNamespace() + "/controller_param_node");
+
+ ctrl_param_server_ = std::make_unique<
+ dynamic_reconfigure::Server<franka_example_controllers::controller_paramConfig>>(
+ ctrl_param_node_);
+
+ ctrl_param_server_->setCallback(
+ boost::bind(&CartesianImpedanceExampleController::request_controller_parameters, this, _1, _2));
+
+ //* Create controllers *//
+ imp_ = std::make_unique<ImpedanceController<double>>();
+ dob_ = std::make_unique<DisturbanceObserver<double>>(fq_);
+ mob_ = std::make_unique<MomentumObserver<double>>(r_th_, km_);
+ sosml_ = std::make_unique<SlidingModeObserver<double>>(s1_, s2_, t1_, t2_);
+
+ return true;
+}
+
+void CartesianImpedanceExampleController::starting(const ros::Time & /*time*/)
+{
+ // compute initial velocity with jacobian and set x_attractor
+ franka::RobotState initial_state = state_handle_->getRobotState();
+ Eigen::Map<Vec7<double>> q_init(initial_state.q.data());
+ Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
+
+ /* set equilibrium pose to current state */
+ ee_pos_world_des_ = initial_transform.translation();
+ ee_quat_world_des_ = Eigen::Quaterniond(initial_transform.rotation());
+
+ /* set nullspace equilibrium configuration to initial q */
+ q_null_des_ = q_init;
+}
+
+void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
+ const ros::Duration & period)
+{
+ if (b_count_loop_time_ == true)
+ {
+ loop_timer_.start();
+ }
+ // cout << std::fixed;
+ // cout << std::setprecision(5);
+ // TODO: add safety logic for time derivative when dt is very small
+ // double dt = period.toSec(); // timestep
+ double dt = 0.001;
+
+ //* ----- set Desired EE Pose & Twist ------------------------------------------------------------
+ // 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 * dt;
+ ee_quat_world_des_ = ori::getNewQuat(ee_quat_world_des_, w_des, dt);
+ // ee_rpy_world_des_ = ori::quaternionToRPY(ee_quat_world_des_, ee_rpy_world_des_prev_);
+
+ //* ----- get Current States --------------------------------------------------------------------
+ franka::RobotState robot_state = state_handle_->getRobotState();
+ Eigen::Map<Vec7<double>> tau_J_d(robot_state.tau_J_d.data()); // NOLINT
+ Eigen::Map<const Vec6<double>> tau_ext_hat_filtered(robot_state.tau_ext_hat_filtered.data());
+ q_mes_ = Eigen::Map<const Vec7<double>>(robot_state.q.data());
+ dq_mes_ = Eigen::Map<const Vec7<double>>(robot_state.dq.data());
+
+ // TODO: check noise level
+ // ddq_eul_ = robo::getEulerDerivative(dq_mes_, dq_mes_prev_, dt);
+ ddq_eul_ = robo::getFilteredEulerDerivative(dq_mes_, dq_mes_prev_, ddq_eul_prev_, dt, 10.0);
+
+ pin::forwardKinematics(model_, data_, q_mes_, dq_mes_);
+ pin::updateFramePlacements(model_, data_);
+
+ ee_pos_world_mes_ = data_.oMf[ee_id_].translation();
+ RotMat<double> R_OE = data_.oMf[ee_id_].rotation();
+ ee_quat_world_mes_ = Quat<double>(R_OE);
+ // ee_rpy_world_mes_ = ori::quaternionToRPY(ee_quat_world_mes_, ee_rpy_world_mes_prev_);
+
+ //* ----- compute Kinematics & Dynamics ----------------------------------------------------------
+ /* Jacobian */
+ Mat67<double> Je, dJe;
+ pin::getFrameJacobian(model_, data_, ee_id_, pin::ReferenceFrame::LOCAL_WORLD_ALIGNED, Je);
+ pin::getFrameJacobianTimeVariation(model_, data_, ee_id_,
+ pin::ReferenceFrame::LOCAL_WORLD_ALIGNED, dJe);
+
+ Mat76<double> Je_t = Je.transpose();
+ MatX<double> Je_pinv, Je_t_pinv;
+ pseudoInverse(Je, Je_pinv);
+ pseudoInverse(Je_t, Je_t_pinv);
+ // NOTE: using `pseudoInverse()` might be more safe against singularities
+ // MatX<double> Je_pinv = Je.completeOrthogonalDecomposition().pseudoInverse();
+ // MatX<double> Je_t_pinv = Je.transpose().completeOrthogonalDecomposition().pseudoInverse();
+
+ /* compute current EE Twist & Acceleration using backward Euler method */
+ ee_vel_world_mes_ = Je * dq_mes_;
+ // ee_acc_world_eul_ = robo::getEulerDerivative(ee_vel_world_mes_, ee_vel_world_mes_prev_, dt);
+ ee_acc_world_eul_ = robo::getFilteredEulerDerivative(ee_vel_world_mes_, ee_vel_world_mes_prev_, ee_acc_world_eul_prev_, dt, 10.0);
+
+ /* Inertia matrix */
+ pin::crba(model_, data_, q_mes_);
+ // Mat7<double> M = data_.M; // upper triangular matrix in joint-space
+ // Vec7<double> k_mass;
+ // k_mass << 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 50.0;
+ // M.diagonal().array() *= k_mass.array();
+ Mat7<double> M = robo::computeFullInertiaMatrix(data_.M);
+ Mat6<double> Me = robo::computeTaskInertiaMatrix(M, Je_pinv, Je_t_pinv);
+
+ /* Coriolis, Centrifugal & Gravity torque */
+ pin::computeCoriolisMatrix(model_, data_, q_mes_, dq_mes_);
+ Mat7<double> C = data_.C;
+ Vec7<double> tau_c = C * dq_mes_;
+
+ // pin::computeGeneralizedGravity(model_, data_, q_mes_); // * gravity is compensated internally
+ // Vec7<double> tau_g = data_.g;
+ Vec7<double> tau_g;
+ tau_g.setZero();
+
+ //* ----- CONTROL LAW ----------------------------------------------------------------------------
+ /* Cartesian impedance control */
+ imp_->set_parameters(K_task_, D_task_, K_null_, D_null_);
+ Vec6<double> ee_pose_err = imp_->get_pose_error(ee_pos_world_des_, ee_pos_world_mes_,
+ ee_quat_world_des_, ee_quat_world_mes_);
+ Vec6<double> ee_pose_err_prev = imp_->get_previous_pose_error();
+ Vec6<double> dpose_err = robo::getEulerDerivative(ee_pose_err, ee_pose_err_prev, dt);
+ Vec6<double> F_task = imp_->get_task_impedance_force(dpose_err);
+
+ /* Task-Force Disturbance compensation */
+ Vec7<double> tau_task = Je_t * (F_task - (b_dob_applied_ ? Fd_hat_dob_ : Vec6<double>::Zero()));
+
+ /* Null-space impedance control */
+ imp_->compute_null_error(q_null_des_, q_mes_);
+ Vec7<double> tau_null = imp_->get_null_impedance_torque(dq_mes_, Je_t, Je_t_pinv);
+
+ /* compute desired joint torque command */
+ Vec7<double> tau_des = tau_task + tau_null + tau_c;
+ tau_des << saturateTorqueRate(tau_des, tau_J_d); // Saturate torque rate to avoid discontinuities
+
+ //* ----- Observers ------------------------------------------------------------------------------
+ Fd_hat_fr_ = Je_t_pinv * tau_ext_hat_filtered;
+
+ /* Momentum Observer (MOB) */
+ h_mes_ = M * dq_mes_;
+ Vec7<double> dh_mes = robo::getEulerDerivative(h_mes_, h_mes_prev_, dt);
+
+ mob_->set_parameters(dt, r_th_, km_); // TODO: set parameter similar level to DOB for comparison
+ mob_->set_dynamics(tau_des, M, C, tau_g);
+ mob_->compute_residual(dh_mes, dq_mes_);
+
+ ddq_hat_mob_ = mob_->get_joint_accel_estimation(dq_mes_);
+ ee_acc_world_hat_mob_ = robo::getTaskAcceleration(Je, dJe, dq_mes_, ddq_hat_mob_);
+ tau_d_hat_mob_ = mob_->get_external_torque_estimation();
+ Fd_hat_mob_ = Je_t_pinv * tau_d_hat_mob_;
+
+ /* 2nd-order Sliding mode Observer (SOSML) */
+ sosml_->set_parameters(dt, s1_, s2_, t1_, t2_);
+ sosml_->set_dynamics(tau_des, M, C, tau_g);
+ sosml_->compute_momentum_estimation(dq_mes_);
+ ddq_hat_sosml_ = sosml_->get_joint_accel_estimation();
+ ee_acc_world_hat_sosml_ = robo::getTaskAcceleration(Je, dJe, dq_mes_, ddq_hat_sosml_);
+ tau_d_hat_sosml_ = sosml_->get_external_torque_estimation();
+ Fd_hat_sosml_ = Je_t_pinv * tau_d_hat_sosml_;
+
+ /* Task-space Force Disturbance Observer (TFDOB) */
+ dob_->set_parameters(dt, fq_);
+ Vec6<double> ddx = (b_sosml_applied_ ? ee_acc_world_hat_sosml_ : ee_acc_world_eul_);
+ Fd_hat_dob_ = dob_->get_force_disturbance(F_task, Me, ddx);
+ tau_d_hat_dob_ = Je_t * Fd_hat_dob_;
+
+ //* send joint torque command *//
+ for (size_t i = 0; i < 7; ++i)
+ {
+ joint_handles_[i].setCommand(tau_des(i));
+ }
+ this->publish_controller_state(); // publish states for rosbag
+
+ //* update previous states *//
+ this->update_previous_states();
+ imp_->update_previous_states();
+ dob_->update_previous_states();
+
+ //* update parameters online either via dynamic reconfigure or target by filtering *//
+ /* update desired twist via joystick */
+ ee_vel_world_des_ = alpha_ * ee_vel_world_target_ + (1.0 - alpha_) * ee_vel_world_des_;
+
+ /* update controller parameters */
+ K_task_ = alpha_ * K_task_target_ + (1.0 - alpha_) * K_task_;
+ D_task_ = alpha_ * D_task_target_ + (1.0 - alpha_) * D_task_;
+ K_null_ = alpha_ * K_null_target_ + (1.0 - alpha_) * K_null_;
+ D_null_ = alpha_ * D_null_target_ + (1.0 - alpha_) * D_null_;
+
+ fq_ = alpha_ * fq_target_ + (1.0 - alpha_) * fq_;
+ s1_ = alpha_ * s1_target_ + (1.0 - alpha_) * s1_;
+ s2_ = alpha_ * s2_target_ + (1.0 - alpha_) * s2_;
+ t1_ = s1_.array().sqrt();
+ t2_ = s2_.array().sqrt();
+ r_th_ = alpha_ * r_th_target_ + (1.0 - alpha_) * r_th_;
+ km_ = alpha_ * km_target_ + (1.0 - alpha_) * km_;
+
+ if(b_count_loop_time_ == true)
+ {
+ if(max_exec_time_ < loop_timer_.getMillisec())
+ {
+ max_exec_time_ = loop_timer_.getMillisec();
+ }
+ cout << "[Execution time] " << loop_timer_.getMillisec() << "(msec) [Max] " << max_exec_time_ << endl;
+ }
+}
+
+//* ----- SUBSCRIBE CALLBACK -----------------------------------------------------------------------
+void CartesianImpedanceExampleController::subscribe_target_twist(
+ const geometry_msgs::TwistConstPtr & msg)
+{
+ // cout << msg->linear.x << ", " << msg->linear.y << ", " << msg->linear.z << endl;
+ ee_vel_world_des_[0] = twist_scale_[0] * msg->linear.x;
+ ee_vel_world_des_[1] = twist_scale_[1] * msg->linear.y;
+ ee_vel_world_des_[2] = twist_scale_[2] * msg->linear.z;
+ ee_vel_world_des_[3] = twist_scale_[3] * msg->angular.x;
+ ee_vel_world_des_[4] = twist_scale_[4] * msg->angular.y;
+ ee_vel_world_des_[5] = twist_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];
+
+ ee_vel_world_target_[0] = static_cast<double>(twist_scale_[0] * msg->axes[0]); // x
+ ee_vel_world_target_[1] = static_cast<double>(twist_scale_[1] * msg->axes[1]); // y
+ ee_vel_world_target_[2] = static_cast<double>(twist_scale_[2] * msg->axes[5]); // z
+ ee_vel_world_target_[3] = static_cast<double>(twist_scale_[3] * msg->axes[2]); // roll
+ ee_vel_world_target_[4] = static_cast<double>(twist_scale_[4] * msg->axes[3]); // pitch
+ ee_vel_world_target_[5] = static_cast<double>(twist_scale_[5] * msg->axes[4]); // yaw
+}
+
+//* ----- PARAMETER SERVER CALLBACK ----------------------------------------------------------------
+void CartesianImpedanceExampleController::request_controller_parameters(
+ franka_example_controllers::controller_paramConfig & config, uint32_t /*level*/)
+{
+ //* Impedance Controller (NOTE: Damping ratio = 1) *//
+ /* task-space PD gains */
+ K_task_target_.setIdentity().diagonal() << config.K_task_x, config.K_task_y, config.K_task_z, //
+ config.K_task_rot, config.K_task_rot, config.K_task_rot;
+ D_task_target_.setIdentity().diagonal() << 2.0 * K_task_target_.diagonal().array().sqrt();
+
+ /* null-space PD gains */
+ K_null_target_.setIdentity().diagonal() << config.K_null, config.K_null, config.K_null, //
+ config.K_null, config.K_null, config.K_null, config.K_null;
+ D_null_target_.setIdentity().diagonal() << 2.0 * K_null_target_.diagonal().array().sqrt();
+
+ //* Disturbance Observer *//
+ fq_target_ << config.fq, config.fq, config.fq, config.fq, config.fq, config.fq;
+
+ //* Sliding Mode Observer *//
+ s1_target_ << config.s1, config.s1, config.s1, config.s1, config.s1, config.s1, config.s1;
+ s2_target_ << config.s2, config.s2, config.s2, config.s2, config.s2, config.s2, config.s2;
+
+ //* Momentum Observer *//
+ r_th_target_ << config.r_th, config.r_th, config.r_th, config.r_th, config.r_th, config.r_th,
+ config.r_th;
+ km_target_ << config.Km, config.Km, config.Km, config.Km, config.Km, config.Km, config.Km;
+}
+
+//* ----- METHODS ----------------------------------------------------------------------------------
+void CartesianImpedanceExampleController::init_states()
+{
+ //* subscriber *//
+ ee_vel_world_target_.setZero();
+
+ //* control parameters *//
+ /* impedance */
+ K_task_.setIdentity().diagonal() << 200.0, 200.0, 200.0, 10.0, 10.0, 10.0; // TODO: set default
+ K_task_target_.setIdentity();
+ D_task_.setIdentity().diagonal() << 2.0 * K_task_.diagonal().array().sqrt();
+ D_task_target_.setIdentity();
+ K_null_.setIdentity().diagonal() << 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5;
+ K_null_target_.setIdentity();
+ D_null_.setIdentity().diagonal() << 2.0 * K_null_.diagonal().array().sqrt();
+ D_null_target_.setIdentity();
+
+ /* DOB */
+ fq_ << 15.0, 15.0, 15.0, 15.0, 15.0, 15.0;
+ fq_target_.setZero();
+
+ /* SOSML */
+ s1_ << 10, 10, 10, 10, 10, 10, 10;
+ s1_target_.setZero();
+ s2_ << 200, 200, 200, 200, 200, 200, 200;
+ s2_target_.setZero();
+ t1_ = s1_.array().sqrt();
+ t1_target_.setZero();
+ t2_ = s2_.array().sqrt();
+ t2_target_.setZero();
+
+ /* MOB */
+ r_th_ << 0, 0, 0, 0, 0, 0, 0;
+ r_th_target_.setZero();
+ km_ << 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5;
+ km_target_.setZero();
+
+ //* Control states *//
+ q_mes_.setZero();
+ dq_mes_.setZero();
+ dq_mes_prev_.setZero();
+ ddq_eul_.setZero();
+ ddq_eul_prev_.setZero();
+ q_null_des_.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_vel_world_des_.setZero();
+ ee_vel_world_des_prev_.setZero();
+ ee_acc_world_des_.setZero();
+ ee_acc_world_des_prev_.setZero();
+
+ ee_pos_world_mes_.setZero();
+ ee_quat_world_mes_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+ ee_rpy_world_mes_.setZero();
+ ee_rpy_world_mes_prev_.setZero();
+ ee_vel_world_mes_.setZero();
+ ee_vel_world_mes_prev_.setZero();
+ ee_acc_world_eul_.setZero();
+ ee_acc_world_eul_prev_.setZero();
+
+ tau_d_hat_dob_.setZero();
+ Fd_hat_dob_.setZero();
+
+ ddq_hat_sosml_.setZero();
+ tau_d_hat_sosml_.setZero();
+ ee_acc_world_hat_sosml_.setZero();
+ Fd_hat_sosml_.setZero();
+
+ h_mes_.setZero();
+ h_mes_prev_.setZero();
+ ddq_hat_mob_.setZero();
+ ee_acc_world_hat_mob_.setZero();
+ tau_d_hat_mob_.setZero();
+ Fd_hat_mob_.setZero();
+
+ Fd_hat_fr_.setZero();
+}
+
+void CartesianImpedanceExampleController::publish_controller_state()
+{
+ for (int i = 0; i < 7; ++i)
+ {
+ msg_ctrl_state_.ddq_eul[i] = ddq_eul_[i];
+ msg_ctrl_state_.ddq_mob[i] = ddq_hat_mob_[i];
+ msg_ctrl_state_.ddq_sosml[i] = ddq_hat_sosml_[i];
+
+ msg_ctrl_state_.tau_dist_dob[i] = tau_d_hat_dob_[i];
+ msg_ctrl_state_.tau_dist_mob[i] = tau_d_hat_mob_[i];
+ msg_ctrl_state_.tau_dist_sosml[i] = tau_d_hat_sosml_[i];
+ }
+
+ msg_ctrl_state_.F_dist_franka.force.x = Fd_hat_fr_[0];
+ msg_ctrl_state_.F_dist_franka.force.y = Fd_hat_fr_[1];
+ msg_ctrl_state_.F_dist_franka.force.z = Fd_hat_fr_[2];
+ msg_ctrl_state_.F_dist_franka.torque.x = Fd_hat_fr_[3];
+ msg_ctrl_state_.F_dist_franka.torque.y = Fd_hat_fr_[4];
+ msg_ctrl_state_.F_dist_franka.torque.z = Fd_hat_fr_[5];
+
+ msg_ctrl_state_.F_dist_dob.force.x = Fd_hat_dob_[0];
+ msg_ctrl_state_.F_dist_dob.force.y = Fd_hat_dob_[1];
+ msg_ctrl_state_.F_dist_dob.force.z = Fd_hat_dob_[2];
+ msg_ctrl_state_.F_dist_dob.torque.x = Fd_hat_dob_[3];
+ msg_ctrl_state_.F_dist_dob.torque.y = Fd_hat_dob_[4];
+ msg_ctrl_state_.F_dist_dob.torque.z = Fd_hat_dob_[5];
+
+ msg_ctrl_state_.F_dist_mob.force.x = Fd_hat_mob_[0];
+ msg_ctrl_state_.F_dist_mob.force.y = Fd_hat_mob_[1];
+ msg_ctrl_state_.F_dist_mob.force.z = Fd_hat_mob_[2];
+ msg_ctrl_state_.F_dist_mob.torque.x = Fd_hat_mob_[3];
+ msg_ctrl_state_.F_dist_mob.torque.y = Fd_hat_mob_[4];
+ msg_ctrl_state_.F_dist_mob.torque.z = Fd_hat_mob_[5];
+
+ /* task-space pose */
+ msg_ctrl_state_.ee_pose_world_des.position.x = ee_pos_world_des_[0];
+ msg_ctrl_state_.ee_pose_world_des.position.y = ee_pos_world_des_[1];
+ msg_ctrl_state_.ee_pose_world_des.position.z = ee_pos_world_des_[2];
+ msg_ctrl_state_.ee_pose_world_des.orientation.x = ee_quat_world_des_.x();
+ msg_ctrl_state_.ee_pose_world_des.orientation.y = ee_quat_world_des_.y();
+ msg_ctrl_state_.ee_pose_world_des.orientation.z = ee_quat_world_des_.z();
+ msg_ctrl_state_.ee_pose_world_des.orientation.w = ee_quat_world_des_.w();
+
+ msg_ctrl_state_.ee_pose_world_mes.position.x = ee_pos_world_mes_[0];
+ msg_ctrl_state_.ee_pose_world_mes.position.y = ee_pos_world_mes_[1];
+ msg_ctrl_state_.ee_pose_world_mes.position.z = ee_pos_world_mes_[2];
+ msg_ctrl_state_.ee_pose_world_mes.orientation.x = ee_quat_world_mes_.x();
+ msg_ctrl_state_.ee_pose_world_mes.orientation.y = ee_quat_world_mes_.y();
+ msg_ctrl_state_.ee_pose_world_mes.orientation.z = ee_quat_world_mes_.z();
+ msg_ctrl_state_.ee_pose_world_mes.orientation.w = ee_quat_world_mes_.w();
+
+ /* task-space twist */
+ msg_ctrl_state_.ee_vel_world_des.linear.x = ee_vel_world_des_[0];
+ msg_ctrl_state_.ee_vel_world_des.linear.y = ee_vel_world_des_[1];
+ msg_ctrl_state_.ee_vel_world_des.linear.z = ee_vel_world_des_[2];
+ msg_ctrl_state_.ee_vel_world_des.angular.x = ee_vel_world_des_[3];
+ msg_ctrl_state_.ee_vel_world_des.angular.y = ee_vel_world_des_[4];
+ msg_ctrl_state_.ee_vel_world_des.angular.z = ee_vel_world_des_[5];
+
+ msg_ctrl_state_.ee_vel_world_mes.linear.x = ee_vel_world_mes_[0];
+ msg_ctrl_state_.ee_vel_world_mes.linear.y = ee_vel_world_mes_[1];
+ msg_ctrl_state_.ee_vel_world_mes.linear.z = ee_vel_world_mes_[2];
+ msg_ctrl_state_.ee_vel_world_mes.angular.x = ee_vel_world_mes_[3];
+ msg_ctrl_state_.ee_vel_world_mes.angular.y = ee_vel_world_mes_[4];
+ msg_ctrl_state_.ee_vel_world_mes.angular.z = ee_vel_world_mes_[5];
+
+ /* task-space acceleration */
+ msg_ctrl_state_.ee_acc_world_eul.linear.x = ee_acc_world_eul_[0];
+ msg_ctrl_state_.ee_acc_world_eul.linear.y = ee_acc_world_eul_[1];
+ msg_ctrl_state_.ee_acc_world_eul.linear.z = ee_acc_world_eul_[2];
+ msg_ctrl_state_.ee_acc_world_eul.angular.x = ee_acc_world_eul_[3];
+ msg_ctrl_state_.ee_acc_world_eul.angular.y = ee_acc_world_eul_[4];
+ msg_ctrl_state_.ee_acc_world_eul.angular.z = ee_acc_world_eul_[5];
+
+ msg_ctrl_state_.ee_acc_world_mob.linear.x = ee_acc_world_hat_mob_[0];
+ msg_ctrl_state_.ee_acc_world_mob.linear.y = ee_acc_world_hat_mob_[1];
+ msg_ctrl_state_.ee_acc_world_mob.linear.z = ee_acc_world_hat_mob_[2];
+ msg_ctrl_state_.ee_acc_world_mob.angular.x = ee_acc_world_hat_mob_[3];
+ msg_ctrl_state_.ee_acc_world_mob.angular.y = ee_acc_world_hat_mob_[4];
+ msg_ctrl_state_.ee_acc_world_mob.angular.z = ee_acc_world_hat_mob_[5];
+
+ msg_ctrl_state_.ee_acc_world_sosml.linear.x = ee_acc_world_hat_sosml_[0];
+ msg_ctrl_state_.ee_acc_world_sosml.linear.y = ee_acc_world_hat_sosml_[1];
+ msg_ctrl_state_.ee_acc_world_sosml.linear.z = ee_acc_world_hat_sosml_[2];
+ msg_ctrl_state_.ee_acc_world_sosml.angular.x = ee_acc_world_hat_sosml_[3];
+ msg_ctrl_state_.ee_acc_world_sosml.angular.y = ee_acc_world_hat_sosml_[4];
+ msg_ctrl_state_.ee_acc_world_sosml.angular.z = ee_acc_world_hat_sosml_[5];
+
+ pub_ctrl_state_.publish(msg_ctrl_state_);
+}
+
+void CartesianImpedanceExampleController::update_previous_states()
+{
+ dq_mes_prev_ = dq_mes_;
+ ddq_eul_prev_ = ddq_eul_;
+ ee_rpy_world_des_prev_ = ee_rpy_world_des_;
+ ee_vel_world_des_prev_ = ee_vel_world_des_;
+ ee_acc_world_des_prev_ = ee_acc_world_des_;
+ ee_rpy_world_mes_prev_ = ee_rpy_world_mes_;
+ ee_vel_world_mes_prev_ = ee_vel_world_mes_;
+ ee_acc_world_eul_prev_ = ee_acc_world_eul_;
+
+ h_mes_prev_ = h_mes_;
+}
+
+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;
+}
+
+//* ----- PRINTER ----------------------------------------------------------------------------------
+void CartesianImpedanceExampleController::print_end_effector_states()
+{
+ // cout << "Target EE twist:\t" << ee_vel_world_target_.transpose() << endl;
+ // cout << "Desired EE twist:\t" << ee_vel_world_des_.transpose() << endl;
+ // std::cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << std::endl;
+ // std::cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << std::endl;
+ cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << endl;
+ cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << endl;
+ cout << "Measured EE RPY [rad]:\t" << ee_rpy_world_mes_.transpose() << endl << endl;
+ // NOTE: results from FR3 state & pinocchio are almost the same.
+ // cout << "Measured Pin EE pos [m]:\t" << ee_pos_world_mes_.transpose() << endl;
+ // cout << "Measured FR3 EE pos [m]:\t" << position.transpose() << endl;
+ // cout << "Measured Pin EE quat:\t" << ee_quat_world_mes_ << endl;
+ // cout << "Measured FR3 EE quat:\t" << orientation << endl;
+}
+
+void CartesianImpedanceExampleController::print_dynamics()
+{
+ // NOTE: dynamics are a little bit different
+ // cout << "FR3:\n" << M_fr << endl;
+ // cout << "Pin:\n" << M << endl;
+ // cout << "Coriolis FR3:\t" << coriolis.transpose() << endl;
+ // cout << "Coriolis Pin:\t" << tau_c.transpose() << endl;
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController,
+ controller_interface::ControllerBase)
diff --git a/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp b/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp
deleted file mode 100644
index e7798abde6b77b30ad58cf259a7af0f5293b60d6..0000000000000000000000000000000000000000
--- a/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp
+++ /dev/null
@@ -1,363 +0,0 @@
-// 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)