ファイル構成を変更

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

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()
         )

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,165 +111,141 @@ 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,
     )
 
     # 何かを掴んでいた時のためにハンドを開く
     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 to goal
     plan_and_execute(
         sciurus17,
         l_gripper,
         logger,
         single_plan_parameters=gripper_plan_request_params,
-        # sleep_time=3.0,
     )
 
     # 物体の上に腕を伸ばす
     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")
-    # plan to goal
     plan_and_execute(
         sciurus17,
         l_arm,
         logger,
         single_plan_parameters=plan_request_params,
-        # sleep_time=3.0,
     )
 
     # 掴みに行く
     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")
-    # plan to goal
     plan_and_execute(
         sciurus17,
         l_arm,
         logger,
         single_plan_parameters=plan_request_params,
-        # sleep_time=3.0,
     )
 
     # ハンドを閉じる
     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 to goal
     plan_and_execute(
         sciurus17,
         l_gripper,
         logger,
         single_plan_parameters=gripper_plan_request_params,
-        # sleep_time=3.0,
     )
 
     # 持ち上げる
     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")
-    # plan to goal
     plan_and_execute(
         sciurus17,
         l_arm,
         logger,
         single_plan_parameters=plan_request_params,
-        # sleep_time=3.0,
     )
 
     # 移動する
     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")
-    # plan to goal
     plan_and_execute(
         sciurus17,
         l_arm,
         logger,
         single_plan_parameters=plan_request_params,
-        # sleep_time=3.0,
     )
 
     # 下ろす
     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")
-    # plan to goal
     plan_and_execute(
         sciurus17,
         l_arm,
         logger,
         single_plan_parameters=plan_request_params,
-        # sleep_time=3.0,
     )
 
     # ハンドを開く
     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 to goal
     plan_and_execute(
         sciurus17,
         l_gripper,
         logger,
         single_plan_parameters=gripper_plan_request_params,
-        # sleep_time=3.0,
     )
 
     # ハンドを持ち上げる
     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")
-    # 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,
     )
 
     # ハンドを閉じる
     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 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

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,