Chloe fa kuka

ramsai.repos

@@ -3,3 +3,7 @@ repositories:
         type: git
         url: https://github.com/ICube-Robotics/iiwa_ros2.git
         version: main
+    kinematics_interface:
+        type: git
+        url: https://github.com/ros-controls/kinematics_interface.git
+        version: master

ramsai_bringup/launch/ramsai.launch.py

@@ -42,7 +42,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             'use_planning',
-            default_value='false',
+            default_value='true',
             description='Start robot with Moveit2 `move_group` planning \
                          config for Pilz and OMPL.',
         )
@@ -169,9 +169,9 @@ def generate_launch_description():
 
     robot_controllers = PathJoinSubstitution(
         [
-            FindPackageShare('iiwa_description'),
+            FindPackageShare('ramsai_description'),
             'config',
-            'iiwa_controllers.yaml',
+            'ramsai_controllers.yaml',
         ]
     )
     rviz_config_file = PathJoinSubstitution(
@@ -241,6 +241,13 @@ def generate_launch_description():
         condition=UnlessCondition(use_sim),
     )
 
+    gpio_command_controller = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['gpio_command_controller', '--controller-manager',
+                   ['controller_manager']],
+    )
+
     robot_controller_spawner = Node(
         package='controller_manager',
         executable='spawner',
@@ -293,6 +300,7 @@ def generate_launch_description():
         delay_rviz_after_joint_state_broadcaster_spawner,
         external_torque_broadcaster_spawner,
         delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
+        gpio_command_controller
     ]
 
     return LaunchDescription(declared_arguments + nodes)

ramsai_bringup/launch/ramsai_planning.launch.py

@@ -16,8 +16,9 @@
 from launch.actions import DeclareLaunchArgument
 from launch.conditions import IfCondition
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 
 
 def generate_launch_description():
@@ -74,11 +75,11 @@ def generate_launch_description():
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
-                [FindPackageShare('iiwa_description'), 'srdf', 'iiwa.srdf.xacro']
+                [FindPackageShare('ramsai_description'), 'srdf', 'iiwa.srdf.xacro']
             ),
             " ",
             "name:=",
-            "iiwa",
+            "iiwa_print",
         ]
     )
 
@@ -155,8 +156,97 @@ def generate_launch_description():
         ],
     )
 
+    ##############################
+    # Hybrid Planning
+    ##############################
+
+    # Load params
+    common_hybrid_planning_param = PathJoinSubstitution([
+        FindPackageShare("ramsai_description"),
+        'moveit2',
+        'common_hybrid_planning_params.yaml',
+      ]
+    )
+
+    global_planner_param = PathJoinSubstitution([
+        FindPackageShare("ramsai_description"),
+        'moveit2',
+        'global_planner.yaml',
+      ]
+    )
+
+    local_planner_param = PathJoinSubstitution([
+        FindPackageShare("ramsai_description"),
+        'moveit2',
+        'local_planner.yaml',
+      ]
+    )
+
+    hybrid_planning_manager_param = PathJoinSubstitution([
+        FindPackageShare("ramsai_description"),
+        'moveit2',
+        'hybrid_planning_manager.yaml',
+      ]
+    )
+
+    servo_params = PathJoinSubstitution([
+        FindPackageShare("ramsai_description"),
+        'moveit2',
+        'servo_config.yaml',
+      ]
+    )
+
+    # Hybrid planner container
+    planning_container = ComposableNodeContainer(
+      name="hybrid_planning_container",
+      namespace="/",
+      package="rclcpp_components",
+      executable="component_container",
+      composable_node_descriptions=[
+        ComposableNode(
+          package="moveit_hybrid_planning",
+          plugin="moveit::hybrid_planning::GlobalPlannerComponent",
+          name="global_planner",
+          parameters=[
+            common_hybrid_planning_param,
+            global_planner_param,
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            robot_description_planning_cartesian_limits,
+            moveit_controllers,
+          ],
+        ),
+        ComposableNode(
+          package="moveit_hybrid_planning",
+          plugin="moveit::hybrid_planning::LocalPlannerComponent",
+          name="local_planner",
+          parameters=[
+            common_hybrid_planning_param,
+            local_planner_param,
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            servo_params,
+            moveit_controllers,
+          ],
+        ),
+        ComposableNode(
+          package="moveit_hybrid_planning",
+          plugin="moveit::hybrid_planning::HybridPlanningManager",
+          name="hybrid_planning_manager",
+          parameters=[
+            common_hybrid_planning_param,
+            hybrid_planning_manager_param,
+          ],
+        ),
+      ],
+      output="screen",
+    )
+
+
     rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare('iiwa_description'), 'rviz', 'iiwa.rviz']
+        [FindPackageShare('ramsai_description'), 'rviz', 'ramsai.rviz']
     )
 
     rviz_node = Node(
@@ -179,6 +269,7 @@ def generate_launch_description():
 
     nodes = [
         move_group_node,
+        planning_container,
         rviz_node,
     ]
 

ramsai_description/CMakeLists.txt

@@ -27,7 +27,11 @@ endif()
 # Install
 ################################################################################
 install(
-  DIRECTORY urdf meshes config
+<<<<<<< Updated upstream
+  DIRECTORY urdf meshes config moveit2 srdf rviz ros2_control
+=======
+  DIRECTORY urdf meshes config moveit2 srdf ros2_control
+>>>>>>> Stashed changes
   DESTINATION share/${PROJECT_NAME}
 )
 

ramsai_description/config/bhf_el7031_config.yaml

@@ -0,0 +1,47 @@
+# Configuration file for Beckhoff EL7031
+vendor_id: 0x00000002
+product_id: 0x1b773052
+sdo:  # sdo data to be transferred at drive startup
+  - {index: 0x8012, sub_index: 1, type: bool, value: 1}
+rpdo:  # RxPDO
+  - index: 0x1600
+    channels:
+      - {index: 0x0000, sub_index: 0x00, type: bool} # Gap
+      - {index: 0x7000, sub_index: 0x02, type: bool} # Enable latch extern on positive edge
+      - {index: 0x7000, sub_index: 0x03, type: bool} # Set counter
+      - {index: 0x7000, sub_index: 0x04, type: bool} # Enable latch extern on negative edge
+      - {index: 0x0000, sub_index: 0x00, type: bit4} # Gap
+      - {index: 0x0000, sub_index: 0x00, type: bit8} # Gap
+      - {index: 0x7000, sub_index: 0x11, type: int16} # Set counter value
+  - index: 0x1602
+    channels:
+      - {index: 0x7010, sub_index: 0x01, type: bool, mask: 1, command_interface: enable} # Enable
+      - {index: 0x7010, sub_index: 0x02, type: bool, mask: 2, command_interface: reset} # Reset
+      - {index: 0x7010, sub_index: 0x03, type: bool} # Reduce torque 
+      - {index: 0x0000, sub_index: 0x00, type: bit5} # Gap
+      - {index: 0x0000, sub_index: 0x00, type: bit8} # Gap
+  - index: 0x1604
+    channels:
+      - {index: 0x7010, sub_index: 0x21, type: int16, command_interface: velocity} # Velocity
+tpdo:  # TxPDO
+  - index: 0x1a03
+    channels:
+      - {index: 0x6010, sub_index: 0x01, type: bool, mask: 1, state_interface: ready_to_enable} # Ready to enable
+      - {index: 0x6010, sub_index: 0x02, type: bool, mask: 2, state_interface: ready} # Ready
+      - {index: 0x6010, sub_index: 0x03, type: bool, mask: 4, state_interface: warning} # Warning
+      - {index: 0x6010, sub_index: 0x04, type: bool, mask: 8, state_interface: error} # Error
+      - {index: 0x6010, sub_index: 0x05, type: bool} # Moving positive
+      - {index: 0x6010, sub_index: 0x06, type: bool} # Moving negative
+      - {index: 0x6010, sub_index: 0x07, type: bool} # Torque reduced
+      - {index: 0x0000, sub_index: 0x00, type: bool} # Gap
+      - {index: 0x0000, sub_index: 0x00, type: bit3} # Gap
+      - {index: 0x6010, sub_index: 0x0C, type: bool} # Digital input 1
+      - {index: 0x6010, sub_index: 0x0D, type: bool} # Digital input 2
+      - {index: 0x6010, sub_index: 0x0E, type: bool} # Sync error
+      - {index: 0x0000, sub_index: 0x00, type: bool} # Gap
+      - {index: 0x6010, sub_index: 0x10, type: bool} # TxPDO Toggle
+sm:  # Sync Manager
+  - {index: 0, type: output, pdo: ~, watchdog: disable}
+  - {index: 1, type: input, pdo: ~, watchdog: disable}
+  - {index: 2, type: output, pdo: rpdo, watchdog: disable}
+  - {index: 3, type: input, pdo: tpdo, watchdog: disable}

ramsai_description/config/initial_positions.yaml

@@ -1,6 +1,6 @@
 # Default initial positions for the panda arm's ros2_control fake system
 initial_positions:
-  joint_a1: 0.0
+  joint_a1: -1.5707
   joint_a2: -0.7854
   joint_a3: 0.0
   joint_a4: 1.3962

ramsai_description/config/ramsai.config.xacro

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <!-- Kuka iiwa 14 7DoF manipulator -->
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="iiwa14">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="iiwa_print">
 
     <!-- Enable setting arguments from the launch file -->
     <xacro:arg name="use_sim" default="false" />
@@ -17,6 +17,9 @@
     <!-- Import iiwa ros2_control description -->
     <xacro:include filename="$(find iiwa_description)/ros2_control/iiwa.r2c_hardware.xacro" />
 
+    <!-- Import extruder ros2_control description -->
+    <xacro:include filename="$(find ramsai_description)/ros2_control/extruder.ros2_control.xacro" />
+
     <!-- Import all Gazebo-customization elements -->
     <!-- <xacro:include filename="$(find iiwa_description)/gazebo/iiwa.gazebo"/> -->
 
@@ -26,15 +29,17 @@
         <static>true</static>
     </gazebo>
 
-    <xacro:iiwa14 parent="world" prefix="$(arg prefix)">
+    <xacro:iiwa_print parent="world" prefix="$(arg prefix)">
         <origin xyz="0 0 0" rpy="0 0 0" />
-    </xacro:iiwa14>
+    </xacro:iiwa_print>
 
     <xacro:iiwa_r2c_hardware
-        name="iiwaRobot" prefix="$(arg prefix)"
+        name="iiwa_print" prefix="$(arg prefix)"
         robot_ip="$(arg robot_ip)" robot_port="$(arg robot_port)"
         command_interface="$(arg command_interface)"
         initial_positions_file="$(arg initial_positions_file)"
         use_sim="$(arg use_sim)" use_fake_hardware="$(arg use_fake_hardware)"
         />
+
+    <xacro:extruder/>
 </robot>

ramsai_description/config/ramsai_controllers.yaml

@@ -0,0 +1,78 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 225  # Hz
+
+    iiwa_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    impedance_controller:
+      type: impedance_controller/ImpedanceController
+
+    ets_state_broadcaster:
+      type: external_torque_sensor_broadcaster/ExternalTorqueSensorBroadcaster
+
+    gpio_command_controller:
+      type: gpio_controllers/GpioCommandController
+
+iiwa_arm_controller:
+  ros__parameters:
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
+      - joint_a7
+
+    state_publish_rate: 200.0 # Defaults to 50
+    action_monitor_rate: 20.0 # Defaults to 20
+
+velocity_controller:
+  ros__parameters:
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
+      - joint_a7
+
+impedance_controller:
+  ros__parameters:
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
+      - joint_a7
+    stiffness: [50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0]
+    damping: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0]
+
+ets_state_broadcaster:
+  ros__parameters:
+    sensor_name: external_torque_sensor
+
+gpio_command_controller:
+  ros__parameters:
+    gpios:
+      - extruder
+    command_interfaces:
+      extruder:
+        - enable
+        - reset
+        - velocity

ramsai_description/moveit2/common_hybrid_planning_params.yaml

@@ -0,0 +1,7 @@
+# Parameters that are shared across several component nodes
+/**:
+  ros__parameters:
+    # A namespace can be added if multiple hybrid planners are launched
+    global_planning_action_name: "global_planning_action"
+    local_planning_action_name: "local_planning_action"
+    hybrid_planning_action_name: "run_hybrid_planning"