From b09444cd547310b961ba1f5b82d22979f3b725d2 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Tue, 28 Jan 2025 19:11:55 +0900 Subject: [PATCH] =?UTF-8?q?=E3=82=B3=E3=83=BC=E3=83=87=E3=82=A3=E3=83=B3?= =?UTF-8?q?=E3=82=B0=E3=82=B9=E3=82=BF=E3=82=A4=E3=83=AB=E3=82=92=E4=BB=96?= =?UTF-8?q?=E8=A3=BD=E5=93=81=E3=81=A8=E7=B5=B1=E4=B8=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/crane_x7_moveit_py_examples.yaml | 2 +- .../crane_x7_examples_py/gripper_control.py | 93 +++--- .../crane_x7_examples_py/joint_values.py | 87 +++--- .../crane_x7_examples_py/pick_and_place.py | 272 ++++++++---------- .../crane_x7_examples_py/pose_groupstate.py | 71 +++-- .../crane_x7_examples_py/utils.py | 29 +- crane_x7_examples_py/launch/example.launch.py | 62 ++-- crane_x7_examples_py/setup.py | 23 +- 8 files changed, 283 insertions(+), 356 deletions(-) diff --git a/crane_x7_examples_py/config/crane_x7_moveit_py_examples.yaml b/crane_x7_examples_py/config/crane_x7_moveit_py_examples.yaml index bf3489e..6f9d44f 100644 --- a/crane_x7_examples_py/config/crane_x7_moveit_py_examples.yaml +++ b/crane_x7_examples_py/config/crane_x7_moveit_py_examples.yaml @@ -23,4 +23,4 @@ ompl_rrtc: # Namespace for individual plan request 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: 5.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 + 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/crane_x7_examples_py/crane_x7_examples_py/gripper_control.py b/crane_x7_examples_py/crane_x7_examples_py/gripper_control.py index 1dfbfeb..5dbd204 100644 --- a/crane_x7_examples_py/crane_x7_examples_py/gripper_control.py +++ b/crane_x7_examples_py/crane_x7_examples_py/gripper_control.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -13,10 +13,6 @@ # limitations under the License. import math -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger # moveit python library from moveit.core.robot_state import RobotState @@ -24,106 +20,105 @@ MoveItPy, PlanRequestParameters, ) +# generic ros libraries +import rclpy +from rclpy.logging import get_logger from crane_x7_examples_py.utils import plan_and_execute def main(args=None): - # ros2の初期化 rclpy.init(args=args) + logger = get_logger('gripper_control') - # ロガー生成 - logger = get_logger('pick_and_place') - - # MoveItPy初期化 - crane_x7 = MoveItPy(node_name='moveit_py') - crane_x7_arm = crane_x7.get_planning_component('arm') - crane_x7_gripper = crane_x7.get_planning_component('gripper') + # instantiate MoveItPy instance and get planning component + crane_x7 = MoveItPy(node_name='gripper_control') logger.info('MoveItPy instance created') + # アーム制御用 planning component + arm = crane_x7.get_planning_component('arm') + # グリッパ制御用 planning component + gripper = crane_x7.get_planning_component('gripper') + # instantiate a RobotState instance using the current robot model robot_model = crane_x7.get_robot_model() - robot_state = RobotState(robot_model) - # planningのパラメータ設定 - # armのパラメータ設定用 arm_plan_request_params = PlanRequestParameters( crane_x7, 'ompl_rrtc', ) - arm_plan_request_params.max_acceleration_scaling_factor \ - = 1.0 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor \ - = 1.0 # Set 0.0 ~ 1.0 - - # gripperのパラメータ設定用 gripper_plan_request_params = PlanRequestParameters( crane_x7, 'ompl_rrtc', ) - 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 + + # 動作速度の調整 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # 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 # SRDFに定義されている'home'の姿勢にする - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state(configuration_name='home') + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) # gripperを60[deg]に開く - robot_state.set_joint_group_positions('gripper', [math.radians(60)]) - crane_x7_gripper.set_start_state_to_current_state() - crane_x7_gripper.set_goal_state(robot_state=robot_state) + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(60.0)]) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_x7, - crane_x7_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) # gripperを0[deg]に閉じる - robot_state.set_joint_group_positions('gripper', [math.radians(0)]) - crane_x7_gripper.set_start_state_to_current_state() - crane_x7_gripper.set_goal_state(robot_state=robot_state) + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(0.0)]) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_x7, - crane_x7_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) # gripperを60[deg]に開く - robot_state.set_joint_group_positions('gripper', [math.radians(60)]) - crane_x7_gripper.set_start_state_to_current_state() - crane_x7_gripper.set_goal_state(robot_state=robot_state) + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(60.0)]) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_x7, - crane_x7_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) # gripperを0[deg]に閉じる - robot_state.set_joint_group_positions('gripper', [math.radians(0)]) - crane_x7_gripper.set_start_state_to_current_state() - crane_x7_gripper.set_goal_state(robot_state=robot_state) + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(0.0)]) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_x7, - crane_x7_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - # MoveItPyの終了 - crane_x7.shutdown() - - # rclpyの終了 + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() diff --git a/crane_x7_examples_py/crane_x7_examples_py/joint_values.py b/crane_x7_examples_py/crane_x7_examples_py/joint_values.py index 82fb5f2..10520a2 100644 --- a/crane_x7_examples_py/crane_x7_examples_py/joint_values.py +++ b/crane_x7_examples_py/crane_x7_examples_py/joint_values.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,61 +14,53 @@ import math -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger - # moveit python library +from moveit.core.kinematic_constraints import construct_joint_constraint from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, PlanRequestParameters, ) -from moveit.core.kinematic_constraints import construct_joint_constraint +# generic ros libraries +import rclpy +from rclpy.logging import get_logger from crane_x7_examples_py.utils import plan_and_execute - def main(args=None): - # ros2の初期化 rclpy.init(args=args) + logger = get_logger('joint_values') - # ロガー生成 - logger = get_logger("joint_values") + # instantiate MoveItPy instance and get planning component + crane_x7 = MoveItPy(node_name='joint_values') + logger.info('MoveItPy instance created') - # MoveItPy初期化 - crane_x7 = MoveItPy(node_name="moveit_py") - crane_x7_arm = crane_x7.get_planning_component("arm") - logger.info("MoveItPy instance created") + # アーム制御用 planning component + arm = crane_x7.get_planning_component('arm') - # instantiate a a RobotState instance using the current robot model + # instantiate a RobotState instance using the current robot model robot_model = crane_x7.get_robot_model() robot_state = RobotState(robot_model) - plan_request_params = PlanRequestParameters( + arm_plan_request_params = PlanRequestParameters( crane_x7, - "ompl_rrtc", + 'ompl_rrtc', ) - # 駆動速度を調整する - plan_request_params.max_acceleration_scaling_factor = 0.5 # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor = 0.5 # Set 0.0 ~ 1.0 - - # SRDFに定義されている"vertical"の姿勢にする - # すべてのジョイントの目標角度が0度になる - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state(configuration_name="vertical") + + # 動作速度の調整 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # SRDFに定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, - single_plan_parameters=plan_request_params, + single_plan_parameters=arm_plan_request_params, ) - # 現在角度をベースに、目標角度を作成する - crane_x7_arm.set_start_state_to_current_state() - - # 各ジョイントの角度を１つずつ変更する joint_names = [ "crane_x7_shoulder_fixed_part_pan_joint", "crane_x7_shoulder_revolute_part_tilt_joint", @@ -79,37 +71,38 @@ def main(args=None): "crane_x7_wrist_joint", ] target_joint_value = math.radians(-45.0) + + # 各関節角度を順番に-45[deg]ずつ動かす for joint_name in joint_names: + arm.set_start_state_to_current_state() + joint_values = {joint_name: target_joint_value} robot_state.joint_positions = joint_values joint_constraint = construct_joint_constraint( robot_state=robot_state, - joint_model_group=crane_x7.get_robot_model() - .get_joint_model_group("arm"), + joint_model_group=crane_x7.get_robot_model().get_joint_model_group("arm"), ) - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, - single_plan_parameters=plan_request_params, + single_plan_parameters=arm_plan_request_params, ) - # 垂直に戻す - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state(configuration_name="vertical") + # SRDFに定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, - single_plan_parameters=plan_request_params, + single_plan_parameters=arm_plan_request_params, ) - # MoveItPyの終了 - crane_x7.shutdown() - - # rclpyの終了 + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() diff --git a/crane_x7_examples_py/crane_x7_examples_py/pick_and_place.py b/crane_x7_examples_py/crane_x7_examples_py/pick_and_place.py index 70df4eb..ed5837f 100644 --- a/crane_x7_examples_py/crane_x7_examples_py/pick_and_place.py +++ b/crane_x7_examples_py/crane_x7_examples_py/pick_and_place.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,85 +12,89 @@ # See the License for the specific language governing permissions and # limitations under the License. +import copy import math -import rclpy -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion from moveit_msgs.msg import Constraints, JointConstraint - -# generic ros libraries -from rclpy.logging import get_logger - -# moveit python library from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, PlanRequestParameters, ) - -from crane_x7_examples_py.utils import plan_and_execute, euler_to_quaternion +import rclpy +from rclpy.logging import get_logger +from crane_x7_examples_py.utils import plan_and_execute +from scipy.spatial.transform import Rotation def main(args=None): - # ros2の初期化 rclpy.init(args=args) - - # ロガー生成 logger = get_logger('pick_and_place') - # MoveItPy初期化 - crane_x7 = MoveItPy(node_name='moveit_py') - crane_x7_arm = crane_x7.get_planning_component('arm') - crane_x7_gripper = crane_x7.get_planning_component('gripper') + # instantiate MoveItPy instance and get planning component + crane_x7 = MoveItPy(node_name='pick_and_place') logger.info('MoveItPy instance created') + # アーム制御用 planning component + arm = crane_x7.get_planning_component('arm') + # グリッパ制御用 planning component + gripper = crane_x7.get_planning_component('gripper') + # instantiate a RobotState instance using the current robot model robot_model = crane_x7.get_robot_model() - robot_state = RobotState(robot_model) - # planningのパラメータ設定 - # armのパラメータ設定用 arm_plan_request_params = PlanRequestParameters( crane_x7, 'ompl_rrtc', ) - arm_plan_request_params.max_acceleration_scaling_factor \ - = 1.0 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor \ - = 1.0 # Set 0.0 ~ 1.0 - - # gripperのパラメータ設定用 gripper_plan_request_params = PlanRequestParameters( crane_x7, 'ompl_rrtc', ) - 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の開閉角度 - GRIPPER_DEFAULT = 0.0 + # 動作速度の調整 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # 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 = 0.0 GRIPPER_OPEN = math.radians(60.0) - GRIPPER_CLOSE = math.radians(20.0) + GRIPPER_GRASP = math.radians(20.0) + # 物体を持ち上げる高さ + LIFTING_HEIFHT = 0.3 + # 物体を掴む位置 + gripper_quat = Rotation.from_euler('xyz', [-180.0, 0.0, -90.0], degrees=True).as_quat() + gripper_quat_msg = Quaternion(x=gripper_quat[0], y=gripper_quat[1], z=gripper_quat[2], w=gripper_quat[3]) + GRASP_POSE = Pose(position=Point(x=0.2, y=0.0, z=0.13), orientation=gripper_quat_msg) + 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.2, y=0.2, z=0.13), orientation=gripper_quat_msg) + PRE_AND_POST_RELEASE_POSE = copy.deepcopy(RELEASE_POSE) + PRE_AND_POST_RELEASE_POSE.position.z = LIFTING_HEIFHT # SRDFに定義されている'home'の姿勢にする - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state(configuration_name='home') + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) # 何かを掴んでいた時のためにハンドを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) - crane_x7_gripper.set_start_state_to_current_state() - crane_x7_gripper.set_goal_state(robot_state=robot_state) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_x7, - crane_x7_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) @@ -107,195 +111,159 @@ def main(args=None): joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) - # 掴む準備をする - target_pose = PoseStamped() - target_pose.header.frame_id = 'crane_x7_shoulder_fixed_part_link' + joint_constraint.joint_name = 'crane_x7_upper_arm_revolute_part_twist_joint' + joint_constraint.position = 0.0 + joint_constraint.tolerance_above = math.radians(30) + joint_constraint.tolerance_below = math.radians(30) + joint_constraint.weight = 0.8 + constraints.joint_constraints.append(joint_constraint) - target_pose.pose.position.x = 0.2 - target_pose.pose.position.y = 0.0 - target_pose.pose.position.z = 0.3 - q = euler_to_quaternion(math.radians(180), math.radians(0), - math.radians(-90)) - target_pose.pose.orientation.x = q[0] - target_pose.pose.orientation.y = q[1] - target_pose.pose.orientation.z = q[2] - target_pose.pose.orientation.w = q[3] + arm.set_path_constraints(constraints) - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state( - pose_stamped_msg=target_pose, pose_link='crane_x7_gripper_base_link', - motion_plan_constraints=[joint_constraint] + # 物体の上に腕を伸ばす + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_x7_mounting_plate_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + arm.set_goal_state( + pose_stamped_msg=goal_pose, + pose_link='crane_x7_gripper_base_link', ) plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) # 掴みに行く - target_pose.pose.position.x = 0.2 - target_pose.pose.position.y = 0.0 - target_pose.pose.position.z = 0.13 - q = euler_to_quaternion(math.radians(180), math.radians(0), - math.radians(-90)) - target_pose.pose.orientation.x = q[0] - target_pose.pose.orientation.y = q[1] - target_pose.pose.orientation.z = q[2] - target_pose.pose.orientation.w = q[3] - - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state( - pose_stamped_msg=target_pose, pose_link='crane_x7_gripper_base_link', - motion_plan_constraints=[joint_constraint] + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_x7_mounting_plate_link' + goal_pose.pose = GRASP_POSE + arm.set_goal_state( + pose_stamped_msg=goal_pose, + pose_link='crane_x7_gripper_base_link', ) plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) # ハンドを閉じる - robot_state.set_joint_group_positions('gripper', [GRIPPER_CLOSE]) - crane_x7_gripper.set_start_state_to_current_state() - crane_x7_gripper.set_goal_state(robot_state=robot_state) + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_GRASP]) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_x7, - crane_x7_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) # 持ち上げる - target_pose.pose.position.x = 0.2 - target_pose.pose.position.y = 0.0 - target_pose.pose.position.z = 0.3 - q = euler_to_quaternion(math.radians(180), math.radians(0), - math.radians(-90)) - target_pose.pose.orientation.x = q[0] - target_pose.pose.orientation.y = q[1] - target_pose.pose.orientation.z = q[2] - target_pose.pose.orientation.w = q[3] - - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state( - pose_stamped_msg=target_pose, pose_link='crane_x7_gripper_base_link', - motion_plan_constraints=[joint_constraint] + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_x7_mounting_plate_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + arm.set_goal_state( + pose_stamped_msg=goal_pose, + pose_link='crane_x7_gripper_base_link', ) plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) # 移動する - target_pose.pose.position.x = 0.2 - target_pose.pose.position.y = 0.2 - target_pose.pose.position.z = 0.3 - q = euler_to_quaternion(math.radians(180), math.radians(0), - math.radians(-90)) - target_pose.pose.orientation.x = q[0] - target_pose.pose.orientation.y = q[1] - target_pose.pose.orientation.z = q[2] - target_pose.pose.orientation.w = q[3] - - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state( - pose_stamped_msg=target_pose, pose_link='crane_x7_gripper_base_link', - motion_plan_constraints=[joint_constraint] + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_x7_mounting_plate_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + arm.set_goal_state( + pose_stamped_msg=goal_pose, + pose_link='crane_x7_gripper_base_link', ) plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) # 下ろす - target_pose.pose.position.x = 0.2 - target_pose.pose.position.y = 0.2 - target_pose.pose.position.z = 0.13 - q = euler_to_quaternion(math.radians(180), math.radians(0), - math.radians(-90)) - target_pose.pose.orientation.x = q[0] - target_pose.pose.orientation.y = q[1] - target_pose.pose.orientation.z = q[2] - target_pose.pose.orientation.w = q[3] - - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state( - pose_stamped_msg=target_pose, pose_link='crane_x7_gripper_base_link', - motion_plan_constraints=[joint_constraint] + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_x7_mounting_plate_link' + goal_pose.pose = RELEASE_POSE + arm.set_goal_state( + pose_stamped_msg=goal_pose, + pose_link='crane_x7_gripper_base_link', ) plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) + # ハンドを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) - crane_x7_gripper.set_start_state_to_current_state() - crane_x7_gripper.set_goal_state(robot_state=robot_state) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_x7, - crane_x7_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - # 少しだけハンドを持ち上げる - target_pose.pose.position.x = 0.2 - target_pose.pose.position.y = 0.2 - target_pose.pose.position.z = 0.2 - q = euler_to_quaternion(math.radians(180), math.radians(0), - math.radians(-90)) - target_pose.pose.orientation.x = q[0] - target_pose.pose.orientation.y = q[1] - target_pose.pose.orientation.z = q[2] - target_pose.pose.orientation.w = q[3] - - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state( - pose_stamped_msg=target_pose, pose_link='crane_x7_gripper_base_link', - motion_plan_constraints=[joint_constraint] + # ハンドを持ち上げる + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_x7_mounting_plate_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + arm.set_goal_state( + pose_stamped_msg=goal_pose, + pose_link='crane_x7_gripper_base_link', ) plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # 可動範囲の制限を解除 # SRDFに定義されている'home'の姿勢にする - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state(configuration_name='home', - motion_plan_constraints=[]) + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) # ハンドを閉じる - robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) - crane_x7_gripper.set_start_state_to_current_state() - crane_x7_gripper.set_goal_state(robot_state=robot_state) + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_CLOSE]) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_x7, - crane_x7_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - # MoveItPyの終了 - crane_x7.shutdown() - - # rclpyの終了 + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() diff --git a/crane_x7_examples_py/crane_x7_examples_py/pose_groupstate.py b/crane_x7_examples_py/crane_x7_examples_py/pose_groupstate.py index 74e8879..b6b9352 100644 --- a/crane_x7_examples_py/crane_x7_examples_py/pose_groupstate.py +++ b/crane_x7_examples_py/crane_x7_examples_py/pose_groupstate.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,75 +12,70 @@ # See the License for the specific language governing permissions and # limitations under the License. -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger - # moveit python library from moveit.planning import ( MoveItPy, PlanRequestParameters, ) +# generic ros libraries +import rclpy +from rclpy.logging import get_logger + from crane_x7_examples_py.utils import plan_and_execute def main(args=None): - # ros2の初期化 rclpy.init(args=args) + logger = get_logger('pose_groupstate') - # ロガー生成 - logger = get_logger("pose_groupstate") + # instantiate MoveItPy instance and get planning component + crane_x7 = MoveItPy(node_name='pose_groupstate') + logger.info('MoveItPy instance created') - # MoveItPy初期化 - crane_x7 = MoveItPy(node_name="moveit_py") - crane_x7_arm = crane_x7.get_planning_component("arm") - logger.info("MoveItPy instance created") + # アーム制御用 planning component + arm = crane_x7.get_planning_component('arm') - # planningのパラメータ設定 - plan_request_params = PlanRequestParameters( + arm_plan_request_params = PlanRequestParameters( crane_x7, - "ompl_rrtc", + 'ompl_rrtc', ) - # 速度＆加速度のスケーリングファクタを設定 - plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + # 動作速度の調整 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # 現在の位置から"home"ポジションに動かす - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state(configuration_name="home") + # SRDFに定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, - single_plan_parameters=plan_request_params, + single_plan_parameters=arm_plan_request_params, ) - # 現在の位置から"vertical"ポジションに動かす - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state(configuration_name="vertical") + # SRDFに定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, - single_plan_parameters=plan_request_params, + single_plan_parameters=arm_plan_request_params, ) - # 現在の位置から"home"ポジションに動かす - crane_x7_arm.set_start_state_to_current_state() - crane_x7_arm.set_goal_state(configuration_name="home") + # SRDFに定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_x7, - crane_x7_arm, + arm, logger, - single_plan_parameters=plan_request_params, + single_plan_parameters=arm_plan_request_params, ) - # MoveItPyの終了 - crane_x7.shutdown() - - # rclpyの終了 + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() diff --git a/crane_x7_examples_py/crane_x7_examples_py/utils.py b/crane_x7_examples_py/crane_x7_examples_py/utils.py index 6f5d9db..08577cf 100644 --- a/crane_x7_examples_py/crane_x7_examples_py/utils.py +++ b/crane_x7_examples_py/crane_x7_examples_py/utils.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 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 @@ -10,8 +10,6 @@ # limitations under the License. import time -import numpy as np -import quaternion # generic ros libraries from rclpy.logging import get_logger @@ -51,28 +49,3 @@ def plan_and_execute( result = False time.sleep(sleep_time) return result - - -# euler --> quaternion -def euler_to_quaternion(roll, pitch, yaw): - cy = np.cos(yaw * 0.5) - sy = np.sin(yaw * 0.5) - cr = np.cos(roll * 0.5) - sr = np.sin(roll * 0.5) - cp = np.cos(pitch * 0.5) - sp = np.sin(pitch * 0.5) - - qw = cy * cr * cp + sy * sr * sp - qx = cy * sr * cp - sy * cr * sp - qy = cy * cr * sp + sy * sr * cp - qz = sy * cr * cp - cy * sr * sp - - return [qx, qy, qz, qw] - - -# rotation matrix --> quaternion -def rotation_matrix_to_quaternion(rotation_matrix): - # numpy-quaternionを使用して回転行列からクォータニオンを計算 - # 3x3の回転行列をnumpy.quaternionに変換する - q = quaternion.from_rotation_matrix(rotation_matrix) - return q diff --git a/crane_x7_examples_py/launch/example.launch.py b/crane_x7_examples_py/launch/example.launch.py index 6a22c14..95469f6 100644 --- a/crane_x7_examples_py/launch/example.launch.py +++ b/crane_x7_examples_py/launch/example.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,10 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - from ament_index_python.packages import get_package_share_directory -from crane_x7_description.crane_x7_description.robot_description_loader \ +from crane_x7_description.robot_description_loader \ import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -25,16 +23,14 @@ def generate_launch_description(): - ld = LaunchDescription() + description_loader = RobotDescriptionLoader() - ld.add_action( - DeclareLaunchArgument( - 'loaded_description', - default_value=description_loader.load(), - description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() \ - in crane_x7_description.', - ) + declare_loaded_description = DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() \ + in crane_x7_description.', ) moveit_config = ( @@ -43,22 +39,6 @@ def generate_launch_description(): publish_robot_description=True, publish_robot_description_semantic=True, ) - .robot_description( - file_path=os.path.join( - get_package_share_directory('crane_x7_description'), - 'urdf', - 'crane_x7.urdf.xacro', - ), - mappings={}, - ) - .robot_description_kinematics( - file_path=get_package_share_directory('crane_x7_moveit_config') - + '/config/kinematics.yaml' - ) - .trajectory_execution( - file_path=get_package_share_directory('crane_x7_moveit_config') - + '/config/controllers.yaml' - ) .moveit_cpp( file_path=get_package_share_directory('crane_x7_examples_py') + '/config/crane_x7_moveit_py_examples.yaml' @@ -77,19 +57,33 @@ def generate_launch_description(): default_value='gripper_control', description=( 'Set an example executable name: ' - '[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]' + '[gripper_control, pose_groupstate, joint_values, pick_and_place]' ), ) + 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='crane_x7_examples_py', executable=LaunchConfiguration('example'), output='screen', - parameters=[moveit_config.to_dict()], + parameters=[config_dict], ) - ld.add_action(declare_example_name) - ld.add_action(example_node) + return LaunchDescription([ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + example_node, + ]) + - return ld diff --git a/crane_x7_examples_py/setup.py b/crane_x7_examples_py/setup.py index 78e3bae..1dfe537 100644 --- a/crane_x7_examples_py/setup.py +++ b/crane_x7_examples_py/setup.py @@ -1,25 +1,34 @@ +from glob import glob +import os + from setuptools import find_packages, setup package_name = 'crane_x7_examples_py' setup( name=package_name, - version='0.0.0', + version='0.1.0', packages=find_packages(exclude=['test']), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ('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='root', - maintainer_email='root@todo.todo', - description='TODO: Package description', - license='Apache-2.0', + 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 = crane_x7_examples_py.gripper_control:main', + 'joint_values = crane_x7_examples_py.joint_values:main', + 'pick_and_place = crane_x7_examples_py.pick_and_place:main', + 'pose_groupstate = crane_x7_examples_py.pose_groupstate:main', ], }, )