moveit_pyでロボットを動かすサンプルを追加します

README.md

@@ -104,6 +104,9 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control'
 - sciurus17_examples
   - [README](./sciurus17_examples/README.md)
   - Sciurus17のサンプルコード集です  
+- sciurus17_examples_py
+  - [README](./sciurus17_examples_py/README.md)
+  - Sciurus17のPythonサンプルコード集です  
 - sciurus17_gazebo
   - Sciurus17のGazeboシミュレーションパッケージです
 - sciurus17_moveit_config

sciurus17_examples_py/README.md

@@ -0,0 +1,122 @@
+# sciurus17_examples_py
+
+このパッケージはSciurus17 ROS 2パッケージのPythonサンプルコード集です。
+
+- [sciurus17\_examples](#sciurus17_examples)
+  - [起動方法](#起動方法)
+  - [サンプルプログラムを実行する](#サンプルプログラムを実行する)
+    - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合)
+  - [Examples](#examples)
+    - [gripper\_control](#gripper_control)
+    - [neck\_control](#neck_control)
+    - [waist\_control](#waist_control)
+    - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)
+    - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
+
+## 起動方法
+Sciurus17の起動方法は[sciurus17_examplesのREADME](../sciurus17_examples/README.md)を参照してください。
+
+## サンプルプログラムを実行する
+
+準備ができたらサンプルプログラムを実行します。
+例えばグリッパを開閉するサンプルは次のコマンドで実行できます。
+
+```sh
+ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control'
+```
+
+終了するときは`Ctrl+c`を入力します。
+
+### Gazeboでサンプルプログラムを実行する場合
+
+Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。
+
+```sh
+ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control' use_sim_time:='true'
+```
+
+## Examples
+
+`demo.launch`を実行している状態で各サンプルを実行できます。
+
+- [gripper\_control](#gripper_control)
+- [neck\_control](#neck_control)
+- [waist\_control](#waist_control)
+- [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)
+- [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
+
+実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。
+
+```sh
+ros2 launch sciurus17_examples_py example.launch.py -s
+```
+
+### gripper_control
+
+ハンドを開閉させるコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control'
+```
+
+[back to example list](#examples)
+
+---
+
+### neck_control
+
+首を上下左右へ動かすコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples_py example.launch.py example:='neck_control'
+```
+
+[back to example list](#examples)
+
+---
+
+### waist_control
+
+腰を左右へひねる動作をするコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples_py example.launch.py example:='waist_control'
+```
+
+[back to example list](#examples)
+
+---
+
+### pick_and_place_right_arm_waist
+
+右手でターゲットを掴んで動かすコード例です。腰の回転も使用します。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples_py example.launch.py example:='pick_and_place_right_arm_waist'
+```
+
+[back to example list](#examples)
+
+---
+
+### pick_and_place_left_arm
+
+左手でターゲットを掴んで動かすコード例です。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples_py example.launch.py example:='pick_and_place_left_arm'
+```
+
+[back to example list](#examples)
+
+---

sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml

@@ -0,0 +1,26 @@
+planning_scene_monitor_options:
+  name: "planning_scene_monitor"
+  robot_description: "robot_description"
+  joint_state_topic: "/joint_states"
+  attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
+  publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
+  monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
+  wait_for_initial_state_timeout: 10.0
+
+planning_pipelines:
+  pipeline_names: ["ompl"]
+
+plan_request_params:
+  planning_attempts: 1
+  planning_pipeline: ompl
+  max_velocity_scaling_factor: 1.0
+  max_acceleration_scaling_factor: 1.0
+
+ompl_rrtc_default:  # Namespace for individual plan request
+  plan_request_params:  # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
+    planning_attempts: 1  # Number of attempts the planning pipeline tries to solve a given motion planning problem
+    planning_pipeline: ompl  # Name of the pipeline that is being used
+    planner_id: "RRTConnectkConfigDefault"  # Name of the specific planner to be used by the pipeline
+    max_velocity_scaling_factor: 1.0  # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    max_acceleration_scaling_factor: 1.0  # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    planning_time: 1.0  # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned

sciurus17_examples_py/launch/example.launch.py

@@ -0,0 +1,76 @@
+# Copyright 2024 RT Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+from sciurus17_description.robot_description_loader import RobotDescriptionLoader
+
+
+def generate_launch_description():
+
+    description_loader = RobotDescriptionLoader()
+    declare_loaded_description = DeclareLaunchArgument(
+        'loaded_description',
+        default_value=description_loader.load(),
+        description='Set robot_description text.  \
+                    It is recommended to use RobotDescriptionLoader() \
+                    in sciurus17_description.')
+
+    moveit_config = (MoveItConfigsBuilder('sciurus17').planning_scene_monitor(
+        publish_robot_description=True,
+        publish_robot_description_semantic=True,
+        ).moveit_cpp(file_path=get_package_share_directory('sciurus17_examples_py') +
+                     '/config/sciurus17_moveit_py_examples.yaml').to_moveit_configs())
+
+    moveit_config.robot_description = {
+        'robot_description': LaunchConfiguration('loaded_description')
+    }
+
+    moveit_config.move_group_capabilities = {'capabilities': ''}
+
+    declare_example_name = DeclareLaunchArgument(
+        'example',
+        default_value='gripper_control',
+        description=('Set an example executable name: '
+                     '[gripper_control, neck_control, waist_control, '
+                     'pick_and_place_right_arm_waist, pick_and_place_left_arm]'))
+
+    declare_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time', default_value='false',
+        description=('Set true when using the gazebo simulator.')
+    )
+
+    # 下記Issue対応のためここでパラメータを設定する
+    # https://github.com/moveit/moveit2/issues/2940#issuecomment-2401302214
+    config_dict = moveit_config.to_dict()
+    config_dict.update({'use_sim_time': LaunchConfiguration('use_sim_time')})
+
+    example_node = Node(
+        name=[LaunchConfiguration('example'), '_node'],
+        package='sciurus17_examples_py',
+        executable=LaunchConfiguration('example'),
+        output='screen',
+        parameters=[config_dict],
+    )
+
+    return LaunchDescription([
+        declare_loaded_description,
+        declare_use_sim_time,
+        declare_example_name,
+        example_node
+    ])

sciurus17_examples_py/package.xml

@@ -0,0 +1,25 @@
+<?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>sciurus17_examples_py</name>
+  <version>0.1.0</version>
+  <description>Python examples of Sciurus17 ROS package</description>
+  <maintainer email="[email protected]">RT Corporation</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">chama1176</author>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>moveit</exec_depend>
+  <exec_depend>moveit_py</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

sciurus17_examples_py/sciurus17_examples_py/gripper_control.py

@@ -0,0 +1,132 @@
+# Copyright 2024 RT Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+import math
+
+
+# moveit python library
+from moveit.core.robot_state import RobotState
+from moveit.planning import (
+    MoveItPy,
+    PlanRequestParameters,
+)
+# generic ros libraries
+import rclpy
+from rclpy.logging import get_logger
+
+from sciurus17_examples_py.utils import plan_and_execute
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    logger = get_logger('moveit_py.gripper_control')
+
+    # instantiate MoveItPy instance and get planning component
+    sciurus17 = MoveItPy(node_name='gripper_control')
+    logger.info('MoveItPy instance created')
+
+    # 腕制御用 planning component
+    arm = sciurus17.get_planning_component('two_arm_group')
+    # 左グリッパ制御用 planning component
+    l_gripper = sciurus17.get_planning_component('l_gripper_group')
+    # 右グリッパ制御用 planning component
+    r_gripper = sciurus17.get_planning_component('r_gripper_group')
+
+    robot_model = sciurus17.get_robot_model()
+
+    plan_request_params = PlanRequestParameters(
+        sciurus17,
+        'ompl_rrtc_default',
+    )
+    gripper_plan_request_params = PlanRequestParameters(
+        sciurus17,
+        'ompl_rrtc_default',
+    )
+    # 動作速度の調整
+    plan_request_params.max_acceleration_scaling_factor = 0.1    # Set 0.0 ~ 1.0
+    plan_request_params.max_velocity_scaling_factor = 0.1    # Set 0.0 ~ 1.0
+    # 動作速度の調整
+    gripper_plan_request_params.max_acceleration_scaling_factor = 0.1    # Set 0.0 ~ 1.0
+    gripper_plan_request_params.max_velocity_scaling_factor = 0.1    # Set 0.0 ~ 1.0
+
+    # グリッパの開閉角
+    R_GRIPPER_CLOSE = math.radians(0.0)
+    R_GRIPPER_OPEN = math.radians(40.0)
+    L_GRIPPER_CLOSE = math.radians(0.0)
+    L_GRIPPER_OPEN = math.radians(-40.0)
+
+    # SRDFに定義されている'two_arm_init_pose'の姿勢にする
+    arm.set_start_state_to_current_state()
+    arm.set_goal_state(configuration_name='two_arm_init_pose')
+    plan_and_execute(
+        sciurus17,
+        arm,
+        logger,
+        single_plan_parameters=plan_request_params,
+    )
+
+    # 右グリッパ開閉
+    for _ in range(2):
+        r_gripper.set_start_state_to_current_state()
+        robot_state = RobotState(robot_model)
+        robot_state.set_joint_group_positions('r_gripper_group', [R_GRIPPER_OPEN])
+        r_gripper.set_goal_state(robot_state=robot_state)
+        plan_and_execute(
+            sciurus17,
+            r_gripper,
+            logger,
+            single_plan_parameters=gripper_plan_request_params,
+        )
+
+        r_gripper.set_start_state_to_current_state()
+        robot_state = RobotState(robot_model)
+        robot_state.set_joint_group_positions('r_gripper_group', [R_GRIPPER_CLOSE])
+        r_gripper.set_goal_state(robot_state=robot_state)
+        plan_and_execute(
+            sciurus17,
+            r_gripper,
+            logger,
+            single_plan_parameters=gripper_plan_request_params,
+        )
+
+    # 左グリッパ開閉
+    for _ in range(2):
+        l_gripper.set_start_state_to_current_state()
+        robot_state = RobotState(robot_model)
+        robot_state.set_joint_group_positions('l_gripper_group', [L_GRIPPER_OPEN])
+        l_gripper.set_goal_state(robot_state=robot_state)
+        plan_and_execute(
+            sciurus17,
+            l_gripper,
+            logger,
+            single_plan_parameters=gripper_plan_request_params,
+        )
+
+        l_gripper.set_start_state_to_current_state()
+        robot_state = RobotState(robot_model)
+        robot_state.set_joint_group_positions('l_gripper_group', [L_GRIPPER_CLOSE])
+        l_gripper.set_goal_state(robot_state=robot_state)
+        plan_and_execute(
+            sciurus17,
+            l_gripper,
+            logger,
+            single_plan_parameters=gripper_plan_request_params,
+        )
+
+    # Finish with error. Related Issue
+    # https://github.com/moveit/moveit2/issues/2693
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()