From 677fa63d6d788501a8786f55300de74066d2b5b5 Mon Sep 17 00:00:00 2001
From: chama1176 <yakushigawa@rt-net.jp>
Date: Fri, 16 Aug 2024 15:30:57 +0900
Subject: [PATCH] =?UTF-8?q?=E3=82=B0=E3=83=AA=E3=83=83=E3=83=91=E3=83=BC?=
 =?UTF-8?q?=E9=96=8B=E9=96=89=E3=82=92=E7=A2=BA=E8=AA=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../pick_and_place_left_arm.py                | 87 +++++++++++++++++--
 1 file changed, 80 insertions(+), 7 deletions(-)

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 b732b64..d06b9e0 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
@@ -1,14 +1,29 @@
+# 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
 
 # generic ros libraries
 import rclpy
 from rclpy.logging import get_logger
+from geometry_msgs.msg import Pose, Point, Quaternion
 
 # moveit python library
 from moveit.core.robot_state import RobotState
 from moveit.planning import (
     MoveItPy,
-    MultiPipelinePlanRequestParameters,
+    PlanRequestParameters,
 )
 
 def plan_and_execute(
@@ -49,19 +64,77 @@ def main(args=None):
 
     # instantiate MoveItPy instance and get planning component
     sciurus17 = MoveItPy(node_name="pick_and_place_left_arm")
-    l_arm = sciurus17.get_planning_component("l_arm_group")
     logger.info("MoveItPy instance created")
+
+    # 左腕制御用 planning component
+    l_arm = sciurus17.get_planning_component("l_arm_group")
+    # 左グリッパ制御用 planning component
+    l_gripper = sciurus17.get_planning_component("l_gripper_group")
+
+    robot_model = sciurus17.get_robot_model()
     
-    # set plan start state using predefined state
-    l_arm.set_start_state_to_current_state()
+    plan_request_params = PlanRequestParameters(
+        sciurus17, 
+        "ompl_rrtc",
+    )
+    gripper_plan_request_params = PlanRequestParameters(
+        sciurus17, 
+        "ompl_rrtc",
+    )
+    # 動作速度の調整
+    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
 
-    # set pose goal using predefined state
+    # グリッパの開閉角
+    GRIPPER_CLOSE = math.radians(0.0)
+    GRIPPER_OPEN = math.radians(-40.0)
+    GRIPPER_GRASP = math.radians(-20.0)
+    # 物体を掴む位置
+    PICK_POSE = Pose(position=Point(x=0.25, y=0.0, z=0.12))
+    # 物体を置く位置
+    PLACE_POSE = Pose(position=Point(x=0.35, y=0.0, z=0.12))
+    # 物体を持ち上げる高さ
+    LIFTING_HEIFHT = 0.25
+    
+    # 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_arm, logger, sleep_time=3.0)
+    plan_and_execute(
+        sciurus17,
+        l_gripper, logger,
+        single_plan_parameters=gripper_plan_request_params,
+        sleep_time=3.0,
+    )
+    # 物体の上に腕を伸ばす
+    # 掴みに行く
+    # ハンドを閉じる
+    # 持ち上げる
+    # 移動する
+    # 下ろす
+    # ハンドを開く
+    # ハンドを持ち上げる
+    # SRDFに定義されている"l_arm_init_pose"の姿勢にする
+    # ハンドを閉じる
+
     
-    # related Issue
+    # Finishe with error. Related Issue
     # https://github.com/moveit/moveit2/issues/2693
     rclpy.shutdown()