.clang-format

----
-Language: Cpp
-BasedOnStyle: google
-AccessModifierOffset: -2 # public, private, protected
-AlignAfterOpenBracket: Align # align style of arguments in function
-AlignConsecutiveMacros: true # align consecutive macros
-# AlignArrayOfStructures: Left # align array's elements
-AlignConsecutiveAssignments: false  # aligning consecutive assignments
-AlignConsecutiveDeclarations: false # aligning consecutive declarations
-AlignEscapedNewlines: Left # new line '\' alignment 
-AlignOperands: true # align operator after new line
-AlignTrailingComments: true
-AllowAllArgumentsOnNextLine: true
-AllowAllConstructorInitializersOnNextLine: true
-AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: Never
-AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: None
-AllowShortLambdasOnASingleLine: None
-AllowShortIfStatementsOnASingleLine: Never
-AllowShortLoopsOnASingleLine: false
-AlwaysBreakAfterDefinitionReturnType: None
-AlwaysBreakAfterReturnType: None
-AlwaysBreakBeforeMultilineStrings: true
-AlwaysBreakTemplateDeclarations: Yes
-BinPackArguments: true
-BinPackParameters: true
-BraceWrapping:
-  AfterCaseLabel: false
-  AfterClass: true
-  AfterControlStatement: true
-  AfterEnum: true
-  AfterFunction: true
-  AfterNamespace: true
-  AfterObjCDeclaration: true
-  AfterStruct: true
-  AfterUnion: true
-  AfterExternBlock: true
-  BeforeCatch: true
-  BeforeElse: true
-  # BeforeLambdaBody: true
-  IndentBraces: false
-  SplitEmptyFunction: true
-  SplitEmptyRecord: true
-  SplitEmptyNamespace: true
-BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Custom
-BreakBeforeInheritanceComma: false
-BreakInheritanceList: AfterColon
-BreakBeforeTernaryOperators: true
-BreakConstructorInitializersBeforeComma: false
-BreakConstructorInitializers: BeforeColon
-BreakAfterJavaFieldAnnotations: false
-BreakStringLiterals: true
-ColumnLimit: 100
-CommentPragmas: "^ IWYU pragma:"
-CompactNamespaces: false
-ConstructorInitializerAllOnOneLineOrOnePerLine: true
-ConstructorInitializerIndentWidth: 2
-ContinuationIndentWidth: 2
-Cpp11BracedListStyle: false
-DeriveLineEnding: true
-DerivePointerAlignment: false
-DisableFormat: false
-ExperimentalAutoDetectBinPacking: false
-FixNamespaceComments: true
-ForEachMacros:
-  - foreach
-  - Q_FOREACH
-  - BOOST_FOREACH
-IncludeBlocks: Preserve
-IncludeCategories:
-  - Regex: '^<ext/.*\.h>'
-    Priority: 2
-    SortPriority: 0
-  - Regex: '^<.*\.h>'
-    Priority: 1
-    SortPriority: 0
-  - Regex: "^<.*"
-    Priority: 2
-    SortPriority: 0
-  - Regex: ".*"
-    Priority: 3
-    SortPriority: 0
-IncludeIsMainRegex: "([-_](test|unittest))?$"
-IncludeIsMainSourceRegex: ""
-IndentCaseLabels: false
-IndentGotoLabels: false
-IndentPPDirectives: None
-IndentWidth: 2
-IndentWrappedFunctionNames: false
-JavaScriptQuotes: Leave
-JavaScriptWrapImports: true
-KeepEmptyLinesAtTheStartOfBlocks: false
-MacroBlockBegin: ""
-MacroBlockEnd: ""
-MaxEmptyLinesToKeep: 1
-NamespaceIndentation: None
-ObjCBinPackProtocolList: Never
-ObjCBlockIndentWidth: 2
-ObjCSpaceAfterProperty: false
-ObjCSpaceBeforeProtocolList: true
-PenaltyBreakAssignment: 2
-PenaltyBreakBeforeFirstCallParameter: 1
-PenaltyBreakComment: 300
-PenaltyBreakFirstLessLess: 120
-PenaltyBreakString: 1000
-PenaltyBreakTemplateDeclaration: 10
-PenaltyExcessCharacter: 1000000
-PenaltyReturnTypeOnItsOwnLine: 200
-PointerAlignment: Middle
-RawStringFormats:
-  - Language: Cpp
-    Delimiters:
-      - cc
-      - CC
-      - cpp
-      - Cpp
-      - CPP
-      - "c++"
-      - "C++"
-    CanonicalDelimiter: ""
-    BasedOnStyle: google
-  - Language: TextProto
-    Delimiters:
-      - pb
-      - PB
-      - proto
-      - PROTO
-    EnclosingFunctions:
-      - EqualsProto
-      - EquivToProto
-      - PARSE_PARTIAL_TEXT_PROTO
-      - PARSE_TEST_PROTO
-      - PARSE_TEXT_PROTO
-      - ParseTextOrDie
-      - ParseTextProtoOrDie
-    CanonicalDelimiter: ""
-    BasedOnStyle: google
-ReflowComments: true
-SortIncludes: true
-SortUsingDeclarations: true
-SpaceAfterCStyleCast: false
-SpaceAfterLogicalNot: false
-SpaceAfterTemplateKeyword: true
-SpaceBeforeAssignmentOperators: true
-SpaceBeforeCpp11BracedList: false
-SpaceBeforeCtorInitializerColon: true
-SpaceBeforeInheritanceColon: true
-SpaceBeforeParens: ControlStatements
-SpaceBeforeRangeBasedForLoopColon: true
-SpaceInEmptyBlock: false
-SpaceInEmptyParentheses: false
-SpacesBeforeTrailingComments: 2
-SpacesInAngles: false
-SpacesInConditionalStatement: false
-SpacesInContainerLiterals: true
-SpacesInCStyleCastParentheses: false
-SpacesInParentheses: false
-SpacesInSquareBrackets: false
-SpaceBeforeSquareBrackets: false
-Standard: Auto
-StatementMacros:
-  - Q_UNUSED
-  - QT_REQUIRE_VERSION
-TabWidth: 8
-UseCRLF: false
-UseTab: Never
----
-

.gitignore

-build/
 .vscode/
-assets/data/*.csv
-assets/data/rosbag/
\ No newline at end of file
+build/
+# assets/data/*.csv
+assets/data/matlab/*.asv
+# assets/data/*.bag

README.md

 # impedance_controller
+This is a repo has several ROS1 sub-packages that are for Franka Research 3 (FR3) robot control
+both in simulation (MuJoCo) and real hardware interface that is originated from main branch.
+Other branches, e.g. 5-observer-implementation-real, 6-simulator-implementation, will be collected
+in this branch as a separate ROS1 package. After the collection is done, this branch will be merged
+to the main branch s.t. impedance_controller repo to be a meta-package that includes controller,
+simulator, and other features.
+
 ## Repository Tree Description
 ```txt
 .impedance_controller
@@ -29,12 +36,8 @@
         |- src
             |- sinusoidal_profile.cpp
     |- robot
-        |- include
-            |- franka_states_struct.hpp
-            |- robotics_func.hpp
-            |- symbolic_autodiff.hpp
-        |- src
-            |- symbolic_autodiff.cpp
+        |- dynamics_func.hpp
+        |- kinematics_func.hpp
     |- utilities
         |- include
             |- cppTypes.hpp

assets/data/matlab/plot_dob_data

+%% load data
+% clc; close all; clear;
+filename_dob = '../data_sim_free_dob.csv';
+data_table_dob = readtable(filename_dob);
+data_array_dob = table2array(data_table_dob);
+
+vec_size_dob = [1, 7, 7, 7, 3, 3, 7, 6, 6, 6, 7, 7, 7, 7, 3, 3, 3, 3, 6, 6, 6, 6, 6, 6, 7, 7, 7, 7, 6, 6];
+start_idx_dob = [1, ];
+
+for i = 1:length(vec_size_dob) - 1
+    start_idx_dob(i+1) = start_idx_dob(i) + vec_size_dob(i);
+end
+
+data_cell = cell(1, length(vec_size_dob));
+
+for i = 1:length(vec_size_dob)
+    data_cell{i} = data_array_dob(:, start_idx_dob(i):start_idx_dob(i) + vec_size_dob(i) - 1);
+end
+
+t_dob = data_cell{1};
+ee_pos_mes_dob = data_cell{16};
+ee_rpy_mes_dob = data_cell{18};
+ee_vel_mes_dob = data_cell{20};
+ddq_hat_mob_filtered_dob = data_cell{28};
+
+%% Joint acceleration
+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_dob, ddq_eul(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'k');
+    % plt1 = plot(t, ddq_hat_sosml(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 2 * lw, 'Color', 'b');
+    % plt1.Color(4) = 0.7;
+    % plt2 = plot(t, ddq_mj(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
+    % plt2.Color(4) = 0.7;
+    % plot(t, ddq_hat_mob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
+    plot(t_dob, ddq_hat_mob_filtered_dob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
+    xlim([t_start, t_end]);
+    % ylim([-1 1]);
+
+    ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
+    
+    % if i == 7
+    %     ylim([-1 1]);
+    % end
+
+    if i == 1
+        legend('Time-derivative + LPF of $\dot{q}$', 'Estimation using Momentum Observer + LPF', ...
+            '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);
+
+%% Pose Tracking Performance
+figure();
+fig_pose = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
+ytitles = {'$p_{ee} \ [m]$', '$\psi_{ee} \ [rad]$'};
+plt_indices = 1:3;
+
+for i = 1:length(plt_indices)
+    nexttile;
+    hold on;
+    grid;
+    plot(t_dob, ee_pos_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
+    plot(t_dob, ee_pos_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
+    % plot(t, fr_ee_pos_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
+    xlim([t_start, t_end]);
+    
+    if i == 1
+        ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
+    end
+
+    if i == 3
+        legend('Desired', 'Actual', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
+    end
+end
+
+for i = 1:length(plt_indices)
+    nexttile;
+    hold on;
+    grid;
+    plot(t_dob, ee_rpy_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
+    plot(t_dob, ee_rpy_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
+    % plot(t, fr_ee_rpy_mes_dob(:, 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_dob, ee_vel_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
+    plot(t_dob, ee_vel_mes_dob(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 1.5 * lw, 'Color', 'r');
+    % plot(t, ee_vel_mj(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
+    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 == 3
+        legend('Desired', 'Actual', 'MuJoCo Data' , '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);

assets/data/matlab/trajectory_test.m

+clc; clear; close all;
+
+T = 2; % period
+f = 1/T;
+w = 2*pi*f;
+
+t = 0.0:0.001:40; % 시간 벡터
+t_pause = 2.0;
+t_start = [T, 3*T + t_pause, 5*T + 2* t_pause, 7*T + 3*t_pause, 9*T + 4*t_pause, 11*T + 5*t_pause]; % 각 신호의 시작 시간
+
+% twist vector
+y = cell(6, 1);
+
+for i = 1:6
+    y{i} = zeros(size(t)); % 초기화 (모든 값을 0으로 설정)
+    active_indices = (t >= t_start(i)) & (t <= t_start(i) + 2*T); % 시작 시간 ~ 2주기 인덱스 선택
+    y{i}(active_indices) = sin(w * (t(active_indices) - t_start(i))); % 두 주기만 sin 값 계산
+end
+
+% 그래프 그리기
+figure();
+for i = 1:6
+    subplot(6, 1, i);
+    plot(t, y{i});
+    grid on;
+    title(['y(', num2str(i), ')'], 'Interpreter', 'none');
+end

assets/model/fr3.urdf → assets/model/fr3_hand.urdf

@@ -289,354 +289,75 @@
     <parent link="fr3_link7"/>
     <child link="fr3_link8"/>
   </joint>
-  <!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
-  <link name="world"/>
-  <joint name="world_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="world"/>
-    <child link="fr3_link0"/>
+  <joint name="fr3_hand_joint" type="fixed">
+    <parent link="fr3_link8"/>
+    <child link="fr3_hand"/>
+    <origin rpy="0 0 -0.7853981633974483" xyz="0 0 0"/>
+  </joint>
+  <!-- sub-link defining detailed meshes for collision with environment -->
+  <link name="fr3_hand">
+    <visual>
+      <geometry>
+        <mesh filename="package://franka_description/meshes/visual/hand.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://franka_description/meshes/collision/hand.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
+  <link name="fr3_hand_sc">
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0.04"/>
+      <geometry>
+        <cylinder length="0.1" radius="0.07"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0 0.05 0.04"/>
+      <geometry>
+        <sphere radius="0.07"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0 -0.05 0.04"/>
+      <geometry>
+        <sphere radius="0.07"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0.1"/>
+      <geometry>
+        <cylinder length="0.1" radius="0.05"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0 0.05 0.1"/>
+      <geometry>
+        <sphere radius="0.05"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0 -0.05 0.1"/>
+      <geometry>
+        <sphere radius="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- fixed joint between both sub-links -->
+  <joint name="fr3_hand_sc_joint" type="fixed">
+    <origin rpy="0 0 0"/>
+    <parent link="fr3_hand"/>
+    <child link="fr3_hand_sc"/>
+  </joint>
+  <!-- Define the hand_tcp frame -->
+  <link name="fr3_hand_tcp"/>
+  <joint name="fr3_hand_tcp_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.1034"/>
+    <parent link="fr3_hand"/>
+    <child link="fr3_hand_tcp"/>
   </joint>
-  <gazebo reference="fr3_joint1">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint1_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint1">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint1_motor">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint2">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint2_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint2">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint2_motor">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint3">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint3_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint3">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint3_motor">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint4">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint4_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint4">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint4_motor">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint5">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint5_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint5">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint5_motor">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint6">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint6_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint6">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint6_motor">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint7">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint7_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint7">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint7_motor">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint1">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint1_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint1">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint1_motor">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint2">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint2_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint2">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint2_motor">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint3">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint3_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint3">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint3_motor">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint4">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint4_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint4">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint4_motor">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint5">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint5_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint5">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint5_motor">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint6">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint6_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint6">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint6_motor">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint7">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint7_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint7">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint7_motor">
-      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint1">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint1_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint1">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint1_motor">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint2">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint2_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint2">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint2_motor">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint3">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint3_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint3">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint3_motor">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint4">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint4_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint4">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint4_motor">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint5">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint5_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint5">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint5_motor">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint6">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint6_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint6">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint6_motor">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo reference="fr3_joint7">
-    <!-- Needed for ODE to output external wrenches on joints -->
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <transmission name="fr3_joint7_transmission">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="fr3_joint7">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint7_motor">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <transmission name="fr3_franka_state">
-    <type>franka_hw/FrankaStateInterface</type>
-    <joint name="fr3_joint1">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </joint>
-    <joint name="fr3_joint2">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </joint>
-    <joint name="fr3_joint3">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </joint>
-    <joint name="fr3_joint4">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </joint>
-    <joint name="fr3_joint5">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </joint>
-    <joint name="fr3_joint6">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </joint>
-    <joint name="fr3_joint7">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_motor1">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </actuator>
-    <actuator name="fr3_motor2">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </actuator>
-    <actuator name="fr3_motor3">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </actuator>
-    <actuator name="fr3_motor4">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </actuator>
-    <actuator name="fr3_motor5">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </actuator>
-    <actuator name="fr3_motor6">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </actuator>
-    <actuator name="fr3_motor7">
-      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <transmission name="fr3_franka_model">
-    <type>franka_hw/FrankaModelInterface</type>
-    <joint name="fr3_joint1">
-      <role>root</role>
-      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
-    </joint>
-    <joint name="fr3_joint8">
-      <role>tip</role>
-      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
-    </joint>
-    <actuator name="fr3_joint1_motor_root">
-      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
-    </actuator>
-    <actuator name="fr3_joint8_motor_tip">
-      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
-    </actuator>
-  </transmission>
-  <gazebo>
-    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
-      <controlPeriod>0.001</controlPeriod>
-      <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
-    </plugin>
-    <self_collide>true</self_collide>
-  </gazebo>
 </robot>