diff --git a/README.md b/README.md index 2a3849f9..34e39a14 100644 --- a/README.md +++ b/README.md @@ -95,7 +95,10 @@ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' - CRANE+ V2のモデルデータ（xacro）を定義するパッケージです - crane_plus_examples - [README](./crane_plus_examples/README.md) - - CRANE+ V2のサンプルコード集です + - CRANE+ V2のC++サンプルコード集です +- crane_plus_examples_py + - [README](./crane_plus_examples_py/README.md) + - CRANE+ V2のPythonサンプルコード集です - crane_plus_gazebo - [README](./crane_plus_gazebo/README.md) - CRANE+ V2のGazeboシミュレーションパッケージです @@ -105,7 +108,12 @@ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' ## How to Use Examples -サンプルプログラムは、`crane_plus_examples`パッケージの[README](./crane_plus_description/README.md)参照してください。 +サンプルプログラムは、C++とPythonの両方を用意しています。詳しくは、以下のリンクをご覧ください。 + +- C++ + - [crane_plus_examples](./crane_plus_examples/README.md) +- Python + - [crane_plus_examples_py](./crane_plus_examples_py/README.md) ## License diff --git a/crane_plus_control/src/crane_plus_hardware.cpp b/crane_plus_control/src/crane_plus_hardware.cpp index b1b9e7f8..6514e000 100644 --- a/crane_plus_control/src/crane_plus_hardware.cpp +++ b/crane_plus_control/src/crane_plus_hardware.cpp @@ -205,7 +205,9 @@ return_type CranePlusHardware::read( RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); - return return_type::ERROR; + // readに失敗しても通信は継続させる。 + // 不確かなデータをセットしないようにOKを返す。 + return return_type::OK; } else { for (uint i = 0; i < hw_position_states_.size(); ++i) { hw_position_states_[i] = joint_positions[i]; @@ -271,7 +273,9 @@ return_type CranePlusHardware::write( RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); - return return_type::ERROR; + // writeに失敗しても通信は継続させる。 + // 不確かなデータをセットしないようにOKを返す。 + return return_type::OK; } prev_comm_timestamp_ = steady_clock_.now(); diff --git a/crane_plus_examples_py/README.md b/crane_plus_examples_py/README.md new file mode 100644 index 00000000..b93d7dea --- /dev/null +++ b/crane_plus_examples_py/README.md @@ -0,0 +1,197 @@ +# crane_plus_examples_py + +このパッケージはCRANE+ V2 ROS 2パッケージのPythonによるサンプルコード集です。 + +## Table of Contents + +- [crane\_plus\_examples\_py](#crane_plus_examples_py) + - [Table of Contents](#table-of-contents) + - [Setup](#setup) + - [How to Run Examples](#how-to-run-examples) + - [Examples](#examples) + - [gripper\_control](#gripper_control) + - [pose\_groupstate](#pose_groupstate) + - [joint\_values](#joint_values) + - [pick\_and\_place](#pick_and_place) + - [Camera Examples](#camera-examples) + - [aruco\_detection](#aruco_detection) + - [color\_detection](#color_detection) + +## Setup + +CRANE+ V2の起動方法は[crane_plus_examplesのREADME](../crane_plus_examples/README.md)を参照してください。 + +## How to Run Examples + +準備ができたらPythonによるサンプルプログラムを実行します。 +例えばグリッパを開閉するサンプルは次のコマンドで実行できます。 + +```sh +ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' +``` + +終了するときは`Ctrl+c`を入力します。 + +> [!NOTE] +> Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。 +> +> ```sh +> ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' use_sim_time:=true +> ``` + +## Examples + +`demo.launch.py`を実行している状態で各サンプルを実行できます。 + +- [gripper_control](#gripper_control) +- [pose_groupstate](#pose_groupstate) +- [joint_values](#joint_values) +- [pick_and_place](#pick_and_place) + +> [!NOTE] +> 実行できるサンプルの一覧は、`examples.launch.py`にオプション`-s`を付けて実行することで表示できます。 +> +> ```sh +> $ ros2 launch crane_plus_examples_py example.launch.py -s +> Arguments (pass arguments as ':='): +> +> 'example': +> Set an example executable name: [gripper_control, pose_groupstate, joint_values, pick_and_place] +> (default: 'gripper_control') +> ``` + +--- + +### gripper_control + +グリッパを開閉させるコード例です。 + +次のコマンドを実行します。 + +```sh +ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' +``` + + + +[back to example list](#examples) + +--- + +### pose_groupstate + +group_stateを使うコード例です。 + +SRDFファイル[crane_plus_moveit_config/config/crane_plus.srdf](../crane_plus_moveit_config/config/crane_plus.srdf) +に記載されている`home`と`vertical`の姿勢に移行します。 + +次のコマンドを実行します。 + +```sh +ros2 launch crane_plus_examples_py example.launch.py example:='pose_groupstate' +``` + + + +[back to example list](#examples) + +--- + +### joint_values + +アームのジョイント角度を１つずつ変更するコード例です。 + +次のコマンドを実行します。 + +```sh +ros2 launch crane_plus_examples_py example.launch.py example:='joint_values' +``` + + + +[back to example list](#examples) + +--- + +### pick_and_place + +モノを掴む・持ち上げる・運ぶ・置くコード例です。 + +次のコマンドを実行します。 + +```sh +ros2 launch crane_plus_examples_py example.launch.py example:='pick_and_place' +``` + + + +[back to example list](#examples) + +--- + +## Camera Examples + +Webカメラ搭載モデルのカメラを使用したサンプルコードです。 + +[crane_plus_examplesのREADME](../crane_plus_examples/README.md)に記載されている「Webカメラ搭載モデルを使用する場合」の手順に従って`demo.launch`を実行している状態で各サンプルを実行できます。 + +- [aruco\_detection](#aruco_detection) +- [color\_detection](#color_detection) + +> [!NOTE] +> 実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで確認できます。 +> +> ```sh +> ros2 launch crane_plus_examples_py camera_example.launch.py -s +> Arguments (pass arguments as ':='): +> +> 'example': +> Set an example executable name: [color_detection] +> (default: 'color_detection') +> ``` + +--- + +### aruco_detection + +モノに取り付けたArUcoマーカをカメラで検出し、マーカ位置に合わせて掴むコード例です。 +マーカは[aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷して、一辺50mmの立方体に取り付けて使用します。 + +検出されたマーカの位置姿勢はtfのフレームとして配信されます。 +tfの`frame_id`はマーカIDごとに異なりID0のマーカの`frame_id`は`target_0`になります。 +掴む対象は`target_0`に設定されています。 +マーカ検出には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。 + +次のコマンドを実行します。 + +```bash +ros2 launch crane_plus_examples_py camera_example.launch.py example:='aruco_detection' +``` + +#### Videos +[![crane_plus_aruco_detection_demo](https://rt-net.github.io/images/crane-plus/aruco_detection.gif)](https://youtu.be/m9dus6LCocc) + +[back to example list](#examples) + +--- + +### color_detection + +特定の色の物体を検出して掴むコード例です。 + +デフォルトでは赤い物体の位置をtfのフレームとして配信します。 +tfの`frame_id`は`target_0`です。 +色検出には[OpenCV](https://docs.opencv.org/4.x/db/d8e/tutorial_threshold.html)を使用しています。 + +次のコマンドを実行します。 + +```sh +ros2 launch crane_plus_examples_py camera_example.launch.py example:='color_detection' +``` + +#### Videos +[![crane_plus_color_detection_demo](https://rt-net.github.io/images/crane-plus/color_detection.gif)](https://youtu.be/Kn0eWA7sALY) + +[back to example list](#examples) + +--- diff --git a/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml b/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml new file mode 100644 index 00000000..bf3489ee --- /dev/null +++ b/crane_plus_examples_py/config/crane_plus_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: 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 diff --git a/crane_plus_examples_py/crane_plus_examples_py/__init__.py b/crane_plus_examples_py/crane_plus_examples_py/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py new file mode 100644 index 00000000..b43f77be --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -0,0 +1,106 @@ +# 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 +# +# 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 cv2 +from cv2 import aruco +from cv_bridge import CvBridge +from geometry_msgs.msg import TransformStamped +import numpy as np +import rclpy +from rclpy.node import Node +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import CameraInfo, Image +from tf2_ros import TransformBroadcaster + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__('aruco_detection') + self.image_subscription = self.create_subscription( + Image, 'image_raw', self.image_callback, 10 + ) + self.camera_info_subscription = self.create_subscription( + CameraInfo, 'camera_info', self.camera_info_callback, 10 + ) + self.tf_broadcaster = TransformBroadcaster(self) + + self.camera_info = None + + self.bridge = CvBridge() + + def image_callback(self, msg): + # 画像データをROSのメッセージからOpenCVの配列に変換 + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) + + if not self.camera_info: + return + + # ArUcoマーカのデータセットを読み込む + # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの + MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) + # マーカID + ids = [] + # 画像座標系上のマーカ頂点位置 + corners = [] + # マーカの検出 + corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) + if ids is None: + return + # マーカの検出数 + n_markers = len(ids) + # カメラパラメータ + CAMERA_MATRIX = np.array(self.camera_info.k).reshape(3, 3) + DIST_COEFFS = np.array(self.camera_info.d).reshape(1, 5) + # マーカ一辺の長さ 0.04 [m] + MARKER_LENGTH = 0.04 + # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS + ) + + # マーカの位置姿勢をtfで配信 + for i in range(n_markers): + # tfの配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = 'target_' + str(ids[i][0]) + t.transform.translation.x = tvecs[i][0][0] + t.transform.translation.y = tvecs[i][0][1] + t.transform.translation.z = tvecs[i][0][2] + # 回転ベクトルをクォータニオンに変換 + marker_orientation_rot = Rotation.from_rotvec(rvecs[i][0]) + marker_orientation_quat = marker_orientation_rot.as_quat() + t.transform.rotation.x = marker_orientation_quat[0] + t.transform.rotation.y = marker_orientation_quat[1] + t.transform.rotation.z = marker_orientation_quat[2] + t.transform.rotation.w = marker_orientation_quat[3] + self.tf_broadcaster.sendTransform(t) + + def camera_info_callback(self, msg): + self.camera_info = msg + + +def main(args=None): + rclpy.init(args=args) + + image_subscriber = ImageSubscriber() + rclpy.spin(image_subscriber) + + image_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py new file mode 100644 index 00000000..bf36f639 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -0,0 +1,136 @@ +# 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 +# +# 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 cv2 +from cv_bridge import CvBridge +from geometry_msgs.msg import TransformStamped +from image_geometry import PinholeCameraModel +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image +from tf2_ros import TransformBroadcaster + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__('color_detection') + self.image_subscription = self.create_subscription( + Image, 'image_raw', self.image_callback, 10 + ) + self.camera_info_subscription = self.create_subscription( + CameraInfo, 'camera_info', self.camera_info_callback, 10 + ) + self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) + self.tf_broadcaster = TransformBroadcaster(self) + self.camera_info = None + self.bridge = CvBridge() + + def image_callback(self, msg): + # カメラのパラメータを取得してから処理を行う + if not self.camera_info: + return + + # 赤い物体を検出するようにHSVの範囲を設定 + # 周囲の明るさ等の動作環境に合わせて調整 + LOW_H_1 = 0 + HIGH_H_1 = 10 + LOW_H_2 = 170 + HIGH_H_2 = 179 + LOW_S = 100 + HIGH_S = 255 + LOW_V = 100 + HIGH_V = 255 + + # ウェブカメラの画像を受け取る + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + + # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) + + # 画像の二値化 + img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) + img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) + + # マスク画像の合成 + img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) + + # ノイズ除去の処理 + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) + + # 穴埋めの処理 + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) + + # 画像の検出領域におけるモーメントを計算 + moment = cv2.moments(img_thresholded) + d_m01 = moment['m01'] + d_m10 = moment['m10'] + d_area = moment['m00'] + + # 検出した領域のピクセル数が10000より大きい場合 + if d_area > 10000: + # カメラモデル作成 + camera_model = PinholeCameraModel() + + # カメラのパラメータを設定 + camera_model.fromCameraInfo(self.camera_info) + + # 画像座標系における把持対象物の位置（2D） + pixel_x = d_m10 / d_area + pixel_y = d_m01 / d_area + point = (pixel_x, pixel_y) + + # 補正後の画像座標系における把持対象物の位置を取得（2D） + rect_point = camera_model.rectifyPoint(point) + + # カメラ座標系から見た把持対象物の方向（Ray）を取得する + ray = camera_model.projectPixelTo3dRay(rect_point) + + # カメラの高さを0.46[m]として把持対象物の位置を計算 + CAMERA_HEIGHT = 0.46 + object_position = [ + ray[0] * CAMERA_HEIGHT, + ray[1] * CAMERA_HEIGHT, + ray[2] * CAMERA_HEIGHT, + ] + + # 把持対象物の位置をTFに配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = 'target_0' + t.transform.translation.x = object_position[0] + t.transform.translation.y = object_position[1] + t.transform.translation.z = object_position[2] + self.tf_broadcaster.sendTransform(t) + + # 閾値による二値化画像を配信 + img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding='mono8') + self.image_thresholded_publisher.publish(img_thresholded_msg) + + def camera_info_callback(self, msg): + self.camera_info = msg + + +def main(args=None): + rclpy.init(args=args) + + image_subscriber = ImageSubscriber() + rclpy.spin(image_subscriber) + + image_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py new file mode 100644 index 00000000..38867001 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -0,0 +1,94 @@ +# 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 +# +# 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 math + +from crane_plus_examples_py.utils import plan_and_execute + +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +import rclpy +from rclpy.logging import get_logger + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('gripper_control') + + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='gripper_control') + logger.info('MoveItPy instance created') + + # グリッパ制御用 planning component + gripper = crane_plus.get_planning_component('gripper') + + # instantiate a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + + plan_request_params = PlanRequestParameters( + crane_plus, + '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 + + # gripperを閉じる + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(30.0)]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=plan_request_params, + ) + + # gripperを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(-30.0)]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=plan_request_params, + ) + + # gripperを0度にする + 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_plus, + gripper, + logger, + single_plan_parameters=plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py new file mode 100644 index 00000000..50eb7345 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -0,0 +1,102 @@ +# 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 +# +# 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 math + +from crane_plus_examples_py.utils import plan_and_execute + +from moveit.core.kinematic_constraints import construct_joint_constraint +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +import rclpy +from rclpy.logging import get_logger + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('joint_values') + + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='joint_values') + logger.info('MoveItPy instance created') + + # アーム制御用 planning component + arm = crane_plus.get_planning_component('arm_tcp') + + # instantiate a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + robot_state = RobotState(robot_model) + + plan_request_params = PlanRequestParameters( + crane_plus, + 'ompl_rrtc', + ) + + # 動作速度の調整 + # Set 0.0 ~ 1.0 + plan_request_params.max_acceleration_scaling_factor = 1.0 + plan_request_params.max_velocity_scaling_factor = 1.0 + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) + + joint_names = [ + 'crane_plus_joint1', + 'crane_plus_joint2', + 'crane_plus_joint3', + 'crane_plus_joint4', + ] + target_joint_value = math.radians(45) + + # 現在角度をベースに、目標角度を作成する + current_state = arm.get_start_state() + joint_values = current_state.get_joint_group_positions('arm_tcp') + + # jointのリストを辞書型の形式に変換する + joint_values_dict = dict(zip(joint_names, joint_values)) + + # 各関節角度を順番に-45[deg]に動かす + for joint_name in joint_names: + # 対象のjointに目標値を設定する + joint_values_dict[joint_name] = target_joint_value + robot_state.joint_positions = joint_values_dict + + joint_constraint = construct_joint_constraint( + robot_state=robot_state, + joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm_tcp'), + ) + + arm.set_start_state_to_current_state() + arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + + plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py new file mode 100644 index 00000000..ec331674 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -0,0 +1,306 @@ +# 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 +# +# 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 copy +import math + +from crane_plus_examples_py.utils import plan_and_execute + +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion + +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from moveit_msgs.msg import BoundingVolume, Constraints, OrientationConstraint, PositionConstraint + +import rclpy +from rclpy.logging import get_logger +from scipy.spatial.transform import Rotation +from shape_msgs.msg import SolidPrimitive + + +def set_goal_constraints(pose): + # 位置姿勢の許容誤差 + POSITION_TOLERANCE = 0.00001 + ORIENTATION_TOLERANCE = 0.0001 + + # 目標位置姿勢 + target_pose = PoseStamped() + target_pose.header.frame_id = 'crane_plus_base' + target_pose.pose = pose + + # 目標位置姿勢の制約設定 + goal_constraints = Constraints() + goal_constraints.name = 'tolerance_goal' + + # 位置の制約設定 + position_constraint = PositionConstraint() + position_constraint.header.frame_id = 'crane_plus_base' + position_constraint.link_name = 'crane_plus_link_tcp' + tolerance_region = BoundingVolume() + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.SPHERE + primitive.dimensions = [POSITION_TOLERANCE] + tolerance_region.primitives.append(primitive) + tolerance_region.primitive_poses.append(target_pose.pose) + position_constraint.constraint_region = tolerance_region + position_constraint.weight = 1.0 + + # 姿勢の制約設定 + orientation_constraint = OrientationConstraint() + orientation_constraint.header.frame_id = 'crane_plus_base' + orientation_constraint.link_name = 'crane_plus_link_tcp' + orientation_constraint.orientation = target_pose.pose.orientation + orientation_constraint.absolute_x_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_y_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_z_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.weight = 1.0 + + goal_constraints.position_constraints.append(position_constraint) + goal_constraints.orientation_constraints.append(orientation_constraint) + + # 制約設定を返す + return goal_constraints + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('pick_and_place') + + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='pick_and_place') + logger.info('MoveItPy instance created') + + # アーム制御用 planning component + arm = crane_plus.get_planning_component('arm_tcp') + # グリッパ制御用 planning component + gripper = crane_plus.get_planning_component('gripper') + + robot_model = crane_plus.get_robot_model() + + arm_plan_request_params = PlanRequestParameters( + crane_plus, + 'ompl_rrtc', + ) + gripper_plan_request_params = PlanRequestParameters( + crane_plus, + '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_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_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + # 物体を持ち上げる高さ + LIFTING_HEIGHT = 0.03 + # 物体の頭上の位置 + gripper_quat = Rotation.from_euler('xyz', [0.0, 90.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] + ) + ABOVE_POSE = Pose(position=Point(x=0.0, y=-0.21, z=0.17), orientation=gripper_quat_msg) + # 物体を掴む位置 + gripper_quat = Rotation.from_euler('xyz', [0.0, 180.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.0, y=-0.1, z=0.02), orientation=gripper_quat_msg) + PRE_AND_POST_GRASP_POSE = copy.deepcopy(GRASP_POSE) + PRE_AND_POST_GRASP_POSE.position.z = LIFTING_HEIGHT + # 物体を置く位置 + gripper_quat = Rotation.from_euler('xyz', [0.0, 90.0, 0.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] + ) + RELEASE_POSE = Pose(position=Point(x=0.25, y=0.0, z=0.06), orientation=gripper_quat_msg) + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperの開閉角を0度にする + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # 物体の上に腕を伸ばす + arm.set_start_state_to_current_state() + goal_constraints = set_goal_constraints(ABOVE_POSE) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + arm.set_start_state_to_current_state() + goal_constraints = set_goal_constraints(PRE_AND_POST_GRASP_POSE) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # 掴みに行く + arm.set_start_state_to_current_state() + goal_constraints = set_goal_constraints(GRASP_POSE) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) + plan_and_execute( + crane_plus, + 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_CLOSE]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # 持ち上げる + arm.set_start_state_to_current_state() + goal_constraints = set_goal_constraints(PRE_AND_POST_GRASP_POSE) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # 下ろす + arm.set_start_state_to_current_state() + goal_constraints = set_goal_constraints(RELEASE_POSE) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) + plan_and_execute( + crane_plus, + 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]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperの開閉角を0度にする + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py new file mode 100644 index 00000000..6dd60bed --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -0,0 +1,295 @@ +# 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 +# +# 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 math + +from crane_plus_examples_py.utils import plan_and_execute + +from geometry_msgs.msg import PoseStamped + +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from moveit_msgs.msg import ( + BoundingVolume, + Constraints, + JointConstraint, + OrientationConstraint, + PositionConstraint, +) + +import numpy as np + +import rclpy +from rclpy.node import Node +from scipy.spatial.transform import Rotation +from shape_msgs.msg import SolidPrimitive +from tf2_ros import TransformException, TransformListener, TransformStamped +from tf2_ros.buffer import Buffer + + +class PickAndPlaceTf(Node): + def __init__(self): + super().__init__('pick_and_place_tf') + self.logger = self.get_logger() + + # tf + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.tf_past = TransformStamped() + + # instantiate MoveItPy instance and get planning component + self.crane_plus = MoveItPy(node_name='moveit_py') + self.logger.info('MoveItPy instance created') + + # アーム制御用 planning component + self.arm = self.crane_plus.get_planning_component('arm_tcp') + # グリッパ制御用 planning component + self.gripper = self.crane_plus.get_planning_component('gripper') + + # instantiate a RobotState instance using the current robot model + self.robot_model = self.crane_plus.get_robot_model() + + self.arm_plan_request_params = PlanRequestParameters( + self.crane_plus, + 'ompl_rrtc', + ) + self.gripper_plan_request_params = PlanRequestParameters( + self.crane_plus, + 'ompl_rrtc', + ) + + # 動作速度の調整 + # Set 0.0 ~ 1.0 + self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 + self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 + + self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 + self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 + + # SRDFに定義されている'home'の姿勢にする + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(configuration_name='home') + plan_and_execute( + self.crane_plus, + self.arm, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + + # 可動範囲を制限する + constraints = Constraints() + constraints.name = 'arm_constraints' + + jointConstraint = JointConstraint() + jointConstraint.joint_name = 'crane_plus_joint1' + jointConstraint.position = 0.0 + jointConstraint.tolerance_above = math.radians(100) + jointConstraint.tolerance_below = math.radians(100) + jointConstraint.weight = 1.0 + constraints.joint_constraints.append(jointConstraint) + + jointConstraint.joint_name = 'crane_plus_joint3' + jointConstraint.position = 0.0 + jointConstraint.tolerance_above = math.radians(0) + jointConstraint.tolerance_below = math.radians(180) + jointConstraint.weight = 1.0 + constraints.joint_constraints.append(jointConstraint) + + self.arm.set_path_constraints(constraints) + + # 待機姿勢 + self._control_arm(0.0, 0.0, 0.3, 0, 0, 0.0) + + # Call on_timer function every second + self.timer = self.create_timer(0.5, self.on_timer) + + def on_timer(self): + # target_0のtf位置姿勢を取得 + try: + tf_msg = self.tf_buffer.lookup_transform( + 'crane_plus_base', 'target_0', rclpy.time.Time() + ) + except TransformException as ex: + self.logger.info(f'Could not transform base_link to target: {ex}') + return + + now_time = self.get_clock().now() + FILTERING_TIME = rclpy.duration.Duration(seconds=2) + STOP_TIME_THRESHOLD = rclpy.duration.Duration(seconds=3) + DISTANCE_THRESHOLD = 0.01 + + # 経過時間と停止時間を計算(nsec) + # 経過時間 + tf_time = rclpy.time.Time.from_msg(tf_msg.header.stamp) + TF_ELAPSED_TIME = now_time - tf_time + # 停止時間 + tf_past_time = rclpy.time.Time.from_msg(self.tf_past.header.stamp) + TF_STOP_TIME = now_time - tf_past_time + + # 現在時刻から2秒以内に受け取ったtfを使用 + if TF_ELAPSED_TIME < FILTERING_TIME: + tf_diff = np.linalg.norm( + [ + self.tf_past.transform.translation.x - tf_msg.transform.translation.x, + self.tf_past.transform.translation.y - tf_msg.transform.translation.y, + self.tf_past.transform.translation.z - tf_msg.transform.translation.z, + ] + ) + # 把持対象の位置が停止していることを判定 + if tf_diff < DISTANCE_THRESHOLD: + # 把持対象が3秒以上停止している場合ピッキング動作開始 + if TF_STOP_TIME > STOP_TIME_THRESHOLD: + self._picking(tf_msg.transform.translation) + else: + self.tf_past = tf_msg + + def _picking(self, target_position): + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + + # 何かを掴んでいた時のためにハンドを開く + self._control_gripper(GRIPPER_OPEN) + + # ロボット座標系（2D）の原点から見た把持対象物への角度を計算 + x = target_position.x + y = target_position.y + theta_rad = math.atan2(y, x) + theta_deg = math.degrees(theta_rad) + + # 把持対象物に正対する + self._control_arm(0.0, 0.0, 0.3, 0, 0, theta_deg) + + # 掴みに行く + if not self._control_arm(x, y, 0.04, 0, 90, theta_deg): + # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る + self._control_arm(0.0, 0.0, 0.3, 0, 0, 0) + return + + # ハンドを閉じる + self._control_gripper(GRIPPER_CLOSE) + + # 移動する + self._control_arm(0.12, 0.0, 0.17, 0, 90, 0) + + # 横へ移動する + self._control_arm(0.0, -0.12, 0.17, 0, 90, -90) + + # 下ろす + self._control_arm(0.0, -0.25, 0.05, 0, 90, -90) + + # ハンドを開く + self._control_gripper(GRIPPER_OPEN) + + # 少しだけハンドを持ち上げる + self._control_arm(0.0, -0.25, 0.10, 0, 90, -90) + + # 待機姿勢に戻る + self._control_arm(0.0, 0.0, 0.3, 0, 0, 0) + + # ハンドを閉じる + self._control_gripper(GRIPPER_DEFAULT) + + # グリッパ制御 + def _control_gripper(self, angle): + self.gripper.set_start_state_to_current_state() + robot_state = RobotState(self.robot_model) + robot_state.set_joint_group_positions('gripper', [angle]) + self.gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + self.crane_plus, + self.gripper, + self.logger, + single_plan_parameters=self.gripper_plan_request_params, + ) + + # アーム制御 + def _control_arm(self, x, y, z, roll, pitch, yaw): + # 位置姿勢の許容誤差 + POSITION_TOLERANCE = 0.00001 + ORIENTATION_TOLERANCE = 0.0001 + + # 目標位置姿勢 + target_pose = PoseStamped() + target_pose.header.frame_id = 'crane_plus_base' + target_pose.pose.position.x = x + target_pose.pose.position.y = y + target_pose.pose.position.z = z + rotation = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=True) + quat = rotation.as_quat() + target_pose.pose.orientation.x = quat[0] + target_pose.pose.orientation.y = quat[1] + target_pose.pose.orientation.z = quat[2] + target_pose.pose.orientation.w = quat[3] + + # 目標位置姿勢の制約設定 + goal_constraints = Constraints() + goal_constraints.name = 'tolerance_goal' + + # 位置の制約設定 + position_constraint = PositionConstraint() + position_constraint.header.frame_id = 'crane_plus_base' + position_constraint.link_name = 'crane_plus_link_tcp' + tolerance_region = BoundingVolume() + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.SPHERE + primitive.dimensions = [POSITION_TOLERANCE] + tolerance_region.primitives.append(primitive) + tolerance_region.primitive_poses.append(target_pose.pose) + position_constraint.constraint_region = tolerance_region + position_constraint.weight = 1.0 + + # 姿勢の制約設定 + orientation_constraint = OrientationConstraint() + orientation_constraint.header.frame_id = 'crane_plus_base' + orientation_constraint.link_name = 'crane_plus_link_tcp' + orientation_constraint.orientation = target_pose.pose.orientation + orientation_constraint.absolute_x_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_y_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_z_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.weight = 1.0 + + goal_constraints.position_constraints.append(position_constraint) + goal_constraints.orientation_constraints.append(orientation_constraint) + + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(motion_plan_constraints=[goal_constraints]) + result = plan_and_execute( + self.crane_plus, + self.arm, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + return result + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + pick_and_place_tf_node = PickAndPlaceTf() + + rclpy.spin(pick_and_place_tf_node) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + pick_and_place_tf_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py new file mode 100644 index 00000000..cef4555c --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -0,0 +1,82 @@ +# 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 +# +# 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. + +from crane_plus_examples_py.utils import plan_and_execute + +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +import rclpy +from rclpy.logging import get_logger + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('pose_groupstate') + + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='pose_groupstate') + logger.info('MoveItPy instance created') + + # アーム制御用 planning component + arm = crane_plus.get_planning_component('arm_tcp') + + plan_request_params = PlanRequestParameters( + crane_plus, + '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 + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py new file mode 100644 index 00000000..e97753af --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -0,0 +1,42 @@ +# 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 +# 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 + + +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + # plan to goal + logger.info('Planning trajectory') + if multi_plan_parameters is not None: + 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) + else: + plan_result = planning_component.plan() + + # execute the plan + if plan_result: + logger.info('Executing plan') + robot_trajectory = plan_result.trajectory + execute_result = robot.execute(robot_trajectory, controllers=[]) + else: + logger.error('Planning failed') + execute_result = False + + time.sleep(sleep_time) + return execute_result diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py new file mode 100644 index 00000000..35ea2af1 --- /dev/null +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -0,0 +1,91 @@ +# 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 +# +# 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. + +from ament_index_python.packages import get_package_share_directory +from crane_plus_description.robot_description_loader \ + import RobotDescriptionLoader +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + description_loader = RobotDescriptionLoader() + declare_loaded_description = DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() \ + in crane_plus_description.', + ) + + moveit_config = ( + MoveItConfigsBuilder('crane_plus') + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .moveit_cpp( + file_path=get_package_share_directory('crane_plus_examples_py') + + '/config/crane_plus_moveit_py_examples.yaml' + ) + .to_moveit_configs() + ) + + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } + + declare_example_name = DeclareLaunchArgument( + 'example', default_value='color_detection', + description=('Set an example executable name: ' + '[aruco_detection, color_detection]') + ) + + 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')}) + + picking_node = Node( + name='pick_and_place_tf', + package='crane_plus_examples_py', + executable='pick_and_place_tf', + output='screen', + parameters=[config_dict] + ) + + example_node = Node( + name=[LaunchConfiguration('example'), '_node'], + package='crane_plus_examples_py', + executable=LaunchConfiguration('example'), + output='screen', + parameters=[config_dict], + ) + + return LaunchDescription([ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + picking_node, + example_node + ]) diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py new file mode 100644 index 00000000..3915f40f --- /dev/null +++ b/crane_plus_examples_py/launch/example.launch.py @@ -0,0 +1,84 @@ +# 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 +# +# 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. + +from ament_index_python.packages import get_package_share_directory +from crane_plus_description.robot_description_loader \ + import RobotDescriptionLoader +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + description_loader = RobotDescriptionLoader() + declare_loaded_description = DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() \ + in crane_plus_description.', + ) + + moveit_config = ( + MoveItConfigsBuilder('crane_plus') + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .moveit_cpp( + file_path=get_package_share_directory('crane_plus_examples_py') + + '/config/crane_plus_moveit_py_examples.yaml' + ) + .to_moveit_configs() + ) + + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } + + declare_example_name = DeclareLaunchArgument( + 'example', + default_value='gripper_control', + description='Set an example executable name: \ + [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_plus_examples_py', + executable=LaunchConfiguration('example'), + output='screen', + parameters=[config_dict], + ) + + return LaunchDescription([ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + example_node + ]) diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml new file mode 100644 index 00000000..5b37710e --- /dev/null +++ b/crane_plus_examples_py/package.xml @@ -0,0 +1,29 @@ + + + + crane_plus_examples_py + 0.1.0 + Python examples of CRANE+ V2 ROS package + RT Corporation + Apache-2.0 + + Nozomi Mizoguchi + Atsushi Kuwagata + Yusuke Kato + + rclpy + std_msgs + moveit + moveit_py + python3-numpy + python3-scipy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/crane_plus_examples_py/resource/crane_plus_examples_py b/crane_plus_examples_py/resource/crane_plus_examples_py new file mode 100644 index 00000000..e69de29b diff --git a/crane_plus_examples_py/setup.cfg b/crane_plus_examples_py/setup.cfg new file mode 100644 index 00000000..54c93bce --- /dev/null +++ b/crane_plus_examples_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/crane_plus_examples_py +[install] +install_scripts=$base/lib/crane_plus_examples_py diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py new file mode 100644 index 00000000..352becd4 --- /dev/null +++ b/crane_plus_examples_py/setup.py @@ -0,0 +1,40 @@ +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'crane_plus_examples_py' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('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='RT Corporation', + maintainer_email='shop@rt-net.jp', + description='python examples of CRANE+ V2 ROS package', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'aruco_detection = crane_plus_examples_py.aruco_detection:main', + 'color_detection = crane_plus_examples_py.color_detection:main', + 'gripper_control = crane_plus_examples_py.gripper_control:main', + 'joint_values = crane_plus_examples_py.joint_values:main', + 'pick_and_place_tf = crane_plus_examples_py.pick_and_place_tf:main', + 'pick_and_place = crane_plus_examples_py.pick_and_place:main', + 'pose_groupstate= crane_plus_examples_py.pose_groupstate:main', + 'utils = crane_plus_examples_py.utils:main', + ], + }, +) diff --git a/crane_plus_examples_py/test/test_copyright.py b/crane_plus_examples_py/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/crane_plus_examples_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/crane_plus_examples_py/test/test_flake8.py b/crane_plus_examples_py/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/crane_plus_examples_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/crane_plus_examples_py/test/test_pep257.py b/crane_plus_examples_py/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/crane_plus_examples_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/crane_plus_moveit_config/launch/run_move_group.launch.py b/crane_plus_moveit_config/launch/run_move_group.launch.py old mode 100755 new mode 100644