diff --git a/README.md b/README.md index 8fadce3..ca97ad4 100644 --- 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 new file mode 100644 index 0000000..78f2f77 --- /dev/null +++ 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 new file mode 100644 index 0000000..a0879fa --- /dev/null +++ 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 new file mode 100644 index 0000000..4c56024 --- /dev/null +++ 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 new file mode 100644 index 0000000..1234c7f --- /dev/null +++ b/sciurus17_examples_py/package.xml @@ -0,0 +1,25 @@ + + + + sciurus17_examples_py + 0.1.0 + Python examples of Sciurus17 ROS package + RT Corporation + Apache License 2.0 + + chama1176 + + rclpy + std_msgs + moveit + moveit_py + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/sciurus17_examples_py/resource/sciurus17_examples_py b/sciurus17_examples_py/resource/sciurus17_examples_py new file mode 100644 index 0000000..e69de29 diff --git a/sciurus17_examples_py/sciurus17_examples_py/__init__.py b/sciurus17_examples_py/sciurus17_examples_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py new file mode 100755 index 0000000..8cb031d --- /dev/null +++ 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() diff --git a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py new file mode 100755 index 0000000..99fb541 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py @@ -0,0 +1,149 @@ +# 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.neck_control') + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name='neck_control') + logger.info('MoveItPy instance created') + + # 首制御用 planning component + neck = sciurus17.get_planning_component('neck_group') + planning_scene_monitor = sciurus17.get_planning_scene_monitor() + + robot_model = sciurus17.get_robot_model() + + 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 + + # SRDFに定義されている'neck_init_pose'の姿勢にする + neck.set_start_state_to_current_state() + neck.set_goal_state(configuration_name='neck_init_pose') + plan_and_execute( + sciurus17, + neck, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在角度をベースに、目標角度を作成する + joint_values = [] + with planning_scene_monitor.read_only() as scene: + 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) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + 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) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + 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) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + 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) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + 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) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + 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( + sciurus17, + neck, + logger, + single_plan_parameters=plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 new file mode 100755 index 0000000..f136799 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py @@ -0,0 +1,224 @@ +# 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 copy +import math + +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +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.pick_and_place_left_arm') + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name='pick_and_place_left_arm') + logger.info('MoveItPy instance created') + + # 左腕制御用 planning component + l_arm = sciurus17.get_planning_component('l_arm_group') + # 左グリッパ制御用 planning component + l_gripper = sciurus17.get_planning_component('l_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_CLOSE = math.radians(0.0) + GRIPPER_OPEN = math.radians(-40.0) + GRIPPER_GRASP = math.radians(-20.0) + # 物体を持ち上げる高さ + LIFTING_HEIFHT = 0.25 + # 物体を掴む位置 + GRASP_POSE = Pose(position=Point(x=0.25, y=0.0, z=0.12), + orientation=Quaternion(x=-0.707, y=0.0, z=0.0, w=0.707)) # downward + PRE_AND_POST_GRASP_POSE = copy.deepcopy(GRASP_POSE) + PRE_AND_POST_GRASP_POSE.position.z = LIFTING_HEIFHT + # 物体を置く位置 + RELEASE_POSE = Pose(position=Point(x=0.35, y=0.0, z=0.12), + orientation=Quaternion(x=-0.707, y=0.0, z=0.0, w=0.707)) # downward + PRE_AND_POST_RELEASE_POSE = copy.deepcopy(RELEASE_POSE) + PRE_AND_POST_RELEASE_POSE.position.z = LIFTING_HEIFHT + + # SRDFに定義されている'l_arm_init_pose'の姿勢にする + l_arm.set_start_state_to_current_state() + l_arm.set_goal_state(configuration_name='l_arm_init_pose') + plan_and_execute( + sciurus17, + l_arm, + logger, + single_plan_parameters=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', [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_arm.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=plan_request_params, + ) + + # 掴みに行く + l_arm.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=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', [GRIPPER_GRASP]) + l_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + l_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # 持ち上げる + l_arm.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=plan_request_params, + ) + + # 移動する + l_arm.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=plan_request_params, + ) + + # 下ろす + l_arm.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=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', [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_arm.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=plan_request_params, + ) + + # SRDFに定義されている'l_arm_init_pose'の姿勢にする + l_arm.set_start_state_to_current_state() + l_arm.set_goal_state(configuration_name='l_arm_init_pose') + plan_and_execute( + sciurus17, + l_arm, + logger, + single_plan_parameters=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', [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() 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 new file mode 100755 index 0000000..bba7737 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -0,0 +1,237 @@ +# 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 copy +import math + +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from moveit_msgs.msg import Constraints, JointConstraint +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.pick_and_place_right_arm_waist') + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name='pick_and_place_right_arm_waist') + logger.info('MoveItPy instance created') + + # 右腕と腰制御用 planning component + r_arm_waist = sciurus17.get_planning_component('r_arm_waist_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 = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # グリッパの開閉角 + GRIPPER_CLOSE = math.radians(0.0) + GRIPPER_OPEN = math.radians(40.0) + GRIPPER_GRASP = math.radians(20.0) + # 物体を持ち上げる高さ + LIFTING_HEIFHT = 0.25 + # 物体を掴む位置 + GRASP_POSE = Pose(position=Point(x=0.25, y=0.0, z=0.12), + orientation=Quaternion(x=0.707, y=0.0, z=0.0, w=0.707)) # downward + PRE_AND_POST_GRASP_POSE = copy.deepcopy(GRASP_POSE) + PRE_AND_POST_GRASP_POSE.position.z = LIFTING_HEIFHT + # 物体を置く位置 + RELEASE_POSE = Pose(position=Point(x=0.35, y=0.0, z=0.12), + orientation=Quaternion(x=0.707, y=0.0, z=0.0, w=0.707)) # downward + PRE_AND_POST_RELEASE_POSE = copy.deepcopy(RELEASE_POSE) + PRE_AND_POST_RELEASE_POSE.position.z = LIFTING_HEIFHT + + # SRDFに定義されている'r_arm_waist_init_pose'の姿勢にする + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(configuration_name='r_arm_waist_init_pose') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=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', [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, + ) + + constraints = Constraints() + joint_constraint = JointConstraint() + joint_constraint.joint_name = 'waist_yaw_joint' + joint_constraint.position = 0.0 + joint_constraint.tolerance_above = math.radians(45.0) + joint_constraint.tolerance_below = math.radians(45.0) + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + # 物体の上に腕を伸ばす + r_arm_waist.set_path_constraints(path_constraints=constraints) + r_arm_waist.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=plan_request_params, + ) + + # 掴みに行く + r_arm_waist.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=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', [GRIPPER_GRASP]) + r_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + r_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # 持ち上げる + r_arm_waist.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=plan_request_params, + ) + + # 移動する + r_arm_waist.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=plan_request_params, + ) + + # 下ろす + r_arm_waist.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=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', [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_arm_waist.set_start_state_to_current_state() + 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, + logger, + single_plan_parameters=plan_request_params, + ) + + # SRDFに定義されている'r_arm_waist_init_pose'の姿勢にする + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(configuration_name='r_arm_waist_init_pose') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=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', [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, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sciurus17_examples_py/sciurus17_examples_py/utils.py b/sciurus17_examples_py/sciurus17_examples_py/utils.py new file mode 100644 index 0000000..94dda86 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/utils.py @@ -0,0 +1,28 @@ +import time + + +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + logger.info('Planning trajectory') + if multi_plan_parameters is not None: + plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters) + elif single_plan_parameters is not None: + plan_result = planning_component.plan(single_plan_parameters=single_plan_parameters) + else: + plan_result = planning_component.plan() + + # execute the plan + if plan_result: + logger.info('Executing plan') + robot_trajectory = plan_result.trajectory + robot.execute(robot_trajectory, controllers=[]) + else: + logger.error('Planning failed') + + time.sleep(sleep_time) diff --git a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py new file mode 100755 index 0000000..73afdbb --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py @@ -0,0 +1,110 @@ +# 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.waist_control') + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name='waist_control') + logger.info('MoveItPy instance created') + + # 腰制御用 planning component + waist = sciurus17.get_planning_component('waist_group') + planning_scene_monitor = sciurus17.get_planning_scene_monitor() + + robot_model = sciurus17.get_robot_model() + + 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 + + # SRDFに定義されている'waist_init_pose'の姿勢にする + waist.set_start_state_to_current_state() + waist.set_goal_state(configuration_name='waist_init_pose') + plan_and_execute( + sciurus17, + waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在角度をベースに、目標角度を作成する + joint_values = [] + with planning_scene_monitor.read_only() as scene: + 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) + robot_state.set_joint_group_positions('waist_group', joint_values) + waist.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + waist, + logger, + 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) + robot_state.set_joint_group_positions('waist_group', joint_values) + waist.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + waist, + logger, + 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( + sciurus17, + waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sciurus17_examples_py/setup.cfg b/sciurus17_examples_py/setup.cfg new file mode 100644 index 0000000..8ea28b9 --- /dev/null +++ b/sciurus17_examples_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sciurus17_examples_py +[install] +install_scripts=$base/lib/sciurus17_examples_py diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py new file mode 100644 index 0000000..8ee83fb --- /dev/null +++ b/sciurus17_examples_py/setup.py @@ -0,0 +1,36 @@ +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'sciurus17_examples_py' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, + 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='RT Corporation', + maintainer_email='shop@rt-net.jp', + description='python examples of Sciurus17 ROS package', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'gripper_control = sciurus17_examples_py.gripper_control:main', + 'neck_control = sciurus17_examples_py.neck_control:main', + 'waist_control = sciurus17_examples_py.waist_control:main', + 'pick_and_place_right_arm_waist = \ + sciurus17_examples_py.pick_and_place_right_arm_waist:main', + 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', + ], + }, +) diff --git a/sciurus17_examples_py/test/test_copyright.py b/sciurus17_examples_py/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/sciurus17_examples_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/sciurus17_examples_py/test/test_flake8.py b/sciurus17_examples_py/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/sciurus17_examples_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/sciurus17_examples_py/test/test_pep257.py b/sciurus17_examples_py/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/sciurus17_examples_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'