ARM: Cartesian Arm Control using MoveIt

.github/workflows/ci.yaml

@@ -16,6 +16,8 @@ env:
     uwrt_mars_rover_drivetrain
     uwrt_mars_rover_drivetrain_description
     uwrt_mars_rover_drivetrain_hw
+    uwrt_mars_rover_arm_urdf
+    uwrt_mars_rover_arm_urdf_moveit
   ROS_DISTRO: galactic
   IMAGE: "rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-desktop-latest"
 

.gitignore

@@ -12,6 +12,10 @@ msg/*Result.msg
 msg/_*.py
 build_isolated/
 devel_isolated/
+compile_commands.json
+.clangd
+
+.ccls-cache/
 
 # Generated by dynamic reconfigure
 *.cfgc

uwrt_mars_rover/package.xml

@@ -15,6 +15,7 @@
   <exec_depend>uwrt_mars_rover_description</exec_depend>
   <exec_depend>uwrt_mars_rover_drivetrain</exec_depend>
   <exec_depend>uwrt_mars_rover_drivetrain_hw</exec_depend>
+  <exec_depend>uwrt_mars_rover_arm</exec_depend>
 
   <test_depend>ament_cmake_xmllint</test_depend>
 

uwrt_mars_rover_arm/uwrt_mars_rover_arm/CMakeLists.txt

@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.8)
+project(uwrt_mars_rover_arm)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+
+if(BUILD_TESTING)
+  # generate compile_commands.json for clang-tidy
+  set(CMAKE_EXPORT_COMPILE_COMMANDS ON CACHE INTERNAL "")
+
+  # cppcheck
+  find_package(ament_cmake_cppcheck REQUIRED)
+  ament_cppcheck()
+
+  # clang-format
+  find_package(ament_cmake_clang_format REQUIRED)
+  ament_clang_format()
+
+  # flake8
+  find_package(ament_cmake_flake8 REQUIRED)
+  ament_flake8()
+
+  # xmllint
+  find_package(ament_cmake_xmllint REQUIRED)
+  ament_xmllint()
+
+  # clang-tidy
+  find_package(ament_cmake_clang_tidy REQUIRED)
+  ament_clang_tidy(${CMAKE_BINARY_DIR})
+endif()
+
+ament_package()

uwrt_mars_rover_arm/uwrt_mars_rover_arm/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>uwrt_mars_rover_arm</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">niiquaye</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>uwrt_mars_rover_arm_hw</build_depend>
+
+
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_clang_tidy</test_depend>
+  <test_depend>ament_cmake_clang_tidy</test_depend>
+  <test_depend>ament_cmake_clang_format</test_depend>
+  <test_depend>ament_cmake_flake8</test_depend>
+  <test_depend>ament_cmake_xmllint</test_depend>
+  <test_depend>ament_cmake_cppcheck</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

uwrt_mars_rover_arm/uwrt_mars_rover_arm_description/uwrt_mars_rover_arm_urdf/CMakeLists.txt

@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.5)
+project(uwrt_mars_rover_arm_urdf)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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(gazebo_ros2_control REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(
+  DIRECTORY launch rviz urdf config meshes
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

uwrt_mars_rover_arm/uwrt_mars_rover_arm_description/uwrt_mars_rover_arm_urdf/config/joint_names_dummy_urdf.yaml

@@ -0,0 +1 @@
+controller_joint_names: ['', 'Joint_1', 'Joint_2', 'Joint_3', 'Joint_4', 'Joint_5', 'Joint_6', ]

uwrt_mars_rover_arm/uwrt_mars_rover_arm_description/uwrt_mars_rover_arm_urdf/launch/armlaunchbase.py

@@ -0,0 +1,222 @@
+# MIT License
+#
+# Copyright (c) 2019-2021 University of Waterloo Robotics Team;
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
+                            RegisterEventHandler)
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import (Command, FindExecutable, LaunchConfiguration,
+                                  PathJoinSubstitution)
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'controllers_file',
+            default_value='arm_controllers.yaml',
+            description='YAML file with the controllers configuration.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'description_package',
+            default_value='uwrt_mars_rover_arm_description',
+            description='Description package with robot URDF/xacro files. Usually the argument \
+        is not set, it enables use of a custom description.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'description_file',
+            description='URDF/XACRO description file with the robot.',
+            default_value='urdf.xacro'
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'use_sim',
+            default_value='true',
+            description='Start robot in Gazebo simulation.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'use_fake_hardware',
+            default_value='true',
+            description='Start robot with fake hardware mirroring command to its states.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'fake_sensor_commands',
+            default_value='true',
+            description="Enable fake command interfaces for sensors used for simple simulations. \
+            Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'slowdown', default_value='3.0', description='Slowdown factor of the robot.'
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'robot_controller',
+            default_value='forward_position_controller',
+            description='Robot controller to start.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'start_rviz',
+            default_value='true',
+            description='Start RViz2 automatically with this launch file.',
+        )
+    )
+
+    # Initialize Arguments
+    controllers_file = LaunchConfiguration('controllers_file')
+    description_package = LaunchConfiguration('description_package')
+    description_file = LaunchConfiguration('description_file')
+    use_sim = LaunchConfiguration('use_sim')
+    use_fake_hardware = LaunchConfiguration('use_fake_hardware')
+    fake_sensor_commands = LaunchConfiguration('fake_sensor_commands')
+    slowdown = LaunchConfiguration('slowdown')
+    robot_controller = LaunchConfiguration('robot_controller')
+    start_rviz = LaunchConfiguration('start_rviz')
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name='xacro')]),
+            ' ',
+            PathJoinSubstitution(
+                [FindPackageShare(description_package), 'urdf', description_file]
+            ),
+            ' ',
+            'use_sim:=',
+            use_sim,
+            ' ',
+            'use_fake_hardware:=',
+            use_fake_hardware,
+            ' ',
+            'fake_sensor_commands:=',
+            fake_sensor_commands,
+            ' ',
+            'slowdown:=',
+            slowdown,
+        ]
+    )
+    robot_description = {'robot_description': robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [FindPackageShare(description_package),
+         'config', controllers_file,
+         ]
+    )
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(description_package), 'rviz', 'rviz.rviz']
+    )
+
+    control_node = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[robot_description, robot_controllers],
+        output={
+            'stdout': 'screen',
+            'stderr': 'screen',
+        },
+    )
+    robot_state_pub_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='both',
+        parameters=[robot_description],
+    )
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='log',
+        arguments=['-d', rviz_config_file],
+        condition=IfCondition(start_rviz),
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+    )
+
+    robot_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=[robot_controller, '-c', '/controller_manager'],
+    )
+
+    # Delay rviz start after `joint_state_broadcaster`
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
+    # Delay start of robot_controller after `joint_state_broadcaster`
+    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[robot_controller_spawner],
+        )
+    )
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py')
+        )
+    )
+
+    nodes = [
+        gazebo,
+        control_node,
+        robot_state_pub_node,
+        joint_state_broadcaster_spawner,
+        robot_controller_spawner,
+        rviz_node,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)