Moveit!2を使ったマニピュレータの制御

Moveit!2のインストール

Ubuntu22.04上にHubmleがインストールされていることを前提とします。

下記コマンドでMoveit!2をインストールします。

sudo apt install ros-humble-moveit

これだけでOK。

マニピュレータのモデルの利用

ROS 2対応のUR5のリポジトリをダウンロードして動作確認をする。

下記コマンドでUR5のリポジトリをダウンロード。

cd ~/ros2_ws/src

git clone -b humble https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git

下記で依存パッケージダウンロード

rosdep update

rosdep install --ignore-src --from-paths . -y

下記コマンドでbuildする。

cd ~/ros2_ws/src

catkin build

buildが終わったら下記のコマンドでUR5のURDFを表示。

ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5 use_sim_time:=True

下記写真のような画面が立ち上がる。

UR5のロボットのモデルの準備

公式の最新のURのリポジトリだとよくわからないので，古いUR5のURDFファイルを取ってきてMoveit2に与えて逆運動学が解けるようにします．

ロボットのモデルを管理するパッケージを作る．今回は「ur5_description」という名前にします．

cd ~/ros2_ws/src

ros2 pkg create --build-type ament_cmake ur5_description

cd ur5_description

mkdir urdf

cd urdf

wget https://raw.githubusercontent.com/uwgraphics/RelaxedIK/master/urdfs/ur5.urdf

cd ..

mkdir rviz

cd rviz

gedit ur5.rviz #ファイルを編集

mkdir launch

cd launch

gedit view_robot.launch.py #ファイルを編集

wgetでリンク先のur5のurdfファイルをダウンロードしている．

ur5.rvizとview_robot.launch.pyは下記の内容を記述．

ur5.rviz

Panels:

  - Class: rviz_common/Displays

    Name: Displays

  - Class: rviz_common/Views

    Name: Views

Visualization Manager:

  Displays:

    - Class: rviz_default_plugins/Grid

      Name: Grid

      Value: true

    - Alpha: 0.8

      Class: rviz_default_plugins/RobotModel

      Description Topic:

        Value: /robot_description

      Name: RobotModel

      Value: true

    - Class: rviz_default_plugins/TF

      Name: TF

      Value: true

  Global Options:

    Fixed Frame: base_link

  Tools:

    - Class: rviz_default_plugins/MoveCamera

  Value: true

  Views:

    Current:

      Class: rviz_default_plugins/Orbit

      Distance: 1.7

      Name: Current View

      Pitch: 0.33

      Value: Orbit (rviz)

      Yaw: 5.5

Window Geometry:

  Height: 800

  Width: 1200

view_robot.launch.py

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription

from launch_ros.actions import Node

def generate_launch_description():

    urdf_file_name = 'ur5.urdf'

    urdf = os.path.join(

        get_package_share_directory('ur5_description'),

        'urdf',

        urdf_file_name)

    return LaunchDescription([

        Node(

            package='robot_state_publisher',

            executable='robot_state_publisher',

            name='robot_state_publisher',

            output='screen',

            parameters=[{'robot_description': open(urdf).read()}]

        )

    ])

ROS1のときはURDFファイルがworkspace配下の/srcの中にあるパッケージのプログラム中にlaunchファイルとかurdfがあればOKだったのですが，ROS 2では，workspace配下にあるinstallディレクトリにコピーしなくてはいけないので，CMakeLists.txtを編集します．

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)

project(ur5_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(robot_state_publisher REQUIRED)

find_package(xacro REQUIRED)

# uncomment the following section in order to fill in

# further dependencies manually.

# find_package(<dependency> REQUIRED)

if(BUILD_TESTING)

  find_package(ament_lint_auto REQUIRED)

  # the following line skips the linter which checks for copyrights

  # comment the line when a copyright and license is added to all source files

  set(ament_cmake_copyright_FOUND TRUE)

  # the following line skips cpplint (only works in a git repo)

  # comment the line when this package is in a git repo and when

  # a copyright and license is added to all source files

  set(ament_cmake_cpplint_FOUND TRUE)

  ament_lint_auto_find_test_dependencies()

endif()

#追加

install(DIRECTORY

  urdf

  launch

  rviz

  DESTINATION share/${PROJECT_NAME}/

)

ament_package()

package.xmlも修正しておく

package.xml

<?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>ur5_description</name>

  <version>0.0.0</version>

  <description>TODO: Package description</description>

  <maintainer email="test@test.com">test</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>

  #追記

  <build_depend>robot_state_publisher</build_depend>

  <build_depend>xacro</build_depend>

  <exec_depend>robot_state_publisher</exec_depend>

  <exec_depend>xacro</exec_depend>

  <export>

    <build_type>ament_cmake</build_type>

  </export>

</package>

修正が終わったらビルドをする．

cd ~/ros2_ws

colcon build

source ~/.bashrc

ビルドが成功したら下記のようにlaunchファイルを立ち上げる

ros2 launch ur5_description view_robot.launch.py

下記のようにrviz上にUR5のモデルが表示される．

関節角の指定ができていないので正確に表示ができていない．．．

Moveit2 Setup Assistantのインストール

下記コマンドでmoveit2のSetup Assistantをインストール

 sudo apt install ros-humble-moveit-setup-assistant

インストールが終わったら下記コマンドで立ち上げ

ros2 launch moveit_setup_assistant setup_assistant.launch.py

立ち上げると下記の画面が現れる。