Added packages for simulation(ignition-gazebo).

SteMuc · Apr 27, 2024 · 429e39e · 429e39e
1 parent 87aa42b
commit 429e39e
Show file tree

Hide file tree

Showing 15 changed files with 282 additions and 19 deletions.
diff --git a/my_panda_bringup/CMakeLists.txt b/my_panda_bringup/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(my_panda_bringup)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/my_panda_bringup/launch/bringup.launch.py b/my_panda_bringup/launch/bringup.launch.py
diff --git a/my_panda_bringup/package.xml b/my_panda_bringup/package.xml
@@ -0,0 +1,24 @@
+<?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_bringup</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>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <!-- <exec_depend>my_panda_moveit</exec_depend> -->
+  <exec_depend>my_panda_controller</exec_depend>
+
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/my_panda_controller/CMakeLists.txt b/my_panda_controller/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.8)
+project(my_panda_controller)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/my_panda_controller/config/panda_ros_controllers.yaml b/my_panda_controller/config/panda_ros_controllers.yaml
@@ -0,0 +1,63 @@
+simulation_ignition_ros_control:
+  ros__parameters:
+    use_sim_time: true
+
+# Separate controller manager used for simulation - only difference is 
+# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control)  
+simulation_controller_manager:
+  ros__parameters:
+    use_sim_time: true
+    update_rate: 1000 # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    panda_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+controller_manager:
+  ros__parameters:
+    use_sim_time: false
+    update_rate: 1000 # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    panda_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+panda_arm_controller:
+  ros__parameters:
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
+    gains:
+      panda_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. }
+      panda_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. }
+      panda_joint3: { p: 600., d: 30., i: 0., i_clamp: 1. }
+      panda_joint4: { p: 600., d: 30., i: 0., i_clamp: 1. }
+      panda_joint5: { p: 250., d: 10., i: 0., i_clamp: 1. }
+      panda_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
+      panda_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+      - velocity
+
+    state_publish_rate: 50.0 # Defaults to 50
+    action_monitor_rate: 20.0 # Defaults to 20
+
+    allow_partial_joints_goal: false # Defaults to false
+    open_loop_control: true
+    allow_integration_in_goal_trajectories: true
+    constraints:
+      stopped_velocity_tolerance: 0.0 # Defaults to 0.01
+      goal_time: 0.0 # Defaults to 0.0 (start immediately)
+
+
diff --git a/my_panda_controller/package.xml b/my_panda_controller/package.xml
@@ -0,0 +1,31 @@
+<?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_controller</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>
+
+  <exec_depend>my_panda_description</exec_depend>
+
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>position_controllers</exec_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/launch/description_launch.launch.py b/my_panda_description/launch/description_launch.launch.py
@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
 import os
 
 from ament_index_python.packages import get_package_share_directory

diff --git a/my_panda_description/package.xml b/my_panda_description/package.xml
@@ -8,6 +8,9 @@
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
 
   <depend>xacro</depend>
   <depend>urdf</depend>

diff --git a/my_panda_description/robots/panda_arm.urdf.xacro b/my_panda_description/robots/panda_arm.urdf.xacro
@@ -5,6 +5,9 @@
   <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" />
+  <xacro:arg name="use_sim" default="false" />
+  <xacro:arg name="simulation_controllers_config_file"
+    default="$(find my_panda_controller)/config/joint_controller_manipulation.yaml" />
 
   <!-- box shaped table as base -->
   <link name="base">
@@ -34,7 +37,12 @@
       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)"
+  <xacro:include
+    filename="$(find my_panda_description)/robots/ros2_control/panda_arm.ros2_control.xacro" />
+  <xacro:panda_arm_ros2_control 
+    ns="$(arg arm_id)" 
+    use_sim="$(arg use_sim)"
+    simulation_controllers_config_file="$(arg simulation_controllers_config_file)"
+    robot_ip="$(arg robot_ip)"
     use_fake_hardware="$(arg use_fake_hardware)" fake_sensor_commands="$(arg fake_sensor_commands)" />
 </robot>
diff --git a/...ption/robots/panda_arm.ros2_control.xacro → ...ros2_control/panda_arm.ros2_control.xacro b/...ption/robots/panda_arm.ros2_control.xacro → ...ros2_control/panda_arm.ros2_control.xacro
@@ -1,14 +1,17 @@
 <?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">
+  <xacro:macro name="panda_arm_ros2_control"
+    params="ns use_sim simulation_controllers_config_file 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>
@@ -18,23 +21,40 @@
       <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"/>
+
+          <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}"/>
+      <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}" />
 
+      <xacro:if value="$(arg use_sim)">
+        <hardware>
+          <plugin>ign_ros2_control/IgnitionSystem</plugin>
+        </hardware>
+      </xacro:if>
     </ros2_control>
+
+    <!-- Joint state publisher -->
+    <xacro:if value="$(arg use_sim)">
+      <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+        <parameters>${simulation_controllers_config_file}</parameters>
+      </plugin>
+    </xacro:if>
   </xacro:macro>
-</robot>
+</robot>
+
+<!-- https://github.com/livanov93/ign_ros2_control -->
diff --git a/my_panda_gazebo/CMakeLists.txt b/my_panda_gazebo/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.8)
+project(my_panda_gazebo)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/my_panda_gazebo/launch/simulation.launch.py b/my_panda_gazebo/launch/simulation.launch.py
@@ -0,0 +1,41 @@
+#!/usr/bin/env python3
+
+from launch import LaunchDescription
+from launch.actions import (
+    IncludeLaunchDescription,
+    DeclareLaunchArgument,
+)
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    PathJoinSubstitution,
+    PythonExpression,
+    LaunchConfiguration,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node, SetParameter
+from launch_ros.substitutions import FindPackageShare
+
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    get_package_share_directory("ros_gz_sim"),
+                    "launch",
+                    "gz_sim.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={'gz_args': '-r empty.sdf'}.items(),
+    )
+
+    return LaunchDescription(
+        [
+            gz_sim
+        ]
+    )
diff --git a/my_panda_gazebo/package.xml b/my_panda_gazebo/package.xml
@@ -0,0 +1,26 @@
+<?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_gazebo</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>
+  <exec_depend>my_panda_bringup</exec_depend>
+
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+
+  <exec_depend>ros_gz_sim</exec_depend>
+  <!-- Ignition dependency is specified in the ros_gz_sim package, 
+    version can chosen using GZ_VERSION (or IGNITION_VERSION) env variable, 
+    for details refer to the ros_gz_sim package -->
+  <exec_depend>ros_gz_bridge</exec_depend>
+  <exec_depend>ign_ros2_control</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/my_panda_viz/launch/view_model.launch.py b/my_panda_viz/launch/view_model.launch.py
@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
 import os
 import sys
 
@@ -14,7 +16,9 @@ def generate_launch_description():
     # Declare the launch argument for load_gripper_value
     load_gripper_value_launch_arg = DeclareLaunchArgument(
         'load_gripper_value',
-        default_value='true'
+        default_value='false',
+        description='Use Franka Gripper as end-effector if true. Robot is loaded without '
+                        'end-effector otherwise'
     )
 
     rviz_file = os.path.join(get_package_share_directory('my_panda_viz'), 'rviz',
@@ -25,7 +29,7 @@ def generate_launch_description():
         executable='joint_state_publisher_gui',
         name='joint_state_publisher_gui'
     )
-
+    
     node_rviz = Node(
         package='rviz2',
         executable='rviz2',
@@ -51,3 +55,5 @@ def generate_launch_description():
             launch_franka_description,
         ]
     )
+
+    ##https://answers.ros.org/question/322874/ros2-what-is-different-between-declarelaunchargument-and-launchconfiguration/
diff --git a/my_panda_viz/package.xml b/my_panda_viz/package.xml
@@ -9,7 +9,11 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+
   <depend>my_panda_description</depend>
+
   <depend>joint_state_publisher</depend>
   <depend>joint_state_publisher_gui</depend>
   <depend>robot_state_publisher</depend>