moveit_pyサンプルを追加 (#164)

* jazzyでビルドが通るように修正 * capabilitiesに対応 * gz-sim対応 * OMPLの設定ファイルを追加 * doc, ciをJazzy対応 * Revert "jazzyでビルドが通るように修正" This reverts commit 7fdbb47. * 誤ってフォーマッターをかけてしまっていたので修正します * usb camの依存パッケージのバージョンについて記載 * リリースされるまでバージョン指定を入れておく * two handsをompl設定に追加 * MoveitConfigsBuilder周辺の記載を変更 * add mock components support * 登録位置への移動サンプル * グリッパー開閉を確認 * 一通りの流れを再現 * ファイル構成を変更 * licence * add gripper_control example * utilsを分離 * 左右腕が違ったので反転 * mergeを修正 * 不要なものを削除 * flake8通るように修正 * add neck sample * fix format * add waist sample * add right arm waist sample * READMEを修正 * fix copyright * Update sciurus17_examples_py/sciurus17_examples_py/gripper_control.py Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/sciurus17_examples_py/neck_control.py Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/README.md Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/README.md Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/sciurus17_examples_py/waist_control.py Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py Co-authored-by: Kuwamai <[email protected]> * Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py Co-authored-by: Kuwamai <[email protected]> * レビュー指摘事項に対応 * 指摘事項を修正 * flake8通るように修正 * Update sciurus17_examples_py/package.xml Co-authored-by: Kuwamai <[email protected]> * add author tag * READMEに追加 --------- Co-authored-by: Kuwamai <[email protected]>
rt-net · Dec 25, 2024 · 1976541 · 1976541
1 parent 40d079e
commit 1976541
Show file tree

Hide file tree

Showing 18 changed files with 1,245 additions and 0 deletions.
diff --git a/README.md b/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

diff --git a/sciurus17_examples_py/README.md b/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)
+
+---
diff --git a/sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml b/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
diff --git a/sciurus17_examples_py/launch/example.launch.py b/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
+    ])
diff --git a/sciurus17_examples_py/package.xml b/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>
diff --git a/sciurus17_examples_py/resource/sciurus17_examples_py b/sciurus17_examples_py/resource/sciurus17_examples_py
diff --git a/sciurus17_examples_py/sciurus17_examples_py/__init__.py b/sciurus17_examples_py/sciurus17_examples_py/__init__.py
diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/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()