Franka Arm fr3 support

rox_bringup/configs/franka/initial_positions.yaml

@@ -0,0 +1,7 @@
+fr3_joint1: 1.5
+fr3_joint2: -1.7
+fr3_joint3: 2.5
+fr3_joint4: -1.0
+fr3_joint5: 1.5
+fr3_joint6: 0.0
+fr3_joint7: 0.0

rox_bringup/configs/franka/simulation_controllers.yaml

@@ -0,0 +1,39 @@
+/**: 
+  controller_manager:
+    ros__parameters:
+      update_rate: 1000  # Hz
+      thread_priority: 98
+      enable_overrun: false
+
+      fr3_arm_controller:
+        type: joint_trajectory_controller/JointTrajectoryController
+
+      joint_state_broadcaster:
+        type: joint_state_broadcaster/JointStateBroadcaster
+
+
+
+/**: 
+  fr3_arm_controller:
+    ros__parameters:
+      command_interfaces:
+        - position
+      state_interfaces:
+        - position
+        - velocity
+      joints:
+        - fr3_joint1
+        - fr3_joint2
+        - fr3_joint3
+        - fr3_joint4
+        - fr3_joint5
+        - fr3_joint6
+        - fr3_joint7
+      gains:
+        fr3_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. }
+        fr3_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. }
+        fr3_joint3: { p: 600., d: 30., i: 0., i_clamp: 1. }
+        fr3_joint4: { p: 600., d: 30., i: 0., i_clamp: 1. }
+        fr3_joint5: { p: 250., d: 10., i: 0., i_clamp: 1. }
+        fr3_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
+        fr3_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }

rox_bringup/launch/bringup_sim_launch.py

@@ -84,13 +84,18 @@ def execution_stage(context: LaunchContext,
             initial_joint_controller_name = 'arm_controller'
         elif arm_typ in ['ur5', 'ur10', 'ur5e', 'ur10e']:
             arm_manufacturer = 'ur'
-
+        elif arm_typ == 'fr3':
+        # new block for Franka FR3
+            arm_manufacturer = 'franka'
+            initial_joint_controller_name = 'fr3_arm_controller'
+
         controllers_yaml = os.path.join(
             get_package_share_directory('rox_bringup'),
             'configs', 
             arm_manufacturer, 
             'simulation_controllers.yaml'
         )
+        print(f"Using controller config: {controllers_yaml}")
 
         # Generates a final YAML parameter file from the controllers template (with substitutions applied),
         # and returns file_path, shutdown_handler
@@ -214,6 +219,9 @@ def execution_stage(context: LaunchContext,
             env_var_value += ':' + os.path.dirname(get_package_share_directory('elite_description'))
         elif arm_typ == 'ur5' or arm_typ == 'ur10' or arm_typ == 'ur5e' or arm_typ == 'ur10e':
             env_var_value += ':' + os.path.dirname(get_package_share_directory('ur_description'))
+        elif arm_typ == 'fr3':
+            env_var_value += ':' + os.path.dirname(get_package_share_directory('franka_description'))
+
         # Set environment variable for gripper description packages
         # if gripper_typ == 'epick':
         #     env_var_value += ':' + os.path.dirname(get_package_share_directory('epick_description'))
@@ -265,7 +273,7 @@ def generate_launch_description():
 
     declare_arm_type_cmd = DeclareLaunchArgument(
             'arm_type', default_value='',
-            choices=['', 'ur5', 'ur10', 'ur5e', 'ur10e', 'ec66', 'cs66'],
+            choices=['', 'ur5', 'ur10', 'ur5e', 'ur10e', 'ec66', 'cs66','fr3'],
             description='Arm Types\n\t'
         )
 

rox_description/launch/description_launch.py

@@ -129,7 +129,7 @@ def generate_launch_description():
 
     declare_arm_type_cmd = DeclareLaunchArgument(
             'arm_type', default_value='',
-            choices=['', 'ur5', 'ur10', 'ur5e', 'ur10e', 'ec66', 'cs66'],
+            choices=['', 'ur5', 'ur10', 'ur5e', 'ur10e', 'ec66', 'cs66','fr3'],
             description='Arm Types\n\t'        
         )
 

rox_description/package.xml

@@ -19,6 +19,14 @@
   <exec_depend>ur_robot_driver</exec_depend>
   <exec_depend>ur_simulation_gz</exec_depend>
 
+  <!-- Dependencies for Franka Panda arm (ROS 2 Jazzy) -->
+  <exec_depend>franka_description</exec_depend>
+  <exec_depend>franka_msgs</exec_depend>
+  <exec_depend>franka_gripper</exec_depend>
+  <exec_depend>franka_hw</exec_depend>
+  <exec_depend>franka_gazebo</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
+
   <!-- Dependencies for Realsense D435 -->
   <exec_depend>realsense2_description</exec_depend>
 

rox_description/urdf/rox.urdf.xacro

@@ -171,6 +171,23 @@ Contributor: Adarsh Karan K P
       <xacro:include filename="$(find rox_description)/urdf/xacros/elite_arm.urdf.xacro"/>
     </xacro:if>
 
+    <!-- Franka Robotics Arms-->
+    <xacro:if value="${arm == 'fr3'}">
+
+      <!-- Different height for AC arm-->
+      <xacro:if value="${ur_dc == False}">
+        <xacro:property name="arm_height" value="0.376" />
+      </xacro:if>
+
+      <xacro:if value="${ur_dc == True}">
+        <xacro:property name="arm_height" value="0.276" />
+      </xacro:if>
+
+      <!-- Include the franka arm -->
+      <xacro:include filename="$(find rox_description)/urdf/xacros/franka_arm.urdf.xacro"/>
+    </xacro:if>
+
+
     <!-- add gripper-->
     <xacro:unless value="${gripper == ''}">
 

rox_description/urdf/xacros/franka_arm.urdf.xacro

@@ -0,0 +1,60 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="franka_arm">
+
+  <xacro:include filename="$(find franka_description)/robots/common/franka_robot.xacro"/>
+  <xacro:include filename="$(find franka_description)/robots/common/franka_arm.ros2_control.xacro"/>
+
+  <xacro:arg name="use_gz" default="false"/>
+  <xacro:arg name="include_arm_ros2_control" default="false"/>
+  <xacro:arg name="arm" default="fr3"/>
+  <xacro:arg name="use_mock_hardware" default="false"/>
+  <xacro:arg name="mock_sensor_commands" default="false"/>
+
+  <xacro:property name="joint_limits"
+        value="${xacro.load_yaml('$(find franka_description)/robots/fr3/joint_limits.yaml')}" />
+  <xacro:property name="kinematics"
+        value="${xacro.load_yaml('$(find franka_description)/robots/fr3/kinematics.yaml')}" />
+  <xacro:property name="inertials"
+        value="${xacro.load_yaml('$(find franka_description)/robots/fr3/inertials.yaml')}" />
+  <xacro:property name="dynamics"
+        value="${xacro.load_yaml('$(find franka_description)/robots/fr3/dynamics.yaml')}" />
+
+  <xacro:if value="${arm == 'fr3'}">
+    <xacro:property name="arm_height" value="0.376"/>
+    <xacro:arg name="arm_parent" default="world"/>
+    <xacro:arg name="initial_positions_file"
+               default="$(find rox_bringup)/configs/franka/initial_joint_positions.yaml"/>
+    <xacro:property name="ur_initial_positions"
+                    value="${xacro.load_yaml('$(arg initial_positions_file)')}"/>
+
+    <xacro:franka_robot
+      arm_id="fr3"
+      hand="false"
+      joint_limits="${joint_limits}"
+      kinematics="${kinematics}"
+      inertials="${inertials}"
+      dynamics="${dynamics}"
+      xyz="0.218 0 ${arm_height}"
+      parent="cabinet_link"
+      gazebo="$(arg use_gz)"
+      ros2_control="$(arg include_arm_ros2_control)"
+      description_pkg="franka_description"
+      gazebo_effort="false"
+    />
+
+    <xacro:unless value="$(arg use_gz)">
+      <xacro:if value="$(arg include_arm_ros2_control)">
+        <xacro:franka_arm_ros2_control
+          arm_id="fr3"
+          robot_ip=""
+          gazebo="false"           
+          hand="false"
+          arm_prefix=""
+          gazebo_effort="false"
+          use_fake_hardware="$(arg use_mock_hardware)"   
+          fake_sensor_commands="$(arg mock_sensor_commands)"/>
+      </xacro:if>
+    </xacro:unless>
+  </xacro:if>
+
+</robot>