From 67ed12ec1881f1f30bc1db9b3ae71b774d3d9264 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 18 Dec 2024 11:08:38 +0900 Subject: [PATCH] =?UTF-8?q?=E3=83=AC=E3=83=93=E3=83=A5=E3=83=BC=E6=8C=87?= =?UTF-8?q?=E6=91=98=E4=BA=8B=E9=A0=85=E3=81=AB=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples_py/README.md | 69 ++----------------- .../launch/example.launch.py | 31 +++++---- .../sciurus17_examples_py/gripper_control.py | 2 + .../sciurus17_examples_py/neck_control.py | 6 ++ .../pick_and_place_left_arm.py | 42 ++++++----- .../pick_and_place_right_arm_waist.py | 42 ++++++----- .../sciurus17_examples_py/waist_control.py | 3 + 7 files changed, 81 insertions(+), 114 deletions(-) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 7f59f3a..78f2f77 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -2,15 +2,8 @@ このパッケージはSciurus17 ROS 2パッケージのPythonサンプルコード集です。 -- [sciurus17\_examples\_py](#sciurus17_examples_py) - - [準備（実機を使う場合）](#準備実機を使う場合) - - [1. Sciurus17本体をPCに接続する](#1-sciurus17本体をpcに接続する) - - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する) - - [3. move\_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する) - - [準備 (Gazeboを使う場合)](#準備-gazeboを使う場合) - - [1. move\_groupとGazeboを起動する](#1-move_groupとgazeboを起動する) - - [準備（Mock Componentsを使う場合）](#準備mock-componentsを使う場合) - - [1. move\_groupとcontrollerを起動する](#1-move_groupとcontrollerを起動する) +- [sciurus17\_examples](#sciurus17_examples) + - [起動方法](#起動方法) - [サンプルプログラムを実行する](#サンプルプログラムを実行する) - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合) - [Examples](#examples) @@ -20,62 +13,8 @@ - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) - -## 準備（実機を使う場合） - -### 1. Sciurus17本体をPCに接続する -Sciurus17本体をPCに接続します。 -接続方法は製品マニュアルを参照してください。 - -**※Sciurus17本体が接触しないように、十分なスペースを確保してください。** - -### 2. USB通信ポートの接続を確認する - -USB通信ポートの設定については`sciurus17_control`の -[README](../sciurus17_control/README.md) -を参照してください。 - -**正しく設定できていない場合、Sciurus17が動作しないので注意してください** - -### 3. move_groupとcontrollerを起動する - -次のコマンドでmove_group (`sciurus17_moveit_config`)と -controller (`sciurus17_control`)を起動します。 - -```sh -ros2 launch sciurus17_examples demo.launch.py -``` - -## 準備 (Gazeboを使う場合) - -### 1. move_groupとGazeboを起動する - -次のコマンドでmove_group (`sciurus17_moveit_config`)と -Gazeboを起動します。 - -```sh -ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py -``` - -頭部カメラや胸部カメラのシミュレーションを行わない場合は、 -`use_head_camera`、`use_chest_camera`オプションを`false`に設定します。 - -```sh -ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py use_head_camera:=false use_chest_camera:=false -``` - -## 準備（Mock Componentsを使う場合） - -### 1. move_groupとcontrollerを起動する - -次のコマンドでmove_group (`sciurus17_moveit_config`)と -controller (`sciurus17_control`)を起動します。 - -```sh -ros2 launch sciurus17_examples demo.launch.py use_mock_components:=true -``` - -Mock Componentsではカメラを使ったサンプルを実行することはできません。 +## 起動方法 +Sciurus17の起動方法は[sciurus17_examplesのREADME](../sciurus17_examples/README.md)を参照してください。 ## サンプルプログラムを実行する diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index 024cfc2..c5bd907 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -17,28 +17,25 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.actions import SetParameter from moveit_configs_utils import MoveItConfigsBuilder from sciurus17_description.robot_description_loader import RobotDescriptionLoader def generate_launch_description(): - ld = LaunchDescription() + description_loader = RobotDescriptionLoader() - - ld.add_action( - DeclareLaunchArgument('loaded_description', + 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.')) + in sciurus17_description.') moveit_config = (MoveItConfigsBuilder('sciurus17').planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, - ).trajectory_execution(file_path='config/moveit_controllers.yaml').planning_pipelines( - pipelines=['ompl']).moveit_cpp( - file_path=get_package_share_directory('sciurus17_examples_py') + - '/config/sciurus17_moveit_py_examples.yaml').to_moveit_configs()) + ).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') @@ -53,6 +50,11 @@ def generate_launch_description(): '[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.') + ) + example_node = Node( name=[LaunchConfiguration('example'), '_node'], package='sciurus17_examples_py', @@ -61,7 +63,10 @@ def generate_launch_description(): parameters=[moveit_config.to_dict()], ) - ld.add_action(declare_example_name) - ld.add_action(example_node) - - return ld + return LaunchDescription([ + declare_loaded_description, + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + declare_example_name, + example_node + ]) diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py index 4fda0f0..8cb031d 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -75,6 +75,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 右グリッパ開閉 for _ in range(2): r_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -98,6 +99,7 @@ def main(args=None): single_plan_parameters=gripper_plan_request_params, ) + # 左グリッパ開閉 for _ in range(2): l_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) diff --git a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py index 065b619..99fb541 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py @@ -65,6 +65,7 @@ def main(args=None): robot_state = scene.current_state joint_values = robot_state.get_joint_group_positions('neck_group') + # 首を左に向ける joint_values[0] = math.radians(45.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -77,6 +78,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 首を右に向ける joint_values[0] = math.radians(-45.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -89,6 +91,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 首を前に向ける joint_values[0] = math.radians(0.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -101,6 +104,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 首を上に向ける joint_values[1] = math.radians(45.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -113,6 +117,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 首を下に向ける joint_values[1] = math.radians(-45.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -125,6 +130,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 'neck_init_pose'に戻す neck.set_start_state_to_current_state() neck.set_goal_state(configuration_name='neck_init_pose') plan_and_execute( diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py index 5145280..5c58b26 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py @@ -94,9 +94,10 @@ def main(args=None): # 物体の上に腕を伸ばす l_arm.set_start_state_to_current_state() - l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_GRASP_POSE), - pose_link='l_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -106,9 +107,10 @@ def main(args=None): # 掴みに行く l_arm.set_start_state_to_current_state() - l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=GRASP_POSE), - pose_link='l_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = GRASP_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -130,9 +132,10 @@ def main(args=None): # 持ち上げる l_arm.set_start_state_to_current_state() - l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_GRASP_POSE), - pose_link='l_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -142,9 +145,10 @@ def main(args=None): # 移動する l_arm.set_start_state_to_current_state() - l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_RELEASE_POSE), - pose_link='l_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -154,9 +158,10 @@ def main(args=None): # 下ろす l_arm.set_start_state_to_current_state() - l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=RELEASE_POSE), - pose_link='l_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = RELEASE_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -178,9 +183,10 @@ def main(args=None): # ハンドを持ち上げる l_arm.set_start_state_to_current_state() - l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_RELEASE_POSE), - pose_link='l_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py index 49f2c29..5cff84e 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -107,9 +107,10 @@ def main(args=None): # 物体の上に腕を伸ばす r_arm_waist.set_path_constraints(path_constraints=constraints) r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_GRASP_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -119,9 +120,10 @@ def main(args=None): # 掴みに行く r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=GRASP_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = GRASP_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -143,9 +145,10 @@ def main(args=None): # 持ち上げる r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_GRASP_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -155,9 +158,10 @@ def main(args=None): # 移動する r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_RELEASE_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -167,9 +171,10 @@ def main(args=None): # 下ろす r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=RELEASE_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = RELEASE_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -191,9 +196,10 @@ def main(args=None): # ハンドを持ち上げる r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_RELEASE_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, diff --git a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py index 8b96f67..73afdbb 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py @@ -65,6 +65,7 @@ def main(args=None): robot_state = scene.current_state joint_values = robot_state.get_joint_group_positions('waist_group') + # 腰を左に向ける joint_values[0] = math.radians(45.0) waist.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -77,6 +78,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 腰を右に向ける joint_values[0] = math.radians(-45.0) waist.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -89,6 +91,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 'waist_init_pose'に戻す waist.set_start_state_to_current_state() waist.set_goal_state(configuration_name='waist_init_pose') plan_and_execute(