diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index 842aac7..2b59ede 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -12,28 +12,13 @@ # 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 sciurus17_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch_ros.actions import SetParameter from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -import yaml -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_move_group_launch -from moveit_configs_utils.launches import generate_moveit_rviz_launch -from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch -from moveit_configs_utils.launches import generate_rsp_launch from sciurus17_description.robot_description_loader import RobotDescriptionLoader -from ament_index_python.packages import get_package_share_directory - - -from ament_index_python.packages import get_package_share_directory def generate_launch_description(): @@ -41,49 +26,38 @@ def generate_launch_description(): 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 sciurus17_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, - ) - .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_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_config.robot_description = { 'robot_description': LaunchConfiguration('loaded_description') - } + } - moveit_config.move_group_capabilities = { - "capabilities": "" - } + moveit_config.move_group_capabilities = {'capabilities': ''} declare_example_name = DeclareLaunchArgument( - 'example', default_value='gripper_control', + '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]') - ) + 'pick_and_place_right_arm_waist, pick_and_place_left_arm]')) example_node = Node( name=[LaunchConfiguration('example'), '_node'], - package='sciurus17_examples_py', - executable=LaunchConfiguration('example'), - output='screen', + package='sciurus17_examples_py', + executable=LaunchConfiguration('example'), + output='screen', parameters=[moveit_config.to_dict()], ) diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py index 69a586d..c0767cf 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -11,15 +11,8 @@ # 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 @@ -27,41 +20,44 @@ 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") + 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") + sciurus17 = MoveItPy(node_name='gripper_control') + logger.info('MoveItPy instance created') # 腕制御用 planning component - arm = sciurus17.get_planning_component("two_arm_group") + arm = sciurus17.get_planning_component('two_arm_group') # 左グリッパ制御用 planning component - l_gripper = sciurus17.get_planning_component("l_gripper_group") + l_gripper = sciurus17.get_planning_component('l_gripper_group') # 右グリッパ制御用 planning component - r_gripper = sciurus17.get_planning_component("r_gripper_group") + r_gripper = sciurus17.get_planning_component('r_gripper_group') robot_model = sciurus17.get_robot_model() plan_request_params = PlanRequestParameters( sciurus17, - "ompl_rrtc_default", + 'ompl_rrtc_default', ) gripper_plan_request_params = PlanRequestParameters( sciurus17, - "ompl_rrtc_default", + '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 + 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 + 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) @@ -69,9 +65,9 @@ def main(args=None): L_GRIPPER_CLOSE = math.radians(0.0) L_GRIPPER_OPEN = math.radians(-40.0) - # SRDFに定義されている"two_arm_init_pose"の姿勢にする + # SRDFに定義されている'two_arm_init_pose'の姿勢にする arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name="two_arm_init_pose") + arm.set_goal_state(configuration_name='two_arm_init_pose') plan_and_execute( sciurus17, arm, @@ -82,7 +78,7 @@ def main(args=None): 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]) + 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, @@ -93,7 +89,7 @@ def main(args=None): 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]) + 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, @@ -105,7 +101,7 @@ def main(args=None): 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]) + 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, @@ -116,7 +112,7 @@ def main(args=None): 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]) + 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, 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 92ddc42..6db5e66 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 @@ -11,51 +11,47 @@ # 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 +import math -# 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 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 +from std_msgs.msg import Header + def main(args=None): rclpy.init(args=args) - logger = get_logger("moveit_py.pick_and_place_left_arm") + 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") + 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") + l_arm = sciurus17.get_planning_component('l_arm_group') # 左グリッパ制御用 planning component - l_gripper = sciurus17.get_planning_component("l_gripper_group") + l_gripper = sciurus17.get_planning_component('l_gripper_group') robot_model = sciurus17.get_robot_model() plan_request_params = PlanRequestParameters( sciurus17, - "ompl_rrtc_default", + 'ompl_rrtc_default', ) gripper_plan_request_params = PlanRequestParameters( sciurus17, - "ompl_rrtc_default", + '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 + 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) @@ -65,20 +61,18 @@ def main(args=None): 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 + 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 + 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"の姿勢にする + # SRDFに定義されている'l_arm_init_pose'の姿勢にする l_arm.set_start_state_to_current_state() - l_arm.set_goal_state(configuration_name="l_arm_init_pose") + l_arm.set_goal_state(configuration_name='l_arm_init_pose') plan_and_execute( sciurus17, l_arm, @@ -89,7 +83,7 @@ def main(args=None): # 何かを掴んでいた時のためにハンドを開く l_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions("l_gripper_group", [GRIPPER_OPEN]) + robot_state.set_joint_group_positions('l_gripper_group', [GRIPPER_OPEN]) l_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( sciurus17, @@ -100,9 +94,9 @@ 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") + 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') plan_and_execute( sciurus17, l_arm, @@ -112,20 +106,20 @@ 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") + l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), + pose=GRASP_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]) + robot_state.set_joint_group_positions('l_gripper_group', [GRIPPER_GRASP]) l_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( sciurus17, @@ -136,9 +130,9 @@ 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") + 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') plan_and_execute( sciurus17, l_arm, @@ -148,9 +142,9 @@ 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") + 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') plan_and_execute( sciurus17, l_arm, @@ -160,9 +154,9 @@ 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") + l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), + pose=RELEASE_POSE), + pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -173,7 +167,7 @@ def main(args=None): # ハンドを開く l_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions("l_gripper_group", [GRIPPER_OPEN]) + robot_state.set_joint_group_positions('l_gripper_group', [GRIPPER_OPEN]) l_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( sciurus17, @@ -184,9 +178,9 @@ 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") + 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') plan_and_execute( sciurus17, l_arm, @@ -194,9 +188,9 @@ def main(args=None): single_plan_parameters=plan_request_params, ) - # SRDFに定義されている"l_arm_init_pose"の姿勢にする + # SRDFに定義されている'l_arm_init_pose'の姿勢にする l_arm.set_start_state_to_current_state() - l_arm.set_goal_state(configuration_name="l_arm_init_pose") + l_arm.set_goal_state(configuration_name='l_arm_init_pose') plan_and_execute( sciurus17, l_arm, @@ -207,7 +201,7 @@ def main(args=None): # ハンドを閉じる l_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions("l_gripper_group", [GRIPPER_CLOSE]) + robot_state.set_joint_group_positions('l_gripper_group', [GRIPPER_CLOSE]) l_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( sciurus17, diff --git a/sciurus17_examples_py/sciurus17_examples_py/utils.py b/sciurus17_examples_py/sciurus17_examples_py/utils.py index 95903dc..94dda86 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/utils.py +++ b/sciurus17_examples_py/sciurus17_examples_py/utils.py @@ -1,5 +1,6 @@ import time + def plan_and_execute( robot, planning_component, @@ -8,23 +9,20 @@ def plan_and_execute( multi_plan_parameters=None, sleep_time=0.0, ): - """Helper function to plan and execute a motion.""" - logger.info("Planning trajectory") + logger.info('Planning trajectory') if multi_plan_parameters is not None: - plan_result = planning_component.plan( - multi_plan_parameters=multi_plan_parameters) + 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) + 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") + logger.info('Executing plan') robot_trajectory = plan_result.trajectory robot.execute(robot_trajectory, controllers=[]) else: - logger.error("Planning failed") + logger.error('Planning failed') time.sleep(sleep_time) diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index de5dcab..d0e9372 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -1,5 +1,5 @@ -import os from glob import glob +import os from setuptools import find_packages, setup @@ -10,10 +10,10 @@ 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, + 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))), ], install_requires=['setuptools'],