Created my_panda_robot folder containing ROS pkgs

SteMuc · Apr 22, 2024 · 463c2bd · 463c2bd
commit 463c2bd
Show file tree

Hide file tree

Showing 17 changed files with 1,290 additions and 0 deletions.
diff --git a/my_panda_description/CMakeLists.txt b/my_panda_description/CMakeLists.txt
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.8)
+project(my_panda_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(xacro REQUIRED)
+find_package(urdf REQUIRED)
+find_package(franka_description REQUIRED)
+
+install(DIRECTORY launch robots meshes rviz
+  DESTINATION share/${PROJECT_NAME}
+)
+ament_package()
diff --git a/my_panda_description/launch/description_launch.launch.py b/my_panda_description/launch/description_launch.launch.py
@@ -0,0 +1,32 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    load_gripper_parameter_name = 'load_gripper'
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+
+    franka_xacro_file = os.path.join(get_package_share_directory('my_panda_description'), 'robots',
+                                     'panda_arm.urdf.xacro')
+    robot_description = Command(
+        [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper])
+
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as end-effector if true. Robot is loaded without '
+                        'end-effector otherwise'),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{'robot_description': robot_description}],
+        ),
+    ])
diff --git a/my_panda_description/meshes/table.mtl b/my_panda_description/meshes/table.mtl
@@ -0,0 +1,16 @@
+newmtl table
+  Ns 10.0000
+  Ni 1.5000
+  d 1.0000
+  Tr 0.0000
+  Tf 1.0000 1.0000 1.0000 
+  illum 2
+  Ka 0.0000 0.0000 0.0000
+  Kd 0.5880 0.5880 0.5880
+  Ks 0.0000 0.0000 0.0000
+  Ke 0.0000 0.0000 0.0000
+  map_Ka table.tga
+  map_Kd table.png
+
+
+
diff --git a/my_panda_description/meshes/table.obj b/my_panda_description/meshes/table.obj
@@ -0,0 +1,48 @@
+# table.obj
+#
+
+o table
+#mtllib table.mtl
+
+v -0.500000 -0.500000 0.500000
+v 0.500000 -0.500000 0.500000
+v -0.500000 0.500000 0.500000
+v 0.500000 0.500000 0.500000
+v -0.500000 0.500000 -0.500000
+v 0.500000 0.500000 -0.500000
+v -0.500000 -0.500000 -0.500000
+v 0.500000 -0.500000 -0.500000
+
+vt 0.000000 0.000000
+vt 1.000000 0.000000
+vt 0.000000 1.000000
+vt 1.000000 1.000000
+
+vn 0.000000 0.000000 1.000000
+vn 0.000000 1.000000 0.000000
+vn 0.000000 0.000000 -1.000000
+vn 0.000000 -1.000000 0.000000
+vn 1.000000 0.000000 0.000000
+vn -1.000000 0.000000 0.000000
+
+g table
+#usemtl table
+s 1
+f 1/1/1 2/2/1 3/3/1
+f 3/3/1 2/2/1 4/4/1
+s 2
+f 3/1/2 4/2/2 5/3/2
+f 5/3/2 4/2/2 6/4/2
+s 3
+f 5/4/3 6/3/3 7/2/3
+f 7/2/3 6/3/3 8/1/3
+s 4
+f 7/1/4 8/2/4 1/3/4
+f 1/3/4 8/2/4 2/4/4
+s 5
+f 2/1/5 8/2/5 4/3/5
+f 4/3/5 8/2/5 6/4/5
+s 6
+f 7/1/6 1/2/6 5/3/6
+f 5/3/6 1/2/6 3/4/6
+
diff --git a/my_panda_description/meshes/table.png b/my_panda_description/meshes/table.png
diff --git a/my_panda_description/meshes/table.stl b/my_panda_description/meshes/table.stl
diff --git a/my_panda_description/package.xml b/my_panda_description/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>my_panda_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="stefanoangeli12@gmail.com">user</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>xacro</depend>
+  <depend>urdf</depend>
+  <depend>franka_description</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/my_panda_description/robots/hand.urdf.xacro b/my_panda_description/robots/hand.urdf.xacro
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
+  <xacro:include filename="hand.xacro"/>
+  <xacro:hand ns="panda" safety_distance="0.03"/>
+</robot>
diff --git a/my_panda_description/robots/hand.xacro b/my_panda_description/robots/hand.xacro
@@ -0,0 +1,93 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
+  <!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
+  <xacro:macro name="hand" params="connected_to:='' ns:='' rpy:='0 0 0' xyz:='0 0 0' safety_distance:=0">
+    <xacro:unless value="${connected_to == ''}">
+      <joint name="${ns}_hand_joint" type="fixed">
+        <parent link="${connected_to}"/>
+        <child link="${ns}_hand"/>
+        <origin xyz="${xyz}" rpy="${rpy}"/>
+      </joint>
+    </xacro:unless>
+    <link name="${ns}_hand">
+      <visual>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/visual/hand.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}"/>
+        <geometry>
+          <cylinder radius="${0.04+safety_distance}" length="0.1"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 -0.05 0.04" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.04+safety_distance}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0.05 0.04" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.04+safety_distance}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 0.1" rpy="0 ${pi/2} ${pi/2}"/>
+        <geometry>
+          <cylinder radius="${0.02+safety_distance}" length="0.1"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 -0.05 0.1" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.02+safety_distance}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0.05 0.1" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.02+safety_distance}"/>
+        </geometry>
+      </collision>
+    </link>
+    <!-- Define the hand_tcp frame -->
+    <link name="${ns}_hand_tcp"/>
+    <joint name="${ns}_hand_tcp_joint" type="fixed">
+      <origin xyz="0 0 0.1034" rpy="0 0 0"/>
+      <parent link="${ns}_hand"/>
+      <child link="${ns}_hand_tcp"/>
+    </joint>
+    <link name="${ns}_leftfinger">
+      <visual>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
+        </geometry>
+      </visual>
+    </link>
+    <link name="${ns}_rightfinger">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
+        </geometry>
+      </visual>
+    </link>
+    <joint name="${ns}_finger_joint1" type="prismatic">
+      <parent link="${ns}_hand"/>
+      <child link="${ns}_leftfinger"/>
+      <origin xyz="0 0 0.0584" rpy="0 0 0"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+    </joint>
+    <joint name="${ns}_finger_joint2" type="prismatic">
+      <parent link="${ns}_hand"/>
+      <child link="${ns}_rightfinger"/>
+      <origin xyz="0 0 0.0584" rpy="0 0 0"/>
+      <axis xyz="0 -1 0"/>
+      <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+      <mimic joint="${ns}_finger_joint1"/>
+    </joint>
+  </xacro:macro>
+</robot>
diff --git a/my_panda_description/robots/panda_arm.ros2_control.xacro b/my_panda_description/robots/panda_arm.ros2_control.xacro
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="panda_arm_ros2_control" params="ns robot_ip use_fake_hardware:=^|false fake_sensor_commands:=^|false">
+    <ros2_control name="FrankaHardwareInterface" type="system">
+      <hardware>
+        <xacro:if value="${use_fake_hardware}">
+          <plugin>fake_components/GenericSystem</plugin>
+          <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+          <param name="state_following_offset">0.0</param>
+        </xacro:if>
+        <xacro:unless value="${use_fake_hardware}">
+          <plugin>franka_hardware/FrankaHardwareInterface</plugin>
+          <param name="robot_ip">${robot_ip}</param>
+        </xacro:unless>
+      </hardware>
+
+      <xacro:macro name="configure_joint" params="joint_name initial_position">
+        <joint name="${joint_name}">
+          <param name="initial_position">${initial_position}</param>
+          <command_interface name="effort"/>
+          <command_interface name="velocity"/>
+          <command_interface name="position"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+          <state_interface name="effort"/>
+        </joint>
+      </xacro:macro>
+
+      <xacro:configure_joint joint_name="${ns}_joint1" initial_position="0.0"/>
+      <xacro:configure_joint joint_name="${ns}_joint2" initial_position="${-pi/4}"/>
+      <xacro:configure_joint joint_name="${ns}_joint3" initial_position="0.0"/>
+      <xacro:configure_joint joint_name="${ns}_joint4" initial_position="${-3*pi/4}"/>
+      <xacro:configure_joint joint_name="${ns}_joint5" initial_position="0.0"/>
+      <xacro:configure_joint joint_name="${ns}_joint6" initial_position="${pi/2}"/>
+      <xacro:configure_joint joint_name="${ns}_joint7" initial_position="${pi/4}"/>
+
+    </ros2_control>
+  </xacro:macro>
+</robot>
diff --git a/my_panda_description/robots/panda_arm.urdf.xacro b/my_panda_description/robots/panda_arm.urdf.xacro
@@ -0,0 +1,40 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:arg name="arm_id" default="panda" /> <!-- Name of this panda -->
+  <xacro:arg name="hand" default="false" /> <!-- Should a franka_gripper be mounted at the flange?" -->
+  <xacro:arg name="robot_ip" default="" /> <!-- IP address or hostname of the robot" -->
+  <xacro:arg name="use_fake_hardware" default="false" />
+  <xacro:arg name="fake_sensor_commands" default="false" />
+
+  <!-- box shaped table as base -->
+  <link name="base">
+    <visual>
+      <origin xyz="0 0 0.5" rpy="0 0 0" />
+      <geometry>
+        <box size="1 2 1" />
+      </geometry>
+      <material name="Red">
+        <color rgba="0.8980392156862745 0.0 0.0 1.0" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.5" rpy="0 0 0" />
+      <geometry>
+        <box size="1 2 1" />
+      </geometry>
+    </collision>
+  </link>
+
+  <xacro:include filename="$(find my_panda_description)/robots/panda_arm.xacro" />
+  <xacro:panda_arm arm_id="$(arg arm_id)" connected_to="base" xyz="0 0 1" safety_distance="0.03" />
+
+  <xacro:if value="$(arg hand)">
+    <xacro:include filename="$(find my_panda_description)/robots/hand.xacro" />
+    <xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8"
+      safety_distance="0.03" />
+  </xacro:if>
+
+  <xacro:include filename="$(find my_panda_description)/robots/panda_arm.ros2_control.xacro" />
+  <xacro:panda_arm_ros2_control ns="$(arg arm_id)" robot_ip="$(arg robot_ip)"
+    use_fake_hardware="$(arg use_fake_hardware)" fake_sensor_commands="$(arg fake_sensor_commands)" />
+</robot>