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..c439aef --- /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: # 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 index d55d5e1..fed81ce 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -58,8 +58,8 @@ def generate_launch_description(): .trajectory_execution(file_path="config/moveit_controllers.yaml") .planning_pipelines(pipelines=["ompl"]) .moveit_cpp( - file_path=get_package_share_directory("sciurus17_moveit_config") - + "/config/moveit_cpp.yaml" + file_path=get_package_share_directory("sciurus17_examples_py") + + "/config/sciurus17_moveit_py_examples.yaml" ) .to_moveit_configs() ) 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 aa04dc6..a3a6ce0 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 @@ -38,7 +38,6 @@ def plan_and_execute( sleep_time=0.0, ): """Helper function to plan and execute a motion.""" - # plan to goal logger.info("Planning trajectory") if multi_plan_parameters is not None: plan_result = planning_component.plan( @@ -112,13 +111,11 @@ def main(args=None): # 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 to goal plan_and_execute( sciurus17, l_arm, logger, single_plan_parameters=plan_request_params, - # sleep_time=3.0, ) # 何かを掴んでいた時のためにハンドを開く @@ -126,13 +123,11 @@ def main(args=None): 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 to goal plan_and_execute( sciurus17, l_gripper, logger, single_plan_parameters=gripper_plan_request_params, - # sleep_time=3.0, ) # 物体の上に腕を伸ばす @@ -140,13 +135,11 @@ def main(args=None): 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 to goal plan_and_execute( sciurus17, l_arm, logger, single_plan_parameters=plan_request_params, - # sleep_time=3.0, ) # 掴みに行く @@ -154,13 +147,11 @@ def main(args=None): l_arm.set_goal_state(pose_stamped_msg=PoseStamped( header=Header(frame_id="base_link"), pose=GRASP_POSE), pose_link="l_link7") - # plan to goal plan_and_execute( sciurus17, l_arm, logger, single_plan_parameters=plan_request_params, - # sleep_time=3.0, ) # ハンドを閉じる @@ -168,13 +159,11 @@ def main(args=None): 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 to goal plan_and_execute( sciurus17, l_gripper, logger, single_plan_parameters=gripper_plan_request_params, - # sleep_time=3.0, ) # 持ち上げる @@ -182,13 +171,11 @@ def main(args=None): 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 to goal plan_and_execute( sciurus17, l_arm, logger, single_plan_parameters=plan_request_params, - # sleep_time=3.0, ) # 移動する @@ -196,13 +183,11 @@ def main(args=None): 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 to goal plan_and_execute( sciurus17, l_arm, logger, single_plan_parameters=plan_request_params, - # sleep_time=3.0, ) # 下ろす @@ -210,13 +195,11 @@ def main(args=None): l_arm.set_goal_state(pose_stamped_msg=PoseStamped( header=Header(frame_id="base_link"), pose=RELEASE_POSE), pose_link="l_link7") - # plan to goal plan_and_execute( sciurus17, l_arm, logger, single_plan_parameters=plan_request_params, - # sleep_time=3.0, ) # ハンドを開く @@ -224,13 +207,11 @@ def main(args=None): 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 to goal plan_and_execute( sciurus17, l_gripper, logger, single_plan_parameters=gripper_plan_request_params, - # sleep_time=3.0, ) # ハンドを持ち上げる @@ -238,25 +219,21 @@ def main(args=None): 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 to goal plan_and_execute( sciurus17, l_arm, logger, single_plan_parameters=plan_request_params, - # sleep_time=3.0, ) # 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 to goal plan_and_execute( sciurus17, l_arm, logger, single_plan_parameters=plan_request_params, - # sleep_time=3.0, ) # ハンドを閉じる @@ -264,13 +241,11 @@ def main(args=None): 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 to goal plan_and_execute( sciurus17, l_gripper, logger, single_plan_parameters=gripper_plan_request_params, - # sleep_time=3.0, ) # Finishe with error. Related Issue diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index c5b5772..51ad5b9 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -14,6 +14,7 @@ ['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,