ROS 2でMoveIt2を使う場合には、humbleまでは、C++のインターフェースしか提供されておらず、アプリケーション開発は、ほぼC++を使用するのみでした。

しかし、ROS 2(Iron)からようやくPythonインターフェースである moveit_pyがリリースされ、オフィシャルパッケージでPythonを使ったアプリケーション開発が可能になっています。

moveit_pyの詳細は、オフィシャルサイトを参照していただきたいのですが、チュートリアル以外での活用には、ちょっとハードルが高い感じがします。

moveit_pyについて調査をおこなったところ、以前のPythonインターフェースとその立ち位置が大きく変化しています。MoveGroupCommanderは、別に起動させていたMoveGroupノードへのインターフェースというものであり、MoveGroupノードが起動していなければいけませんでした。しかし、moveit_pyは、MoveGroupのライブラリのPythonラッパーになっており、moveit_pyを使用して作成したアプリケーション（ノード）は、MoveGroupノードを必要としていません。（下図参照）

そのため、moveit_pyを使ったプログラムでは、MoveGroupノードを起動するための設定等をros_parameterまたはDICT形式のデータとして渡す必要があります。

したがって、上記のことを踏まえた上で、アプリケーションを実装する必要があります。

denso_robot_ros2をJazzy上で動作させ、moveit_pyを使ってプログラム開発をしていきます。前述のようにmoveit_pyでは、MoveGroupノードを起動させるときの各種パラメータ設定が必要なのですが、MoveIt2では、MoveItConfigsBuilderを使って設定するようになっています。

move_groupやmoveit_pyのros_parameterの設定には、MoveItConfigsBuilderを使用するようになっています。このクラスは、YAMLファイルで記述されたパラメータ設定を読み込み、move_groupを起動するために必要な情報をDICT型の情報に変換することができます。 この情報は、launchファイルに記述することが一般的ですが、MoveItPyを使ったプログラムを単独で動作させるときには、この情報をインスタンス生成時に、引数として渡す必要があります。

このクラスのもっとも簡単な使用方法は、下記の通りです。

moveit_configs = MoveItConfigsBuilder("robot_name", 
                                     robot_description="robot_description",
                                     package_name = "package_name")
                .robot_description_semantic(Path("my_config") / "my_file.srdf")
                .to_moveit_configs()

''MoveItConfisBuilder''では、以下の情報を取り扱います。

• ''robot_description:'' ロボットのURDF(urdfFファイル or urdf.xacroファイル)
• ''robot_description_semantic:'' ロボットのSRDF(srdfファイル or srdf.xacroファイル)
• ''robot_description_kinematics:'' キネマティクス設定ファイル（kinematics.yaml)
• ''planning_pipelines:'' 軌道計画のプラグイン
• ''trajctory_execution:'' 軌道追従制御ノードの設定(moveit_controllers.taml)
• ''planning_scene_monitor:'' Planning Sceneのトピックの設定
• ''sensors_3d:'' 3Dセンサに関する設定ファイル(sensors_3d.yaml)
• ''joint_limits:'' 各関節の動作範囲、速度、加速の設定ファイル(joint_limits.yaml)
• ''moveit_cpp:'' move_groupへ渡すための設定(planning_pipelinsなどの設定をがファイルにできる）
• ''pilz_cartesian_limits:'' "pilz_industrial_motion_planner"を選択した時の設定ファイル(pilz_cartesial_limits.yaml)

例えば、COBOTTAを動作させるためのdenso_robot_ros2パッケージでは、下記のようになります。

MOVEIT_CONFIG = (
      MoveItConfigsBuilder("denso_robot", package_name="denso_robot_moveit_config")
       .robot_description(
           file_path=os.path.join(get_package_share_directory("denso_robot_descriptions),
                             "urdf", "denso_robot.urdf.xacro"),
           mappings={
                "ip_address": "192.168.0.1",
                "model": "cobotta",
                "send_format": "0",
                "recv_format": "2",
                "namespace": "",
                "verbose": "false",
                "sim": "false",
                "with_tool": "false",
                "ros2_control_hardware_type": "",
                 },
        )
       .robot_description_semantic(
            file_path="srdf/denso_robot.srdf.xacro",
            mappings={ "model": "cobotta", "namespace": "",},
       )
       .planning_scene_monitor(
           publish_robot_description=True,
           publish_robot_description_semantic=True,
       )
       .trajectory_execution(file_path="robots/cobotta/config/moveit_controllers.yaml")
       .joint_limits(file_path="robots/cobotta/config/joint_limits.yaml")
       .robot_description_kinematics(file_path="config/kinematics.yaml")
       .planning_pipelines(
           pipelines=["ompl"],
           default_planning_pipeline="ompl",
       )
       .to_moveit_configs()
 )

上記のコードで生成された、DICT形式の情報は、launchファイルでもNode設定の parametersの要素として使用しています。

では、moveit_pyを使って、COBOTTAの手先を5cm移動させるプログラムを作成します。

 import rclpy
 import rclpy.logging

 from geometry_msgs.msg import PoseStamped
 from moveit.planning import  MoveItPy
 from config import MOVEIT_CONFIG
 
 rclpy.init()
 logger = rclpy.logging.get_logger('moveit_py.pose_goal')
 
 cobotta = MoveItPy(node_name='moveit_py', config_dict=MOVEIT_CONFIG.to_dict())
 arm = cobotta.get_planning_component('arm')
 logger.info("MoveItPy instance created")
 
 model = cobotta.get_robot_model()
 
 s1=arm.get_start_state()
 print(s1.get_pose('J6'))
 
 goal_pose = PoseStamped()
 goal_pose.header.frame_id = 'world'
 goal_pose.pose = copy.deepcopy(s1.get_pose('J6'))
 goal_pose.pose.position.x += 0.05
 
 arm.set_start_state_to_current_state()
 arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='J6')
 plan_result=arm.plan()
 if plan_result:
   trj = plan_result.trajectory
   print(trj)
   cobotta.execute(trj, controllers=[])
 else:
   print('Fail to plan...')
 
 cobotta.shutdown()

moveit_pyを使ったプログラムとして、cobotta_client(jazzy)を公開しています。moveit_pyを使ったクラスの実装は、継続して行っていきますが、現在は、test/sample.pyに実装しています。