From a14f513331b330162f4bbfc3e27e9d351da1963c Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 19 Aug 2024 10:53:42 +0900 Subject: [PATCH] add gripper_control example --- .../config/sciurus17_moveit_py_examples.yaml | 2 +- .../sciurus17_examples_py/gripper_control.py | 162 ++++++++++++++++++ .../pick_and_place_left_arm.py | 7 +- sciurus17_examples_py/setup.py | 1 + 4 files changed, 166 insertions(+), 6 deletions(-) create mode 100755 sciurus17_examples_py/sciurus17_examples_py/gripper_control.py diff --git a/sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml b/sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml index c439aef..a0879fa 100644 --- a/sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml +++ b/sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml @@ -16,7 +16,7 @@ plan_request_params: max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 -ompl_rrtc: # Namespace for individual plan request +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 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..1f259e7 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -0,0 +1,162 @@ +# Copyright 2023 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 time +import math +import copy + +# generic ros libraries +import rclpy +from rclpy.logging import get_logger +from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion +from std_msgs.msg import Header + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + + +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + """Helper function to plan and execute a motion.""" + 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) + + +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): + 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, + ) + + 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, + ) + + # Finishe 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 index a3a6ce0..8f535b2 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 @@ -76,18 +76,15 @@ def main(args=None): plan_request_params = PlanRequestParameters( sciurus17, - "ompl_rrtc", + "ompl_rrtc_default", ) gripper_plan_request_params = PlanRequestParameters( sciurus17, - "ompl_rrtc", + "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) diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index ba4e9de..de5dcab 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -26,6 +26,7 @@ entry_points={ 'console_scripts': [ 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', + 'gripper_control = sciurus17_examples_py.gripper_control:main', ], }, )