From 7fdbb478f5dcc44fbe624fb11c98c4609347d08c Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 8 Jul 2024 16:39:17 +0900 Subject: [PATCH 01/46] =?UTF-8?q?jazzy=E3=81=A7=E3=83=93=E3=83=AB=E3=83=89?= =?UTF-8?q?=E3=81=8C=E9=80=9A=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 219 +++++++++++---------- 1 file changed, 112 insertions(+), 107 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index cf93027..a824151 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -22,127 +22,132 @@ #include "geometry_msgs/msg/point_stamped.hpp" #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" using std::placeholders::_1; namespace sciurus17_examples { -ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) -: Node("color_detection", options) -{ - image_subscription_ = this->create_subscription( - "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); + ColorDetection::ColorDetection(const rclcpp::NodeOptions &options) + : Node("color_detection", options) + { + image_subscription_ = this->create_subscription( + "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); - image_annotated_publisher_ = - this->create_publisher("image_annotated", 10); + image_annotated_publisher_ = + this->create_publisher("image_annotated", 10); - object_point_publisher_ = - this->create_publisher("target_position", 10); -} + object_point_publisher_ = + this->create_publisher("target_position", 10); + } -void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) -{ - // オレンジ色の物体を検出するようにHSVの範囲を設定 - const int LOW_H = 5, HIGH_H = 20; - const int LOW_S = 120, HIGH_S = 255; - const int LOW_V = 120, HIGH_V = 255; - - // 画像全体の10%以上の大きさで映った物体を検出 - const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; - - auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); - - // 画像をRGBからHSVに変換 - cv::Mat img_hsv; - cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); - - // 画像の二値化 - cv::Mat img_thresholded; - cv::inRange( - img_hsv, - cv::Scalar(LOW_H, LOW_S, LOW_V), - cv::Scalar(HIGH_H, HIGH_S, HIGH_V), - img_thresholded); - - // ノイズ除去の処理 - cv::morphologyEx( - img_thresholded, - img_thresholded, - cv::MORPH_OPEN, - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); - - // 穴埋めの処理 - cv::morphologyEx( - img_thresholded, - img_thresholded, - cv::MORPH_CLOSE, - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); - - // 検出領域のみを描画 - cv::Mat img_annotated; - cv_img->image.copyTo(img_annotated, img_thresholded); - - // 二値化した領域の輪郭を取得 - std::vector> contours; - cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); - - if (contours.size()) { - // 最も面積の大きい領域を取得 - std::vector object_moments; - int max_area_i = -1; - int i = 0; - for (const auto & contour : contours) { - object_moments.push_back(cv::moments(contour)); - if (object_moments[max_area_i].m00 < object_moments[i].m00) { - max_area_i = i; + void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + // オレンジ色の物体を検出するようにHSVの範囲を設定 + const int LOW_H = 5, HIGH_H = 20; + const int LOW_S = 120, HIGH_S = 255; + const int LOW_V = 120, HIGH_V = 255; + + // 画像全体の10%以上の大きさで映った物体を検出 + const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; + + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + + // 画像をRGBからHSVに変換 + cv::Mat img_hsv; + cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); + + // 画像の二値化 + cv::Mat img_thresholded; + cv::inRange( + img_hsv, + cv::Scalar(LOW_H, LOW_S, LOW_V), + cv::Scalar(HIGH_H, HIGH_S, HIGH_V), + img_thresholded); + + // ノイズ除去の処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 穴埋めの処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_CLOSE, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 検出領域のみを描画 + cv::Mat img_annotated; + cv_img->image.copyTo(img_annotated, img_thresholded); + + // 二値化した領域の輪郭を取得 + std::vector> contours; + cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); + + if (contours.size()) + { + // 最も面積の大きい領域を取得 + std::vector object_moments; + int max_area_i = -1; + int i = 0; + for (const auto &contour : contours) + { + object_moments.push_back(cv::moments(contour)); + if (object_moments[max_area_i].m00 < object_moments[i].m00) + { + max_area_i = i; + } + i++; } - i++; - } - if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) { - // 画像座標系における物体検出位置（2D） - cv::Point2d object_point; - object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; - object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); - - // 検出領域と検出位置を描画 - const cv::Scalar ANNOTATE_COLOR(256, 0, 256); - const int ANNOTATE_THICKNESS = 4; - const int ANNOTATE_RADIUS = 10; - cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); - cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); - - // 画像の中心を原点とした検出位置に変換 - cv::Point2d translated_object_point; - translated_object_point.x = object_point.x - msg->width / 2.0; - translated_object_point.y = object_point.y - msg->height / 2.0; - - // 検出位置を-1.0 ~ 1.0に正規化 - cv::Point2d normalized_object_point_; - if (msg->width != 0 && msg->height != 0) { - normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); - normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); + if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) + { + // 画像座標系における物体検出位置（2D） + cv::Point2d object_point; + object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; + object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); + + // 検出領域と検出位置を描画 + const cv::Scalar ANNOTATE_COLOR(256, 0, 256); + const int ANNOTATE_THICKNESS = 4; + const int ANNOTATE_RADIUS = 10; + cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); + cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); + + // 画像の中心を原点とした検出位置に変換 + cv::Point2d translated_object_point; + translated_object_point.x = object_point.x - msg->width / 2.0; + translated_object_point.y = object_point.y - msg->height / 2.0; + + // 検出位置を-1.0 ~ 1.0に正規化 + cv::Point2d normalized_object_point_; + if (msg->width != 0 && msg->height != 0) + { + normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); + normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); + } + + // 検出位置を配信 + geometry_msgs::msg::PointStamped object_point_msg; + object_point_msg.header = msg->header; + object_point_msg.point.x = normalized_object_point_.x; + object_point_msg.point.y = normalized_object_point_.y; + object_point_publisher_->publish(object_point_msg); } - - // 検出位置を配信 - geometry_msgs::msg::PointStamped object_point_msg; - object_point_msg.header = msg->header; - object_point_msg.point.x = normalized_object_point_.x; - object_point_msg.point.y = normalized_object_point_.y; - object_point_publisher_->publish(object_point_msg); } - } - // 閾値による二値化画像を配信 - sensor_msgs::msg::Image::SharedPtr img_annotated_msg = - cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); - image_annotated_publisher_->publish(*img_annotated_msg); -} + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_annotated_msg = + cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); + image_annotated_publisher_->publish(*img_annotated_msg); + } -} // namespace sciurus17_examples +} // namespace sciurus17_examples #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection) From 40749ac921cb44ca43ddca7ba4b3606647d28d5e Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 9 Jul 2024 11:53:10 +0900 Subject: [PATCH 02/46] =?UTF-8?q?capabilities=E3=81=AB=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_moveit_config/launch/run_move_group.launch.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sciurus17_moveit_config/launch/run_move_group.launch.py b/sciurus17_moveit_config/launch/run_move_group.launch.py index 5612775..f58eae6 100644 --- a/sciurus17_moveit_config/launch/run_move_group.launch.py +++ b/sciurus17_moveit_config/launch/run_move_group.launch.py @@ -43,6 +43,10 @@ def generate_launch_description(): 'robot_description': LaunchConfiguration('loaded_description') } + moveit_config.move_group_capabilities = { + "capabilities": "" + } + # Move group ld.add_entity(generate_move_group_launch(moveit_config)) From 1491067bad8cd7398b7bf9fe1f838ac9f081eb64 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 9 Jul 2024 11:53:57 +0900 Subject: [PATCH 03/46] =?UTF-8?q?gz-sim=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_gazebo/gui/gui.config | 72 +++++++++---------- .../launch/sciurus17_with_table.launch.py | 14 ++-- sciurus17_gazebo/worlds/table.sdf | 12 ++-- 3 files changed, 49 insertions(+), 49 deletions(-) diff --git a/sciurus17_gazebo/gui/gui.config b/sciurus17_gazebo/gui/gui.config index 72437fb..574721b 100644 --- a/sciurus17_gazebo/gui/gui.config +++ b/sciurus17_gazebo/gui/gui.config @@ -28,11 +28,11 @@ - + 3D View false docked - + ogre2 scene @@ -44,82 +44,82 @@ - + floating 5 5 false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + World control false false @@ -132,7 +132,7 @@ - + true true @@ -143,7 +143,7 @@ - + World stats false false @@ -156,7 +156,7 @@ - + true true @@ -166,7 +166,7 @@ - + false 0 0 @@ -175,12 +175,12 @@ floating false #666666 - + - + false 250 0 @@ -189,12 +189,12 @@ floating false #666666 - + - + false 0 50 @@ -203,7 +203,7 @@ floating false #777777 - + false @@ -211,7 +211,7 @@ - + false 250 50 @@ -220,12 +220,12 @@ floating false #777777 - + - + false 300 50 @@ -234,21 +234,21 @@ floating false #777777 - + - + false docked - + - + false docked - + diff --git a/sciurus17_gazebo/launch/sciurus17_with_table.launch.py b/sciurus17_gazebo/launch/sciurus17_with_table.launch.py index c81988c..2fe2559 100644 --- a/sciurus17_gazebo/launch/sciurus17_with_table.launch.py +++ b/sciurus17_gazebo/launch/sciurus17_with_table.launch.py @@ -40,22 +40,22 @@ def generate_launch_description(): ) # PATHを追加で通さないとSTLファイルが読み込まれない - env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], - 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( + env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], + 'GZ_SIM_RESOURCE_PATH': os.path.dirname( get_package_share_directory('sciurus17_description'))} world_file = os.path.join( get_package_share_directory('sciurus17_gazebo'), 'worlds', 'table.sdf') gui_config = os.path.join( get_package_share_directory('sciurus17_gazebo'), 'gui', 'gui.config') # -r オプションで起動時にシミュレーションをスタートしないと、コントローラが起動しない - ign_gazebo = ExecuteProcess( - cmd=['ign gazebo -r', world_file, '--gui-config', gui_config], + gz_sim = ExecuteProcess( + cmd=['gz sim -r', world_file, '--gui-config', gui_config], output='screen', additional_env=env, shell=True ) - ignition_spawn_entity = Node( + gz_sim_spawn_entity = Node( package='ros_gz_sim', executable='create', output='screen', @@ -136,8 +136,8 @@ def generate_launch_description(): SetParameter(name='use_sim_time', value=True), declare_use_head_camera, declare_use_chest_camera, - ign_gazebo, - ignition_spawn_entity, + gz_sim, + gz_sim_spawn_entity, spawn_joint_state_broadcaster, spawn_right_arm_controller, spawn_right_gripper_controller, diff --git a/sciurus17_gazebo/worlds/table.sdf b/sciurus17_gazebo/worlds/table.sdf index 9572111..4a4ab9c 100644 --- a/sciurus17_gazebo/worlds/table.sdf +++ b/sciurus17_gazebo/worlds/table.sdf @@ -6,16 +6,16 @@ 1.0 + filename="gz-sim-physics-system" + name="gz::sim::systems::Physics"> + filename="gz-sim-user-commands-system" + name="gz::sim::systems::UserCommands"> + filename="gz-sim-scene-broadcaster-system" + name="gz::sim::systems::SceneBroadcaster"> From 45d2ccbe56c7e59402fc243d4883c7b8c7d71a30 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 12 Jul 2024 09:37:35 +0900 Subject: [PATCH 04/46] =?UTF-8?q?OMPL=E3=81=AE=E8=A8=AD=E5=AE=9A=E3=83=95?= =?UTF-8?q?=E3=82=A1=E3=82=A4=E3=83=AB=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/ompl_planning.yaml | 256 ++++++++++++++++++ 1 file changed, 256 insertions(+) create mode 100644 sciurus17_moveit_config/config/ompl_planning.yaml diff --git a/sciurus17_moveit_config/config/ompl_planning.yaml b/sciurus17_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..c700fa1 --- /dev/null +++ b/sciurus17_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,256 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +# To optionally use Ruckig for jerk-limited smoothing, add this line to the request adapters below +# default_planning_request_adapters/AddRuckigTrajectorySmoothing +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath +planner_configs: + APSConfigDefault: + type: geometric::AnytimePathShortening + shortcut: 1 # Attempt to shortcut all new solution paths + hybridize: 1 # Compute hybrid solution trajectories + max_hybrid_paths: 36 # Number of hybrid paths generated per iteration + num_planners: 8 # The number of default planners to use for planning + planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + FMTkConfigDefault: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMTkConfigDefault: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDSTkConfigDefault: + type: geometric::PDST + STRIDEkConfigDefault: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRTkConfigDefault: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRTkConfigDefault: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiESTkConfigDefault: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjESTkConfigDefault: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRMkConfigDefault: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstarkConfigDefault: + type: geometric::LazyPRMstar + SPARSkConfigDefault: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwokConfigDefault: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + TrajOptDefault: + type: geometric::TrajOpt + +l_arm_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + +l_arm_waist_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + +r_arm_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + +r_arm_waist_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault From 537cc7d56e3cb71f0e36858252e7fc391d043de1 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 17 Jul 2024 18:02:18 +0900 Subject: [PATCH 05/46] =?UTF-8?q?doc,=20ci=E3=82=92Jazzy=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/industrial_ci.yml | 2 +- README.en.md | 7 ++++--- README.md | 7 ++++--- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index e6ef27d..f951dcf 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -17,7 +17,7 @@ jobs: strategy: matrix: env: - - { ROS_DISTRO: humble, ROS_REPO: ros, BEFORE_RUN_TARGET_TEST_EMBED: "ici_with_unset_variables source /root/target_ws/install/setup.bash" } + - { ROS_DISTRO: jazzy, ROS_REPO: ros, BEFORE_RUN_TARGET_TEST_EMBED: "ici_with_unset_variables source /root/target_ws/install/setup.bash" } runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 diff --git a/README.en.md b/README.en.md index e2f3758..2cf6fab 100644 --- a/README.en.md +++ b/README.en.md @@ -25,6 +25,7 @@ ROS 2 package suite of Sciurus17. ## Supported ROS 2 distributions - Humble +- Jazzy ### ROS 1 @@ -37,9 +38,9 @@ ROS 2 package suite of Sciurus17. - [Product page](https://www.rt-net.jp/products/sciurus17) - [Web Shop](https://www.rt-shop.jp/index.php?main_page=product_info&products_id=3895&language=en) - Linux OS - - Ubuntu 22.04 + - Ubuntu 24.04 - ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html) + - [Jazzy Jalisco](https://docs.ros.org/en/jazzy/Installation.html) ## Installation @@ -47,7 +48,7 @@ ROS 2 package suite of Sciurus17. ```sh # Setup ROS environment -source /opt/ros/humble/setup.bash +source /opt/ros/jazzy/setup.bash # Download packages mkdir -p ~/ros2_ws/src diff --git a/README.md b/README.md index 2e826f8..e889b74 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,7 @@ ROS 2 package suite of Sciurus17. ## Supported ROS 2 distributions - Humble +- Jazzy ### ROS 1 @@ -38,9 +39,9 @@ ROS 2 package suite of Sciurus17. - [製品ページ](https://www.rt-net.jp/products/sciurus17) - [ウェブショップ](https://www.rt-shop.jp/index.php?main_page=product_info&products_id=3895) - Linux OS - - Ubuntu 22.04 + - Ubuntu 24.04 - ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html) + - [Jazzy Jalisco](https://docs.ros.org/en/jazzy/Installation.html) ## Installation @@ -48,7 +49,7 @@ ROS 2 package suite of Sciurus17. ```sh # Setup ROS environment -source /opt/ros/humble/setup.bash +source /opt/ros/jazzy/setup.bash # Download packages mkdir -p ~/ros2_ws/src From 3531db7c88c8374cdedfe823e063cdaf3bcf4a70 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 18 Jul 2024 09:37:34 +0900 Subject: [PATCH 06/46] =?UTF-8?q?Revert=20"jazzy=E3=81=A7=E3=83=93?= =?UTF-8?q?=E3=83=AB=E3=83=89=E3=81=8C=E9=80=9A=E3=82=8B=E3=82=88=E3=81=86?= =?UTF-8?q?=E3=81=AB=E4=BF=AE=E6=AD=A3"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 7fdbb478f5dcc44fbe624fb11c98c4609347d08c. --- sciurus17_examples/src/color_detection.cpp | 219 ++++++++++----------- 1 file changed, 107 insertions(+), 112 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index a824151..cf93027 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -22,132 +22,127 @@ #include "geometry_msgs/msg/point_stamped.hpp" #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" -#include "cv_bridge/cv_bridge.hpp" +#include "cv_bridge/cv_bridge.h" using std::placeholders::_1; namespace sciurus17_examples { - ColorDetection::ColorDetection(const rclcpp::NodeOptions &options) - : Node("color_detection", options) - { - image_subscription_ = this->create_subscription( - "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); +ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) +: Node("color_detection", options) +{ + image_subscription_ = this->create_subscription( + "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); - image_annotated_publisher_ = - this->create_publisher("image_annotated", 10); + image_annotated_publisher_ = + this->create_publisher("image_annotated", 10); - object_point_publisher_ = - this->create_publisher("target_position", 10); - } + object_point_publisher_ = + this->create_publisher("target_position", 10); +} - void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) - { - // オレンジ色の物体を検出するようにHSVの範囲を設定 - const int LOW_H = 5, HIGH_H = 20; - const int LOW_S = 120, HIGH_S = 255; - const int LOW_V = 120, HIGH_V = 255; - - // 画像全体の10%以上の大きさで映った物体を検出 - const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; - - auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); - - // 画像をRGBからHSVに変換 - cv::Mat img_hsv; - cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); - - // 画像の二値化 - cv::Mat img_thresholded; - cv::inRange( - img_hsv, - cv::Scalar(LOW_H, LOW_S, LOW_V), - cv::Scalar(HIGH_H, HIGH_S, HIGH_V), - img_thresholded); - - // ノイズ除去の処理 - cv::morphologyEx( - img_thresholded, - img_thresholded, - cv::MORPH_OPEN, - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); - - // 穴埋めの処理 - cv::morphologyEx( - img_thresholded, - img_thresholded, - cv::MORPH_CLOSE, - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); - - // 検出領域のみを描画 - cv::Mat img_annotated; - cv_img->image.copyTo(img_annotated, img_thresholded); - - // 二値化した領域の輪郭を取得 - std::vector> contours; - cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); - - if (contours.size()) - { - // 最も面積の大きい領域を取得 - std::vector object_moments; - int max_area_i = -1; - int i = 0; - for (const auto &contour : contours) - { - object_moments.push_back(cv::moments(contour)); - if (object_moments[max_area_i].m00 < object_moments[i].m00) - { - max_area_i = i; - } - i++; +void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) +{ + // オレンジ色の物体を検出するようにHSVの範囲を設定 + const int LOW_H = 5, HIGH_H = 20; + const int LOW_S = 120, HIGH_S = 255; + const int LOW_V = 120, HIGH_V = 255; + + // 画像全体の10%以上の大きさで映った物体を検出 + const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; + + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + + // 画像をRGBからHSVに変換 + cv::Mat img_hsv; + cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); + + // 画像の二値化 + cv::Mat img_thresholded; + cv::inRange( + img_hsv, + cv::Scalar(LOW_H, LOW_S, LOW_V), + cv::Scalar(HIGH_H, HIGH_S, HIGH_V), + img_thresholded); + + // ノイズ除去の処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 穴埋めの処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_CLOSE, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 検出領域のみを描画 + cv::Mat img_annotated; + cv_img->image.copyTo(img_annotated, img_thresholded); + + // 二値化した領域の輪郭を取得 + std::vector> contours; + cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); + + if (contours.size()) { + // 最も面積の大きい領域を取得 + std::vector object_moments; + int max_area_i = -1; + int i = 0; + for (const auto & contour : contours) { + object_moments.push_back(cv::moments(contour)); + if (object_moments[max_area_i].m00 < object_moments[i].m00) { + max_area_i = i; } + i++; + } - if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) - { - // 画像座標系における物体検出位置（2D） - cv::Point2d object_point; - object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; - object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); - - // 検出領域と検出位置を描画 - const cv::Scalar ANNOTATE_COLOR(256, 0, 256); - const int ANNOTATE_THICKNESS = 4; - const int ANNOTATE_RADIUS = 10; - cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); - cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); - - // 画像の中心を原点とした検出位置に変換 - cv::Point2d translated_object_point; - translated_object_point.x = object_point.x - msg->width / 2.0; - translated_object_point.y = object_point.y - msg->height / 2.0; - - // 検出位置を-1.0 ~ 1.0に正規化 - cv::Point2d normalized_object_point_; - if (msg->width != 0 && msg->height != 0) - { - normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); - normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); - } - - // 検出位置を配信 - geometry_msgs::msg::PointStamped object_point_msg; - object_point_msg.header = msg->header; - object_point_msg.point.x = normalized_object_point_.x; - object_point_msg.point.y = normalized_object_point_.y; - object_point_publisher_->publish(object_point_msg); + if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) { + // 画像座標系における物体検出位置（2D） + cv::Point2d object_point; + object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; + object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); + + // 検出領域と検出位置を描画 + const cv::Scalar ANNOTATE_COLOR(256, 0, 256); + const int ANNOTATE_THICKNESS = 4; + const int ANNOTATE_RADIUS = 10; + cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); + cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); + + // 画像の中心を原点とした検出位置に変換 + cv::Point2d translated_object_point; + translated_object_point.x = object_point.x - msg->width / 2.0; + translated_object_point.y = object_point.y - msg->height / 2.0; + + // 検出位置を-1.0 ~ 1.0に正規化 + cv::Point2d normalized_object_point_; + if (msg->width != 0 && msg->height != 0) { + normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); + normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); } - } - // 閾値による二値化画像を配信 - sensor_msgs::msg::Image::SharedPtr img_annotated_msg = - cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); - image_annotated_publisher_->publish(*img_annotated_msg); + // 検出位置を配信 + geometry_msgs::msg::PointStamped object_point_msg; + object_point_msg.header = msg->header; + object_point_msg.point.x = normalized_object_point_.x; + object_point_msg.point.y = normalized_object_point_.y; + object_point_publisher_->publish(object_point_msg); + } } -} // namespace sciurus17_examples + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_annotated_msg = + cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); + image_annotated_publisher_->publish(*img_annotated_msg); +} + +} // namespace sciurus17_examples #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection) From 43216c286b3c3be2e5cd04c177fd486e32e2ad93 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 18 Jul 2024 09:38:06 +0900 Subject: [PATCH 07/46] =?UTF-8?q?=E8=AA=A4=E3=81=A3=E3=81=A6=E3=83=95?= =?UTF-8?q?=E3=82=A9=E3=83=BC=E3=83=9E=E3=83=83=E3=82=BF=E3=83=BC=E3=82=92?= =?UTF-8?q?=E3=81=8B=E3=81=91=E3=81=A6=E3=81=97=E3=81=BE=E3=81=A3=E3=81=A6?= =?UTF-8?q?=E3=81=84=E3=81=9F=E3=81=AE=E3=81=A7=E4=BF=AE=E6=AD=A3=E3=81=97?= =?UTF-8?q?=E3=81=BE=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index cf93027..71da591 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/point_stamped.hpp" #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" using std::placeholders::_1; namespace sciurus17_examples From 3e9401e762b9099426de65cdfed0236871cab41b Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 18 Jul 2024 10:05:34 +0900 Subject: [PATCH 08/46] =?UTF-8?q?usb=20cam=E3=81=AE=E4=BE=9D=E5=AD=98?= =?UTF-8?q?=E3=83=91=E3=83=83=E3=82=B1=E3=83=BC=E3=82=B8=E3=81=AE=E3=83=90?= =?UTF-8?q?=E3=83=BC=E3=82=B8=E3=83=A7=E3=83=B3=E3=81=AB=E3=81=A4=E3=81=84?= =?UTF-8?q?=E3=81=A6=E8=A8=98=E8=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index e889b74..b6ac8de 100644 --- a/README.md +++ b/README.md @@ -46,6 +46,9 @@ ROS 2 package suite of Sciurus17. ## Installation ### Build from source +2024 July 18現在、カメラを使ったサンプルで使用しているusb_camを正常に実行するためには +aptに登録されているものよりも最新のimage_commonのバージョンを使用する必要があります +そのためsciurus17関連のパッケージと合わせてソースからビルドします ```sh # Setup ROS environment @@ -56,6 +59,8 @@ mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src git clone -b ros2 https://github.com/rt-net/sciurus17_ros.git git clone -b ros2 https://github.com/rt-net/sciurus17_description.git +# To run examples with camera +git clone -b 5.1.4 https://github.com/ros-perception/image_common.git # Install dependencies rosdep install -r -y -i --from-paths . From 5f62ad73c981e88512a63888ad73cf68cd432953 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 19 Jul 2024 14:03:53 +0900 Subject: [PATCH 09/46] =?UTF-8?q?=E3=83=AA=E3=83=AA=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=81=95=E3=82=8C=E3=82=8B=E3=81=BE=E3=81=A7=E3=83=90=E3=83=BC?= =?UTF-8?q?=E3=82=B8=E3=83=A7=E3=83=B3=E6=8C=87=E5=AE=9A=E3=82=92=E5=85=A5?= =?UTF-8?q?=E3=82=8C=E3=81=A6=E3=81=8A=E3=81=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index b6ac8de..3c29212 100644 --- a/README.md +++ b/README.md @@ -57,6 +57,7 @@ source /opt/ros/jazzy/setup.bash # Download packages mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src +git clone -b feature/support-jazzy https://github.com/rt-net/rt_manipulators_cpp.git git clone -b ros2 https://github.com/rt-net/sciurus17_ros.git git clone -b ros2 https://github.com/rt-net/sciurus17_description.git # To run examples with camera From adc8feac1a131c9e1c0cfd266277e0de4ae12d58 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 19 Jul 2024 14:04:16 +0900 Subject: [PATCH 10/46] =?UTF-8?q?two=20hands=E3=82=92ompl=E8=A8=AD?= =?UTF-8?q?=E5=AE=9A=E3=81=AB=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/ompl_planning.yaml | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/sciurus17_moveit_config/config/ompl_planning.yaml b/sciurus17_moveit_config/config/ompl_planning.yaml index c700fa1..f56aa98 100644 --- a/sciurus17_moveit_config/config/ompl_planning.yaml +++ b/sciurus17_moveit_config/config/ompl_planning.yaml @@ -254,3 +254,32 @@ r_arm_waist_group: - SPARSkConfigDefault - SPARStwokConfigDefault - TrajOptDefault + +two_arm_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + From e96d86a6d0f9097b55ea9a1461aa38f510cde478 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 19 Jul 2024 14:04:51 +0900 Subject: [PATCH 11/46] =?UTF-8?q?MoveitConfigsBuilder=E5=91=A8=E8=BE=BA?= =?UTF-8?q?=E3=81=AE=E8=A8=98=E8=BC=89=E3=82=92=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_moveit_config/launch/run_move_group.launch.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/sciurus17_moveit_config/launch/run_move_group.launch.py b/sciurus17_moveit_config/launch/run_move_group.launch.py index f58eae6..cceaf13 100644 --- a/sciurus17_moveit_config/launch/run_move_group.launch.py +++ b/sciurus17_moveit_config/launch/run_move_group.launch.py @@ -35,7 +35,12 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("sciurus17") - .planning_pipelines("ompl", ["ompl"]) + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .trajectory_execution(file_path="config/moveit_controllers.yaml") + .planning_pipelines(pipelines=["ompl"]) .to_moveit_configs() ) From 231905bad85eea41f78875bb2744081f5977135a Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 26 Jul 2024 17:09:45 +0900 Subject: [PATCH 12/46] add mock components support --- sciurus17_examples/README.md | 41 ++++++++++++++++++++---- sciurus17_examples/launch/demo.launch.py | 8 +++++ 2 files changed, 42 insertions(+), 7 deletions(-) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 58eb169..d9f87f2 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -9,6 +9,8 @@ - [3. move\_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する) - [準備 (Gazeboを使う場合)](#準備-gazeboを使う場合) - [1. move\_groupとGazeboを起動する](#1-move_groupとgazeboを起動する) + - [準備（Mock Componentsを使う場合）](#準備mock-componentsを使う場合) + - [1. move\_groupとcontrollerを起動する](#1-move_groupとcontrollerを起動する) - [サンプルプログラムを実行する](#サンプルプログラムを実行する) - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合) - [Examples](#examples) @@ -63,6 +65,19 @@ ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py use_head_camera:=false use_chest_camera:=false ``` +## 準備（Mock Componentsを使う場合） + +### 1. move_groupとcontrollerを起動する + +次のコマンドでmove_group (`sciurus17_moveit_config`)と +controller (`sciurus17_control`)を起動します。 + +```sh +ros2 launch sciurus17_examples demo.launch.py use_mock_components:=true +``` + +Mock Componentsではカメラを使ったサンプルを実行することはできません。 + ## サンプルプログラムを実行する 準備ができたらサンプルプログラムを実行します。 @@ -86,13 +101,25 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_ `demo.launch`を実行している状態で各サンプルを実行できます。 -- [gripper\_control](#gripper_control) -- [neck\_control](#neck_control) -- [waist\_control](#waist_control) -- [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) -- [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) -- [head\_camera\_tracking](#head_camera_tracking) -- [chest\_camera\_tracking](#chest_camera_tracking) +- [sciurus17\_examples](#sciurus17_examples) + - [準備（実機を使う場合）](#準備実機を使う場合) + - [1. Sciurus17本体をPCに接続する](#1-sciurus17本体をpcに接続する) + - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する) + - [3. move\_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する) + - [準備 (Gazeboを使う場合)](#準備-gazeboを使う場合) + - [1. move\_groupとGazeboを起動する](#1-move_groupとgazeboを起動する) + - [準備（Mock Componentsを使う場合）](#準備mock-componentsを使う場合) + - [1. move\_groupとcontrollerを起動する](#1-move_groupとcontrollerを起動する) + - [サンプルプログラムを実行する](#サンプルプログラムを実行する) + - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合) + - [Examples](#examples) + - [gripper\_control](#gripper_control) + - [neck\_control](#neck_control) + - [waist\_control](#waist_control) + - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) + - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) + - [head\_camera\_tracking](#head_camera_tracking) + - [chest\_camera\_tracking](#chest_camera_tracking) 実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。 diff --git a/sciurus17_examples/launch/demo.launch.py b/sciurus17_examples/launch/demo.launch.py index ee446ef..8c73ec8 100644 --- a/sciurus17_examples/launch/demo.launch.py +++ b/sciurus17_examples/launch/demo.launch.py @@ -55,11 +55,18 @@ def generate_launch_description(): 'manipulator_config.yaml' ) + declare_use_mock_components = DeclareLaunchArgument( + 'use_mock_components', + default_value='false', + description='Use mock_components or not.' + ) + description_loader = RobotDescriptionLoader() description_loader.port_name = LaunchConfiguration('port_name') description_loader.baudrate = LaunchConfiguration('baudrate') description_loader.timeout_seconds = '1.0' description_loader.manipulator_config_file_path = config_file_path + description_loader.use_mock_components = LaunchConfiguration('use_mock_components') description = description_loader.load() @@ -98,6 +105,7 @@ def generate_launch_description(): declare_baudrate, declare_use_head_camera, declare_use_chest_camera, + declare_use_mock_components, move_group, control_node, head_camera_node, From 3648242f50e6f15636df7b074284637723a0c4dc Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 14 Aug 2024 18:13:57 +0900 Subject: [PATCH 13/46] =?UTF-8?q?=E7=99=BB=E9=8C=B2=E4=BD=8D=E7=BD=AE?= =?UTF-8?q?=E3=81=B8=E3=81=AE=E7=A7=BB=E5=8B=95=E3=82=B5=E3=83=B3=E3=83=97?= =?UTF-8?q?=E3=83=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/example.launch.py | 93 +++++++++++++++++++ sciurus17_examples_py/package.xml | 23 +++++ .../resource/sciurus17_examples_py | 0 .../sciurus17_examples_py/__init__.py | 0 .../pick_and_place_left_arm.py | 70 ++++++++++++++ sciurus17_examples_py/setup.cfg | 4 + sciurus17_examples_py/setup.py | 30 ++++++ sciurus17_examples_py/test/test_copyright.py | 25 +++++ sciurus17_examples_py/test/test_flake8.py | 25 +++++ sciurus17_examples_py/test/test_pep257.py | 23 +++++ .../config/moveit_cpp.yaml | 26 ++++++ 11 files changed, 319 insertions(+) create mode 100644 sciurus17_examples_py/launch/example.launch.py create mode 100644 sciurus17_examples_py/package.xml create mode 100644 sciurus17_examples_py/resource/sciurus17_examples_py create mode 100644 sciurus17_examples_py/sciurus17_examples_py/__init__.py create mode 100755 sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py create mode 100644 sciurus17_examples_py/setup.cfg create mode 100644 sciurus17_examples_py/setup.py create mode 100644 sciurus17_examples_py/test/test_copyright.py create mode 100644 sciurus17_examples_py/test/test_flake8.py create mode 100644 sciurus17_examples_py/test/test_pep257.py create mode 100644 sciurus17_moveit_config/config/moveit_cpp.yaml diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py new file mode 100644 index 0000000..d55d5e1 --- /dev/null +++ b/sciurus17_examples_py/launch/example.launch.py @@ -0,0 +1,93 @@ +# 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 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(): + ld = LaunchDescription() + 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.' + ) + ) + + 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_moveit_config") + + "/config/moveit_cpp.yaml" + ) + .to_moveit_configs() + ) + + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } + + moveit_config.move_group_capabilities = { + "capabilities": "" + } + + declare_example_name = DeclareLaunchArgument( + 'example', default_value='pick_and_place_left_arm', + description=('Set an example executable name: ' + '[gripper_control, neck_control, waist_control,' + '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', + parameters=[moveit_config.to_dict()], + ) + + ld.add_action(declare_example_name) + ld.add_action(example_node) + + return ld diff --git a/sciurus17_examples_py/package.xml b/sciurus17_examples_py/package.xml new file mode 100644 index 0000000..aecc142 --- /dev/null +++ b/sciurus17_examples_py/package.xml @@ -0,0 +1,23 @@ + + + + sciurus17_examples_py + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + rclpy + std_msgs + moveit + moveit_py + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/sciurus17_examples_py/resource/sciurus17_examples_py b/sciurus17_examples_py/resource/sciurus17_examples_py new file mode 100644 index 0000000..e69de29 diff --git a/sciurus17_examples_py/sciurus17_examples_py/__init__.py b/sciurus17_examples_py/sciurus17_examples_py/__init__.py new file mode 100644 index 0000000..e69de29 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 new file mode 100755 index 0000000..b732b64 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py @@ -0,0 +1,70 @@ +import time + +# generic ros libraries +import rclpy +from rclpy.logging import get_logger + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + MultiPipelinePlanRequestParameters, +) + +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + 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( + 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 + robot.execute(robot_trajectory, controllers=[]) + else: + logger.error("Planning failed") + + time.sleep(sleep_time) + +def main(args=None): + rclpy.init(args=args) + 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") + l_arm = sciurus17.get_planning_component("l_arm_group") + logger.info("MoveItPy instance created") + + # set plan start state using predefined state + l_arm.set_start_state_to_current_state() + + # set pose goal using predefined state + l_arm.set_goal_state(configuration_name="l_arm_init_pose") + + # plan to goal + plan_and_execute(sciurus17, l_arm, logger, sleep_time=3.0) + + # related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/sciurus17_examples_py/setup.cfg b/sciurus17_examples_py/setup.cfg new file mode 100644 index 0000000..8ea28b9 --- /dev/null +++ b/sciurus17_examples_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sciurus17_examples_py +[install] +install_scripts=$base/lib/sciurus17_examples_py diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py new file mode 100644 index 0000000..c5b5772 --- /dev/null +++ b/sciurus17_examples_py/setup.py @@ -0,0 +1,30 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'sciurus17_examples_py' + +setup( + name=package_name, + version='0.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]*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ubuntu', + maintainer_email='ubuntu@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', + ], + }, +) diff --git a/sciurus17_examples_py/test/test_copyright.py b/sciurus17_examples_py/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/sciurus17_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/sciurus17_examples_py/test/test_flake8.py b/sciurus17_examples_py/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/sciurus17_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/sciurus17_examples_py/test/test_pep257.py b/sciurus17_examples_py/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/sciurus17_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/sciurus17_moveit_config/config/moveit_cpp.yaml b/sciurus17_moveit_config/config/moveit_cpp.yaml new file mode 100644 index 0000000..c439aef --- /dev/null +++ b/sciurus17_moveit_config/config/moveit_cpp.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 From 677fa63d6d788501a8786f55300de74066d2b5b5 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 16 Aug 2024 15:30:57 +0900 Subject: [PATCH 14/46] =?UTF-8?q?=E3=82=B0=E3=83=AA=E3=83=83=E3=83=91?= =?UTF-8?q?=E3=83=BC=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() From 4532f5598981e7163ba587b019b19cac8939c05d Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 16 Aug 2024 18:20:03 +0900 Subject: [PATCH 15/46] =?UTF-8?q?=E4=B8=80=E9=80=9A=E3=82=8A=E3=81=AE?= =?UTF-8?q?=E6=B5=81=E3=82=8C=E3=82=92=E5=86=8D=E7=8F=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../pick_and_place_left_arm.py | 185 +++++++++++++++--- 1 file changed, 162 insertions(+), 23 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 d06b9e0..aa04dc6 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 @@ -13,11 +13,13 @@ # 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, Point, Quaternion +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 @@ -26,6 +28,7 @@ PlanRequestParameters, ) + def plan_and_execute( robot, planning_component, @@ -39,12 +42,10 @@ def plan_and_execute( logger.info("Planning trajectory") if multi_plan_parameters is not None: plan_result = planning_component.plan( - multi_plan_parameters=multi_plan_parameters - ) + multi_plan_parameters=multi_plan_parameters) elif single_plan_parameters is not None: plan_result = planning_component.plan( - single_plan_parameters=single_plan_parameters - ) + single_plan_parameters=single_plan_parameters) else: plan_result = planning_component.plan() @@ -58,6 +59,7 @@ def plan_and_execute( time.sleep(sleep_time) + def main(args=None): rclpy.init(args=args) logger = get_logger("moveit_py.pick_and_place_left_arm") @@ -72,42 +74,51 @@ def main(args=None): l_gripper = sciurus17.get_planning_component("l_gripper_group") robot_model = sciurus17.get_robot_model() - + plan_request_params = PlanRequestParameters( - sciurus17, + sciurus17, "ompl_rrtc", ) gripper_plan_request_params = PlanRequestParameters( - sciurus17, + 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 + 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 + 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_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 - + # 物体を掴む位置 + 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 + 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 + PRE_AND_POST_RELEASE_POSE = copy.deepcopy(RELEASE_POSE) + PRE_AND_POST_RELEASE_POSE.position.z += LIFTING_HEIFHT + # 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, + l_arm, + logger, single_plan_parameters=plan_request_params, - sleep_time=3.0, + # sleep_time=3.0, ) # 何かを掴んでいた時のためにハンドを開く @@ -118,26 +129,154 @@ def main(args=None): # plan to goal plan_and_execute( sciurus17, - l_gripper, logger, + l_gripper, + logger, single_plan_parameters=gripper_plan_request_params, - sleep_time=3.0, + # 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 # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() From 7d556fe16450afa3a9625ed94b3e2efe38a53e13 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 16 Aug 2024 18:48:22 +0900 Subject: [PATCH 16/46] =?UTF-8?q?=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB?= =?UTF-8?q?=E6=A7=8B=E6=88=90=E3=82=92=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/sciurus17_moveit_py_examples.yaml | 26 +++++++++++++++++++ .../launch/example.launch.py | 4 +-- .../pick_and_place_left_arm.py | 25 ------------------ sciurus17_examples_py/setup.py | 1 + 4 files changed, 29 insertions(+), 27 deletions(-) create mode 100644 sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml 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, From c56f72ad0c2f5252fc178948c94cc062f5b5c8dd Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 19 Aug 2024 10:27:29 +0900 Subject: [PATCH 17/46] licence --- sciurus17_examples_py/launch/example.launch.py | 2 +- sciurus17_examples_py/package.xml | 8 ++++---- sciurus17_examples_py/setup.py | 10 +++++----- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index fed81ce..842aac7 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): } declare_example_name = DeclareLaunchArgument( - 'example', default_value='pick_and_place_left_arm', + '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]') diff --git a/sciurus17_examples_py/package.xml b/sciurus17_examples_py/package.xml index aecc142..c3ad876 100644 --- a/sciurus17_examples_py/package.xml +++ b/sciurus17_examples_py/package.xml @@ -2,10 +2,10 @@ sciurus17_examples_py - 0.0.0 - TODO: Package description - ubuntu - TODO: License declaration + 0.1.0 + python examples of Sciurus17 ROS package + RT Corporation + Apache License 2.0 rclpy std_msgs diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index 51ad5b9..ba4e9de 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version='0.0.0', + version='0.1.0', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', @@ -18,10 +18,10 @@ ], install_requires=['setuptools'], zip_safe=True, - maintainer='ubuntu', - maintainer_email='ubuntu@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', + maintainer='RT Corporation', + maintainer_email='shop@rt-net.jp', + description='python examples of Sciurus17 ROS package', + license='Apache License 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ From a14f513331b330162f4bbfc3e27e9d351da1963c Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 19 Aug 2024 10:53:42 +0900 Subject: [PATCH 18/46] add gripper_control example --- .../config/sciurus17_moveit_py_examples.yaml | 2 +- .../sciurus17_examples_py/gripper_control.py | 162 ++++++++++++++++++ .../pick_and_place_left_arm.py | 7 +- sciurus17_examples_py/setup.py | 1 + 4 files changed, 166 insertions(+), 6 deletions(-) create mode 100755 sciurus17_examples_py/sciurus17_examples_py/gripper_control.py diff --git a/sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml b/sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml index c439aef..a0879fa 100644 --- a/sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml +++ b/sciurus17_examples_py/config/sciurus17_moveit_py_examples.yaml @@ -16,7 +16,7 @@ plan_request_params: max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 -ompl_rrtc: # Namespace for individual plan request +ompl_rrtc_default: # 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 diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py new file mode 100755 index 0000000..1f259e7 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -0,0 +1,162 @@ +# 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 +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 +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + + +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + """Helper function to plan and execute a motion.""" + 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 + robot.execute(robot_trajectory, controllers=[]) + else: + logger.error("Planning failed") + + time.sleep(sleep_time) + + +def main(args=None): + rclpy.init(args=args) + 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") + + # 腕制御用 planning component + arm = sciurus17.get_planning_component("two_arm_group") + # 左グリッパ制御用 planning component + l_gripper = sciurus17.get_planning_component("l_gripper_group") + # 右グリッパ制御用 planning component + r_gripper = sciurus17.get_planning_component("r_gripper_group") + + robot_model = sciurus17.get_robot_model() + + plan_request_params = PlanRequestParameters( + sciurus17, + "ompl_rrtc_default", + ) + gripper_plan_request_params = PlanRequestParameters( + sciurus17, + "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 + # 動作速度の調整 + 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) + R_GRIPPER_OPEN = math.radians(40.0) + L_GRIPPER_CLOSE = math.radians(0.0) + L_GRIPPER_OPEN = math.radians(-40.0) + + # SRDFに定義されている"two_arm_init_pose"の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name="two_arm_init_pose") + plan_and_execute( + sciurus17, + arm, + logger, + single_plan_parameters=plan_request_params, + ) + + 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]) + l_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + l_gripper, + logger, + single_plan_parameters=gripper_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", [L_GRIPPER_CLOSE]) + l_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + l_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + 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]) + r_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + r_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + 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]) + r_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + r_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # Finishe with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 a3a6ce0..8f535b2 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 @@ -76,18 +76,15 @@ def main(args=None): plan_request_params = PlanRequestParameters( sciurus17, - "ompl_rrtc", + "ompl_rrtc_default", ) gripper_plan_request_params = PlanRequestParameters( sciurus17, - "ompl_rrtc", + "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 - # 動作速度の調整 - 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_CLOSE = math.radians(0.0) diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index ba4e9de..de5dcab 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -26,6 +26,7 @@ entry_points={ 'console_scripts': [ 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', + 'gripper_control = sciurus17_examples_py.gripper_control:main', ], }, ) From b80158e7d9018cb440e450d55f44650de8218064 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 20 Aug 2024 11:46:11 +0900 Subject: [PATCH 19/46] =?UTF-8?q?utils=E3=82=92=E5=88=86=E9=9B=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sciurus17_examples_py/gripper_control.py | 30 +----------------- .../pick_and_place_left_arm.py | 31 +------------------ .../sciurus17_examples_py/utils.py | 30 ++++++++++++++++++ 3 files changed, 32 insertions(+), 59 deletions(-) create mode 100644 sciurus17_examples_py/sciurus17_examples_py/utils.py diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py index 1f259e7..3e98188 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -28,35 +28,7 @@ PlanRequestParameters, ) - -def plan_and_execute( - robot, - planning_component, - logger, - single_plan_parameters=None, - multi_plan_parameters=None, - sleep_time=0.0, -): - """Helper function to plan and execute a motion.""" - 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 - robot.execute(robot_trajectory, controllers=[]) - else: - logger.error("Planning failed") - - time.sleep(sleep_time) +from sciurus17_examples_py.utils import plan_and_execute def main(args=None): 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 8f535b2..92ddc42 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 @@ -28,36 +28,7 @@ PlanRequestParameters, ) - -def plan_and_execute( - robot, - planning_component, - logger, - single_plan_parameters=None, - multi_plan_parameters=None, - sleep_time=0.0, -): - """Helper function to plan and execute a motion.""" - 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 - robot.execute(robot_trajectory, controllers=[]) - else: - logger.error("Planning failed") - - time.sleep(sleep_time) - +from sciurus17_examples_py.utils import plan_and_execute def main(args=None): rclpy.init(args=args) diff --git a/sciurus17_examples_py/sciurus17_examples_py/utils.py b/sciurus17_examples_py/sciurus17_examples_py/utils.py new file mode 100644 index 0000000..95903dc --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/utils.py @@ -0,0 +1,30 @@ +import time + +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + """Helper function to plan and execute a motion.""" + 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 + robot.execute(robot_trajectory, controllers=[]) + else: + logger.error("Planning failed") + + time.sleep(sleep_time) From 3d112032a18e40d291bea0114b80033da6c8569b Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 10 Oct 2024 13:10:45 +0900 Subject: [PATCH 20/46] =?UTF-8?q?=E5=B7=A6=E5=8F=B3=E8=85=95=E3=81=8C?= =?UTF-8?q?=E9=81=95=E3=81=A3=E3=81=9F=E3=81=AE=E3=81=A7=E5=8F=8D=E8=BB=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sciurus17_examples_py/gripper_control.py | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py index 3e98188..69a586d 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -80,47 +80,47 @@ def main(args=None): ) for _ in range(2): - l_gripper.set_start_state_to_current_state() + r_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions("l_gripper_group", [L_GRIPPER_OPEN]) - l_gripper.set_goal_state(robot_state=robot_state) + 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, - l_gripper, + r_gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - l_gripper.set_start_state_to_current_state() + r_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions("l_gripper_group", [L_GRIPPER_CLOSE]) - l_gripper.set_goal_state(robot_state=robot_state) + 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, - l_gripper, + r_gripper, logger, single_plan_parameters=gripper_plan_request_params, ) for _ in range(2): - r_gripper.set_start_state_to_current_state() + l_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions("r_gripper_group", [R_GRIPPER_OPEN]) - r_gripper.set_goal_state(robot_state=robot_state) + 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, - r_gripper, + l_gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - r_gripper.set_start_state_to_current_state() + l_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions("r_gripper_group", [R_GRIPPER_CLOSE]) - r_gripper.set_goal_state(robot_state=robot_state) + 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, - r_gripper, + l_gripper, logger, single_plan_parameters=gripper_plan_request_params, ) From 9cd5055cb634c8077a5d71835b997c7b205f555b Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 28 Nov 2024 11:35:27 +0900 Subject: [PATCH 21/46] =?UTF-8?q?merge=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 5 ----- 1 file changed, 5 deletions(-) diff --git a/README.md b/README.md index 7e08161..8fadce3 100644 --- a/README.md +++ b/README.md @@ -46,9 +46,6 @@ ROS 2 package suite of Sciurus17. ## Installation ### Build from source -2024 July 18現在、カメラを使ったサンプルで使用しているusb_camを正常に実行するためには -aptに登録されているものよりも最新のimage_commonのバージョンを使用する必要があります -そのためsciurus17関連のパッケージと合わせてソースからビルドします ```sh # Setup ROS environment @@ -60,8 +57,6 @@ cd ~/ros2_ws/src git clone -b ros2 https://github.com/rt-net/rt_manipulators_cpp.git git clone -b ros2 https://github.com/rt-net/sciurus17_ros.git git clone -b ros2 https://github.com/rt-net/sciurus17_description.git -# To run examples with camera -git clone -b 5.1.4 https://github.com/ros-perception/image_common.git # Install dependencies rosdep install -r -y -i --from-paths . From ab8fb76f8f6cae0e0e112e0058ccbc779776e5eb Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 28 Nov 2024 11:35:48 +0900 Subject: [PATCH 22/46] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E3=82=82?= =?UTF-8?q?=E3=81=AE=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/moveit_cpp.yaml | 26 ------------------- 1 file changed, 26 deletions(-) delete mode 100644 sciurus17_moveit_config/config/moveit_cpp.yaml diff --git a/sciurus17_moveit_config/config/moveit_cpp.yaml b/sciurus17_moveit_config/config/moveit_cpp.yaml deleted file mode 100644 index c439aef..0000000 --- a/sciurus17_moveit_config/config/moveit_cpp.yaml +++ /dev/null @@ -1,26 +0,0 @@ -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 From fe333466d2d5afbd9b398ff7218b10a67caa3008 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 28 Nov 2024 15:18:20 +0900 Subject: [PATCH 23/46] =?UTF-8?q?flake8=E9=80=9A=E3=82=8B=E3=82=88?= =?UTF-8?q?=E3=81=86=E3=81=AB=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/example.launch.py | 66 ++++--------- .../sciurus17_examples_py/gripper_control.py | 46 +++++---- .../pick_and_place_left_arm.py | 94 +++++++++---------- .../sciurus17_examples_py/utils.py | 14 ++- sciurus17_examples_py/setup.py | 8 +- 5 files changed, 95 insertions(+), 133 deletions(-) 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'], From dba255f240af2a9899c017b5d1eff959457de76a Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 28 Nov 2024 21:05:36 +0900 Subject: [PATCH 24/46] add neck sample --- sciurus17_examples_py/README.md | 183 ++++++++++++++++++ .../launch/example.launch.py | 2 +- .../sciurus17_examples_py/neck_control.py | 143 ++++++++++++++ sciurus17_examples_py/setup.py | 1 + 4 files changed, 328 insertions(+), 1 deletion(-) create mode 100644 sciurus17_examples_py/README.md create mode 100755 sciurus17_examples_py/sciurus17_examples_py/neck_control.py diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md new file mode 100644 index 0000000..163d5af --- /dev/null +++ b/sciurus17_examples_py/README.md @@ -0,0 +1,183 @@ +# sciurus17_examples + +このパッケージはSciurus17 ROS 2パッケージのPythonサンプルコード集です。 + +- [sciurus17\_examples](#sciurus17_examples) + - [準備（実機を使う場合）](#準備実機を使う場合) + - [1. Sciurus17本体をPCに接続する](#1-sciurus17本体をpcに接続する) + - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する) + - [3. move\_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する) + - [準備 (Gazeboを使う場合)](#準備-gazeboを使う場合) + - [1. move\_groupとGazeboを起動する](#1-move_groupとgazeboを起動する) + - [準備（Mock Componentsを使う場合）](#準備mock-componentsを使う場合) + - [1. move\_groupとcontrollerを起動する](#1-move_groupとcontrollerを起動する) + - [サンプルプログラムを実行する](#サンプルプログラムを実行する) + - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合) + - [Examples](#examples) + - [gripper\_control](#gripper_control) + - [neck\_control](#neck_control) + - [waist\_control](#waist_control) + - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) + - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) + + +## 準備（実機を使う場合） + +### 1. Sciurus17本体をPCに接続する +Sciurus17本体をPCに接続します。 +接続方法は製品マニュアルを参照してください。 + +**※Sciurus17本体が接触しないように、十分なスペースを確保してください。** + +### 2. USB通信ポートの接続を確認する + +USB通信ポートの設定については`sciurus17_control`の +[README](../sciurus17_control/README.md) +を参照してください。 + +**正しく設定できていない場合、Sciurus17が動作しないので注意してください** + +### 3. move_groupとcontrollerを起動する + +次のコマンドでmove_group (`sciurus17_moveit_config`)と +controller (`sciurus17_control`)を起動します。 + +```sh +ros2 launch sciurus17_examples demo.launch.py +``` + +## 準備 (Gazeboを使う場合) + +### 1. move_groupとGazeboを起動する + +次のコマンドでmove_group (`sciurus17_moveit_config`)と +Gazeboを起動します。 + +```sh +ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py +``` + +頭部カメラや胸部カメラのシミュレーションを行わない場合は、 +`use_head_camera`、`use_chest_camera`オプションを`false`に設定します。 + +```sh +ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py use_head_camera:=false use_chest_camera:=false +``` + +## 準備（Mock Componentsを使う場合） + +### 1. move_groupとcontrollerを起動する + +次のコマンドでmove_group (`sciurus17_moveit_config`)と +controller (`sciurus17_control`)を起動します。 + +```sh +ros2 launch sciurus17_examples demo.launch.py use_mock_components:=true +``` + +Mock Componentsではカメラを使ったサンプルを実行することはできません。 + +## サンプルプログラムを実行する + +準備ができたらサンプルプログラムを実行します。 +例えばグリッパを開閉するサンプルは次のコマンドで実行できます。 + +```sh +ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control' +``` + +終了するときは`Ctrl+c`を入力します。 + +### Gazeboでサンプルプログラムを実行する場合 + +Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。 + +```sh +ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control' use_sim_time:='true' +``` + +## Examples + +`demo.launch`を実行している状態で各サンプルを実行できます。 + +- [gripper\_control](#gripper_control) +- [neck\_control](#neck_control) +- [waist\_control](#waist_control) +- [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) +- [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) + +実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。 + +```sh +ros2 launch sciurus17_examples_py example.launch.py -s +``` + +### gripper_control + +ハンドを開閉させるコード例です。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples example.launch.py example:='gripper_control' +``` + +[back to example list](#examples) + +--- + +### neck_control + +首を上下左右へ動かすコード例です。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples example.launch.py example:='neck_control' +``` + +[back to example list](#examples) + +--- + +### waist_control + +腰を左右へひねる動作をするコード例です。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples example.launch.py example:='waist_control' +``` + +[back to example list](#examples) + +--- + +### pick_and_place_right_arm_waist + +右手でターゲットを掴んで動かすコード例です。腰の回転も使用します。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples example.launch.py example:='pick_and_place_right_arm_waist' +``` + +[back to example list](#examples) + +--- + +### pick_and_place_left_arm + +左手でターゲットを掴んで動かすコード例です。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples example.launch.py example:='pick_and_place_left_arm' +``` + +[back to example list](#examples) + +--- diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index 2b59ede..fca0d49 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -50,7 +50,7 @@ def generate_launch_description(): 'example', default_value='gripper_control', description=('Set an example executable name: ' - '[gripper_control, neck_control, waist_control,' + '[gripper_control, neck_control, waist_control, ' 'pick_and_place_right_arm_waist, pick_and_place_left_arm]')) example_node = Node( diff --git a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py new file mode 100755 index 0000000..3d14b77 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py @@ -0,0 +1,143 @@ +# 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 math + + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + 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.neck_control') + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name='neck_control') + logger.info('MoveItPy instance created') + + # 首制御用 planning component + neck = sciurus17.get_planning_component('neck_group') + planning_scene_monitor = sciurus17.get_planning_scene_monitor() + + robot_model = sciurus17.get_robot_model() + + plan_request_params = PlanRequestParameters( + sciurus17, + '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 + + # SRDFに定義されている'neck_init_pose'の姿勢にする + neck.set_start_state_to_current_state() + neck.set_goal_state(configuration_name='neck_init_pose') + plan_and_execute( + sciurus17, + neck, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在角度をベースに、目標角度を作成する + joint_values = [] + with planning_scene_monitor.read_only() as scene: + robot_state = scene.current_state + joint_values = robot_state.get_joint_group_positions('neck_group') + + joint_values[0] = math.radians(45.0) + neck.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + single_plan_parameters=plan_request_params, + ) + + joint_values[0] = math.radians(-45.0) + neck.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + single_plan_parameters=plan_request_params, + ) + + joint_values[0] = math.radians(0.0) + neck.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + single_plan_parameters=plan_request_params, + ) + + joint_values[1] = math.radians(45.0) + neck.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + single_plan_parameters=plan_request_params, + ) + + joint_values[1] = math.radians(-45.0) + neck.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('neck_group', joint_values) + neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + neck, + logger, + single_plan_parameters=plan_request_params, + ) + + neck.set_start_state_to_current_state() + neck.set_goal_state(configuration_name='neck_init_pose') + plan_and_execute( + sciurus17, + neck, + logger, + single_plan_parameters=plan_request_params, + ) + + # Finishe with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index d0e9372..c3c0b5e 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -25,6 +25,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'neck_control = sciurus17_examples_py.neck_control:main', 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', 'gripper_control = sciurus17_examples_py.gripper_control:main', ], From fd14a8f0ab612784b8e8c6da00019c554e3e6778 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 28 Nov 2024 21:06:47 +0900 Subject: [PATCH 25/46] fix format --- sciurus17_examples_py/sciurus17_examples_py/neck_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py index 3d14b77..be30fa5 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py @@ -124,7 +124,7 @@ def main(args=None): logger, single_plan_parameters=plan_request_params, ) - + neck.set_start_state_to_current_state() neck.set_goal_state(configuration_name='neck_init_pose') plan_and_execute( From 6cc7b78e46aad78d289944479f8143ec1591a6a3 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 28 Nov 2024 21:18:53 +0900 Subject: [PATCH 26/46] add waist sample --- .../sciurus17_examples_py/waist_control.py | 107 ++++++++++++++++++ sciurus17_examples_py/setup.py | 3 +- 2 files changed, 109 insertions(+), 1 deletion(-) create mode 100755 sciurus17_examples_py/sciurus17_examples_py/waist_control.py diff --git a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py new file mode 100755 index 0000000..4796f9f --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py @@ -0,0 +1,107 @@ +# 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 math + + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + 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.waist_control') + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name='waist_control') + logger.info('MoveItPy instance created') + + # 腰制御用 planning component + waist = sciurus17.get_planning_component('waist_group') + planning_scene_monitor = sciurus17.get_planning_scene_monitor() + + robot_model = sciurus17.get_robot_model() + + plan_request_params = PlanRequestParameters( + sciurus17, + '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 + + # SRDFに定義されている'waist_init_pose'の姿勢にする + waist.set_start_state_to_current_state() + waist.set_goal_state(configuration_name='waist_init_pose') + plan_and_execute( + sciurus17, + waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在角度をベースに、目標角度を作成する + joint_values = [] + with planning_scene_monitor.read_only() as scene: + robot_state = scene.current_state + joint_values = robot_state.get_joint_group_positions('waist_group') + + joint_values[0] = math.radians(45.0) + waist.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('waist_group', joint_values) + waist.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + waist, + logger, + single_plan_parameters=plan_request_params, + ) + + joint_values[0] = math.radians(-45.0) + waist.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('waist_group', joint_values) + waist.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + waist, + logger, + single_plan_parameters=plan_request_params, + ) + + waist.set_start_state_to_current_state() + waist.set_goal_state(configuration_name='waist_init_pose') + plan_and_execute( + sciurus17, + waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # Finishe with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index c3c0b5e..8915f0c 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -25,9 +25,10 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'gripper_control = sciurus17_examples_py.gripper_control:main', 'neck_control = sciurus17_examples_py.neck_control:main', + 'waist_control = sciurus17_examples_py.waist_control:main', 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', - 'gripper_control = sciurus17_examples_py.gripper_control:main', ], }, ) From 65036d679b60dbada16d1cf8e95a2e3de9f24daa Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 28 Nov 2024 21:55:36 +0900 Subject: [PATCH 27/46] add right arm waist sample --- .../pick_and_place_right_arm_waist.py | 232 ++++++++++++++++++ sciurus17_examples_py/setup.py | 2 + 2 files changed, 234 insertions(+) create mode 100755 sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py new file mode 100755 index 0000000..4a4c340 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -0,0 +1,232 @@ +# 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 copy +import math + +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 Constraints, JointConstraint +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_right_arm_waist') + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name='pick_and_place_right_arm_waist') + logger.info('MoveItPy instance created') + + # 右腕と腰制御用 planning component + r_arm_waist = sciurus17.get_planning_component('r_arm_waist_group') + # 右グリッパ制御用 planning component + r_gripper = sciurus17.get_planning_component('r_gripper_group') + + robot_model = sciurus17.get_robot_model() + + plan_request_params = PlanRequestParameters( + sciurus17, + 'ompl_rrtc_default', + ) + gripper_plan_request_params = PlanRequestParameters( + sciurus17, + '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 + 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_CLOSE = math.radians(0.0) + GRIPPER_OPEN = math.radians(40.0) + GRIPPER_GRASP = math.radians(20.0) + # 物体を持ち上げる高さ + 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 + 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 + PRE_AND_POST_RELEASE_POSE = copy.deepcopy(RELEASE_POSE) + PRE_AND_POST_RELEASE_POSE.position.z += LIFTING_HEIFHT + + # SRDFに定義されている'r_arm_waist_init_pose'の姿勢にする + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(configuration_name='r_arm_waist_init_pose') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # 何かを掴んでいた時のためにハンドを開く + r_gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('r_gripper_group', [GRIPPER_OPEN]) + r_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + r_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + constraints = Constraints() + joint_constraint = JointConstraint() + joint_constraint.joint_name = 'waist_yaw_joint' + joint_constraint.position = 0.0 + joint_constraint.tolerance_above = math.radians(45.0) + joint_constraint.tolerance_below = math.radians(45.0) + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + # 物体の上に腕を伸ばす + r_arm_waist.set_path_constraints(path_constraints=constraints) + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), + pose=PRE_AND_POST_GRASP_POSE), + pose_link='r_link7') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # 掴みに行く + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), + pose=GRASP_POSE), + pose_link='r_link7') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # ハンドを閉じる + r_gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('r_gripper_group', [GRIPPER_GRASP]) + r_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + r_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # 持ち上げる + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), + pose=PRE_AND_POST_GRASP_POSE), + pose_link='r_link7') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # 移動する + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), + pose=PRE_AND_POST_RELEASE_POSE), + pose_link='r_link7') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # 下ろす + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), + pose=RELEASE_POSE), + pose_link='r_link7') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # ハンドを開く + r_gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('r_gripper_group', [GRIPPER_OPEN]) + r_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + r_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # ハンドを持ち上げる + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), + pose=PRE_AND_POST_RELEASE_POSE), + pose_link='r_link7') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # SRDFに定義されている'r_arm_waist_init_pose'の姿勢にする + r_arm_waist.set_start_state_to_current_state() + r_arm_waist.set_goal_state(configuration_name='r_arm_waist_init_pose') + plan_and_execute( + sciurus17, + r_arm_waist, + logger, + single_plan_parameters=plan_request_params, + ) + + # ハンドを閉じる + r_gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('r_gripper_group', [GRIPPER_CLOSE]) + r_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + sciurus17, + r_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # Finishe with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index 8915f0c..8ee83fb 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -28,6 +28,8 @@ 'gripper_control = sciurus17_examples_py.gripper_control:main', 'neck_control = sciurus17_examples_py.neck_control:main', 'waist_control = sciurus17_examples_py.waist_control:main', + 'pick_and_place_right_arm_waist = \ + sciurus17_examples_py.pick_and_place_right_arm_waist:main', 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', ], }, From 8870b7d756cb6aa0487925a7fcd137d057b2278b Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 29 Nov 2024 09:44:16 +0900 Subject: [PATCH 28/46] =?UTF-8?q?README=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples_py/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 163d5af..4b57403 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -119,7 +119,7 @@ ros2 launch sciurus17_examples_py example.launch.py -s 次のコマンドを実行します。 ```sh -ros2 launch sciurus17_examples example.launch.py example:='gripper_control' +ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control' ``` [back to example list](#examples) @@ -133,7 +133,7 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' 次のコマンドを実行します。 ```sh -ros2 launch sciurus17_examples example.launch.py example:='neck_control' +ros2 launch sciurus17_examples_py example.launch.py example:='neck_control' ``` [back to example list](#examples) @@ -147,7 +147,7 @@ ros2 launch sciurus17_examples example.launch.py example:='neck_control' 次のコマンドを実行します。 ```sh -ros2 launch sciurus17_examples example.launch.py example:='waist_control' +ros2 launch sciurus17_examples_py example.launch.py example:='waist_control' ``` [back to example list](#examples) @@ -161,7 +161,7 @@ ros2 launch sciurus17_examples example.launch.py example:='waist_control' 次のコマンドを実行します。 ```sh -ros2 launch sciurus17_examples example.launch.py example:='pick_and_place_right_arm_waist' +ros2 launch sciurus17_examples_py example.launch.py example:='pick_and_place_right_arm_waist' ``` [back to example list](#examples) @@ -175,7 +175,7 @@ ros2 launch sciurus17_examples example.launch.py example:='pick_and_place_right_ 次のコマンドを実行します。 ```sh -ros2 launch sciurus17_examples example.launch.py example:='pick_and_place_left_arm' +ros2 launch sciurus17_examples_py example.launch.py example:='pick_and_place_left_arm' ``` [back to example list](#examples) From f930b5daed9feadb13dba655ae61b4a5bbece7ee Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 29 Nov 2024 09:50:14 +0900 Subject: [PATCH 29/46] fix copyright --- sciurus17_examples_py/launch/example.launch.py | 2 +- sciurus17_examples_py/sciurus17_examples_py/gripper_control.py | 2 +- sciurus17_examples_py/sciurus17_examples_py/neck_control.py | 2 +- .../sciurus17_examples_py/pick_and_place_left_arm.py | 2 +- .../sciurus17_examples_py/pick_and_place_right_arm_waist.py | 2 +- sciurus17_examples_py/sciurus17_examples_py/waist_control.py | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index fca0d49..024cfc2 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py index c0767cf..7e42b60 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -1,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py index be30fa5..a203049 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py @@ -1,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. 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 6db5e66..83ff72d 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,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py index 4a4c340..68eca99 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -1,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py index 4796f9f..a0c895a 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py @@ -1,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. From 040742acb72b41b33a0401de2adf082a75e85cd0 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 12:05:14 +0900 Subject: [PATCH 30/46] Update sciurus17_examples_py/sciurus17_examples_py/gripper_control.py Co-authored-by: Kuwamai --- sciurus17_examples_py/sciurus17_examples_py/gripper_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py index 7e42b60..4fda0f0 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -121,7 +121,7 @@ def main(args=None): single_plan_parameters=gripper_plan_request_params, ) - # Finishe with error. Related Issue + # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() From 0bba2e174cd94752a17eb06268c20f4078bf5e17 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 12:05:34 +0900 Subject: [PATCH 31/46] Update sciurus17_examples_py/sciurus17_examples_py/neck_control.py Co-authored-by: Kuwamai --- sciurus17_examples_py/sciurus17_examples_py/neck_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py index a203049..065b619 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py @@ -134,7 +134,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) - # Finishe with error. Related Issue + # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() From 9947152f5e4385e9794591da1c99aaebceb68de7 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 12:07:14 +0900 Subject: [PATCH 32/46] Update sciurus17_examples_py/README.md Co-authored-by: Kuwamai --- sciurus17_examples_py/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 4b57403..1714abc 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -2,7 +2,7 @@ このパッケージはSciurus17 ROS 2パッケージのPythonサンプルコード集です。 -- [sciurus17\_examples](#sciurus17_examples) +- [sciurus17\_examples\_py](#sciurus17_examples_py) - [準備（実機を使う場合）](#準備実機を使う場合) - [1. Sciurus17本体をPCに接続する](#1-sciurus17本体をpcに接続する) - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する) From cfd079c7e47c2e651a83e05749a15c2e9689247e Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 12:07:22 +0900 Subject: [PATCH 33/46] Update sciurus17_examples_py/README.md Co-authored-by: Kuwamai --- sciurus17_examples_py/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 1714abc..7f59f3a 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -1,4 +1,4 @@ -# sciurus17_examples +# sciurus17_examples_py このパッケージはSciurus17 ROS 2パッケージのPythonサンプルコード集です。 From 5e5a5295c2978379a660ef9ed8103b4defcd47e4 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 13:51:15 +0900 Subject: [PATCH 34/46] Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py Co-authored-by: Kuwamai --- .../sciurus17_examples_py/pick_and_place_right_arm_waist.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py index 68eca99..203b7b9 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -223,7 +223,7 @@ def main(args=None): single_plan_parameters=gripper_plan_request_params, ) - # Finishe with error. Related Issue + # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() From cbac82682f701c44d1fa9cb04052201a4336862a Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 13:51:44 +0900 Subject: [PATCH 35/46] Update sciurus17_examples_py/sciurus17_examples_py/waist_control.py Co-authored-by: Kuwamai --- sciurus17_examples_py/sciurus17_examples_py/waist_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py index a0c895a..8b96f67 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py @@ -98,7 +98,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) - # Finishe with error. Related Issue + # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() From 4ac913c81fd519e52ec541e9d0adb0a987e41dbe Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 13:52:02 +0900 Subject: [PATCH 36/46] Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py Co-authored-by: Kuwamai --- .../sciurus17_examples_py/pick_and_place_left_arm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 83ff72d..b22b9a3 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 @@ -210,7 +210,7 @@ def main(args=None): single_plan_parameters=gripper_plan_request_params, ) - # Finishe with error. Related Issue + # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() From 070dd63dc8b5a7ac7016f04a48bc964f55cb7fe3 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 13:54:25 +0900 Subject: [PATCH 37/46] Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py Co-authored-by: Kuwamai --- .../sciurus17_examples_py/pick_and_place_left_arm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b22b9a3..7135c5a 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 @@ -68,7 +68,7 @@ def main(args=None): 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 PRE_AND_POST_RELEASE_POSE = copy.deepcopy(RELEASE_POSE) - PRE_AND_POST_RELEASE_POSE.position.z += LIFTING_HEIFHT + PRE_AND_POST_RELEASE_POSE.position.z = LIFTING_HEIFHT # SRDFに定義されている'l_arm_init_pose'の姿勢にする l_arm.set_start_state_to_current_state() From 10461388f24db8ffd35a45e2298bd8e2807bc33f Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 13:54:39 +0900 Subject: [PATCH 38/46] Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py Co-authored-by: Kuwamai --- .../sciurus17_examples_py/pick_and_place_right_arm_waist.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py index 203b7b9..8ee8fe5 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -71,7 +71,7 @@ def main(args=None): 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 PRE_AND_POST_RELEASE_POSE = copy.deepcopy(RELEASE_POSE) - PRE_AND_POST_RELEASE_POSE.position.z += LIFTING_HEIFHT + PRE_AND_POST_RELEASE_POSE.position.z = LIFTING_HEIFHT # SRDFに定義されている'r_arm_waist_init_pose'の姿勢にする r_arm_waist.set_start_state_to_current_state() From a950e98a7a5a7e4c8bc6005df3f5d9ca9cc015e5 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 13:54:46 +0900 Subject: [PATCH 39/46] Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py Co-authored-by: Kuwamai --- .../sciurus17_examples_py/pick_and_place_right_arm_waist.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py index 8ee8fe5..49f2c29 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -66,7 +66,7 @@ def main(args=None): 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 PRE_AND_POST_GRASP_POSE = copy.deepcopy(GRASP_POSE) - PRE_AND_POST_GRASP_POSE.position.z += LIFTING_HEIFHT + 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 From 896b0499464fb3d98da691833ee296b4568cb06e Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 17 Dec 2024 13:55:44 +0900 Subject: [PATCH 40/46] Update sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py Co-authored-by: Kuwamai --- .../sciurus17_examples_py/pick_and_place_left_arm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7135c5a..5145280 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 @@ -63,7 +63,7 @@ def main(args=None): 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 PRE_AND_POST_GRASP_POSE = copy.deepcopy(GRASP_POSE) - PRE_AND_POST_GRASP_POSE.position.z += LIFTING_HEIFHT + 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 From 67ed12ec1881f1f30bc1db9b3ae71b774d3d9264 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 18 Dec 2024 11:08:38 +0900 Subject: [PATCH 41/46] =?UTF-8?q?=E3=83=AC=E3=83=93=E3=83=A5=E3=83=BC?= =?UTF-8?q?=E6=8C=87=E6=91=98=E4=BA=8B=E9=A0=85=E3=81=AB=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples_py/README.md | 69 ++----------------- .../launch/example.launch.py | 31 +++++---- .../sciurus17_examples_py/gripper_control.py | 2 + .../sciurus17_examples_py/neck_control.py | 6 ++ .../pick_and_place_left_arm.py | 42 ++++++----- .../pick_and_place_right_arm_waist.py | 42 ++++++----- .../sciurus17_examples_py/waist_control.py | 3 + 7 files changed, 81 insertions(+), 114 deletions(-) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 7f59f3a..78f2f77 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -2,15 +2,8 @@ このパッケージはSciurus17 ROS 2パッケージのPythonサンプルコード集です。 -- [sciurus17\_examples\_py](#sciurus17_examples_py) - - [準備（実機を使う場合）](#準備実機を使う場合) - - [1. Sciurus17本体をPCに接続する](#1-sciurus17本体をpcに接続する) - - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する) - - [3. move\_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する) - - [準備 (Gazeboを使う場合)](#準備-gazeboを使う場合) - - [1. move\_groupとGazeboを起動する](#1-move_groupとgazeboを起動する) - - [準備（Mock Componentsを使う場合）](#準備mock-componentsを使う場合) - - [1. move\_groupとcontrollerを起動する](#1-move_groupとcontrollerを起動する) +- [sciurus17\_examples](#sciurus17_examples) + - [起動方法](#起動方法) - [サンプルプログラムを実行する](#サンプルプログラムを実行する) - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合) - [Examples](#examples) @@ -20,62 +13,8 @@ - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) - -## 準備（実機を使う場合） - -### 1. Sciurus17本体をPCに接続する -Sciurus17本体をPCに接続します。 -接続方法は製品マニュアルを参照してください。 - -**※Sciurus17本体が接触しないように、十分なスペースを確保してください。** - -### 2. USB通信ポートの接続を確認する - -USB通信ポートの設定については`sciurus17_control`の -[README](../sciurus17_control/README.md) -を参照してください。 - -**正しく設定できていない場合、Sciurus17が動作しないので注意してください** - -### 3. move_groupとcontrollerを起動する - -次のコマンドでmove_group (`sciurus17_moveit_config`)と -controller (`sciurus17_control`)を起動します。 - -```sh -ros2 launch sciurus17_examples demo.launch.py -``` - -## 準備 (Gazeboを使う場合) - -### 1. move_groupとGazeboを起動する - -次のコマンドでmove_group (`sciurus17_moveit_config`)と -Gazeboを起動します。 - -```sh -ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py -``` - -頭部カメラや胸部カメラのシミュレーションを行わない場合は、 -`use_head_camera`、`use_chest_camera`オプションを`false`に設定します。 - -```sh -ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py use_head_camera:=false use_chest_camera:=false -``` - -## 準備（Mock Componentsを使う場合） - -### 1. move_groupとcontrollerを起動する - -次のコマンドでmove_group (`sciurus17_moveit_config`)と -controller (`sciurus17_control`)を起動します。 - -```sh -ros2 launch sciurus17_examples demo.launch.py use_mock_components:=true -``` - -Mock Componentsではカメラを使ったサンプルを実行することはできません。 +## 起動方法 +Sciurus17の起動方法は[sciurus17_examplesのREADME](../sciurus17_examples/README.md)を参照してください。 ## サンプルプログラムを実行する diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index 024cfc2..c5bd907 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -17,28 +17,25 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.actions import SetParameter from moveit_configs_utils import MoveItConfigsBuilder from sciurus17_description.robot_description_loader import RobotDescriptionLoader def generate_launch_description(): - ld = LaunchDescription() + description_loader = RobotDescriptionLoader() - - ld.add_action( - DeclareLaunchArgument('loaded_description', + declare_loaded_description = DeclareLaunchArgument('loaded_description', default_value=description_loader.load(), description='Set robot_description text. \ It is recommended to use RobotDescriptionLoader() \ - in sciurus17_description.')) + 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_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') @@ -53,6 +50,11 @@ def generate_launch_description(): '[gripper_control, neck_control, waist_control, ' 'pick_and_place_right_arm_waist, pick_and_place_left_arm]')) + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description=('Set true when using the gazebo simulator.') + ) + example_node = Node( name=[LaunchConfiguration('example'), '_node'], package='sciurus17_examples_py', @@ -61,7 +63,10 @@ def generate_launch_description(): parameters=[moveit_config.to_dict()], ) - ld.add_action(declare_example_name) - ld.add_action(example_node) - - return ld + return LaunchDescription([ + declare_loaded_description, + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + declare_example_name, + example_node + ]) diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py index 4fda0f0..8cb031d 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -75,6 +75,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 右グリッパ開閉 for _ in range(2): r_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -98,6 +99,7 @@ def main(args=None): single_plan_parameters=gripper_plan_request_params, ) + # 左グリッパ開閉 for _ in range(2): l_gripper.set_start_state_to_current_state() robot_state = RobotState(robot_model) diff --git a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py index 065b619..99fb541 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py @@ -65,6 +65,7 @@ def main(args=None): robot_state = scene.current_state joint_values = robot_state.get_joint_group_positions('neck_group') + # 首を左に向ける joint_values[0] = math.radians(45.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -77,6 +78,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 首を右に向ける joint_values[0] = math.radians(-45.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -89,6 +91,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 首を前に向ける joint_values[0] = math.radians(0.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -101,6 +104,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 首を上に向ける joint_values[1] = math.radians(45.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -113,6 +117,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 首を下に向ける joint_values[1] = math.radians(-45.0) neck.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -125,6 +130,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 'neck_init_pose'に戻す neck.set_start_state_to_current_state() neck.set_goal_state(configuration_name='neck_init_pose') plan_and_execute( 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 5145280..5c58b26 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 @@ -94,9 +94,10 @@ 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') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -106,9 +107,10 @@ 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') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = GRASP_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -130,9 +132,10 @@ 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') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -142,9 +145,10 @@ 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') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -154,9 +158,10 @@ 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') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = RELEASE_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, @@ -178,9 +183,10 @@ 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') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( sciurus17, l_arm, diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py index 49f2c29..5cff84e 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -107,9 +107,10 @@ def main(args=None): # 物体の上に腕を伸ばす r_arm_waist.set_path_constraints(path_constraints=constraints) r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_GRASP_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -119,9 +120,10 @@ def main(args=None): # 掴みに行く r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=GRASP_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = GRASP_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -143,9 +145,10 @@ def main(args=None): # 持ち上げる r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_GRASP_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_GRASP_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -155,9 +158,10 @@ def main(args=None): # 移動する r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_RELEASE_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -167,9 +171,10 @@ def main(args=None): # 下ろす r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=RELEASE_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = RELEASE_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, @@ -191,9 +196,10 @@ def main(args=None): # ハンドを持ち上げる r_arm_waist.set_start_state_to_current_state() - r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'), - pose=PRE_AND_POST_RELEASE_POSE), - pose_link='r_link7') + goal_pose = PoseStamped() + goal_pose.header.frame_id='base_link' + goal_pose.pose = PRE_AND_POST_RELEASE_POSE + r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( sciurus17, r_arm_waist, diff --git a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py index 8b96f67..73afdbb 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py @@ -65,6 +65,7 @@ def main(args=None): robot_state = scene.current_state joint_values = robot_state.get_joint_group_positions('waist_group') + # 腰を左に向ける joint_values[0] = math.radians(45.0) waist.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -77,6 +78,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 腰を右に向ける joint_values[0] = math.radians(-45.0) waist.set_start_state_to_current_state() robot_state = RobotState(robot_model) @@ -89,6 +91,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) + # 'waist_init_pose'に戻す waist.set_start_state_to_current_state() waist.set_goal_state(configuration_name='waist_init_pose') plan_and_execute( From b16cedeabdc2bf2767358f2d545de0d1a2ef8008 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 23 Dec 2024 11:46:59 +0900 Subject: [PATCH 42/46] =?UTF-8?q?=E6=8C=87=E6=91=98=E4=BA=8B=E9=A0=85?= =?UTF-8?q?=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/example.launch.py | 28 +++++++++++-------- .../pick_and_place_left_arm.py | 13 ++++----- .../pick_and_place_right_arm_waist.py | 13 ++++----- 3 files changed, 29 insertions(+), 25 deletions(-) diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index c5bd907..f459f57 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -20,22 +20,24 @@ from launch_ros.actions import SetParameter from moveit_configs_utils import MoveItConfigsBuilder from sciurus17_description.robot_description_loader import RobotDescriptionLoader +from launch.conditions import IfCondition 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 sciurus17_description.') + declare_loaded_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, - ).moveit_cpp(file_path=get_package_share_directory('sciurus17_examples_py') + - '/config/sciurus17_moveit_py_examples.yaml').to_moveit_configs()) + ).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') @@ -54,19 +56,23 @@ def generate_launch_description(): '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='sciurus17_examples_py', executable=LaunchConfiguration('example'), output='screen', - parameters=[moveit_config.to_dict()], + parameters=[config_dict], ) return LaunchDescription([ declare_loaded_description, declare_use_sim_time, - SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), declare_example_name, example_node ]) 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 5c58b26..f136799 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 @@ -23,7 +23,6 @@ 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): @@ -95,7 +94,7 @@ def main(args=None): # 物体の上に腕を伸ばす l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_GRASP_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -108,7 +107,7 @@ def main(args=None): # 掴みに行く l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = GRASP_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -133,7 +132,7 @@ def main(args=None): # 持ち上げる l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_GRASP_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -146,7 +145,7 @@ def main(args=None): # 移動する l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_RELEASE_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -159,7 +158,7 @@ def main(args=None): # 下ろす l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = RELEASE_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -184,7 +183,7 @@ def main(args=None): # ハンドを持ち上げる l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_RELEASE_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py index 5cff84e..bba7737 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -24,7 +24,6 @@ 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): @@ -108,7 +107,7 @@ def main(args=None): r_arm_waist.set_path_constraints(path_constraints=constraints) r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_GRASP_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -121,7 +120,7 @@ def main(args=None): # 掴みに行く r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = GRASP_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -146,7 +145,7 @@ def main(args=None): # 持ち上げる r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_GRASP_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -159,7 +158,7 @@ def main(args=None): # 移動する r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_RELEASE_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -172,7 +171,7 @@ def main(args=None): # 下ろす r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = RELEASE_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -197,7 +196,7 @@ def main(args=None): # ハンドを持ち上げる r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_RELEASE_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( From 96f2f826d6ec50ae791c6c86c02a81e200ea2033 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 23 Dec 2024 12:03:34 +0900 Subject: [PATCH 43/46] =?UTF-8?q?flake8=E9=80=9A=E3=82=8B=E3=82=88?= =?UTF-8?q?=E3=81=86=E3=81=AB=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples_py/launch/example.launch.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index f459f57..4c56024 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -17,10 +17,8 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.actions import SetParameter from moveit_configs_utils import MoveItConfigsBuilder from sciurus17_description.robot_description_loader import RobotDescriptionLoader -from launch.conditions import IfCondition def generate_launch_description(): @@ -60,7 +58,7 @@ def generate_launch_description(): # 下記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')}) + config_dict.update({'use_sim_time': LaunchConfiguration('use_sim_time')}) example_node = Node( name=[LaunchConfiguration('example'), '_node'], From 453edd838da285e9985f96af8f0dd9efb93193b1 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 24 Dec 2024 10:36:30 +0900 Subject: [PATCH 44/46] Update sciurus17_examples_py/package.xml Co-authored-by: Kuwamai --- sciurus17_examples_py/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/package.xml b/sciurus17_examples_py/package.xml index c3ad876..93d45c0 100644 --- a/sciurus17_examples_py/package.xml +++ b/sciurus17_examples_py/package.xml @@ -3,7 +3,7 @@ sciurus17_examples_py 0.1.0 - python examples of Sciurus17 ROS package + Python examples of Sciurus17 ROS package RT Corporation Apache License 2.0 From d985051ad53a3a86dd4f98723b520aaa1cb4c6e0 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 24 Dec 2024 11:00:39 +0900 Subject: [PATCH 45/46] add author tag --- sciurus17_examples_py/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sciurus17_examples_py/package.xml b/sciurus17_examples_py/package.xml index 93d45c0..1234c7f 100644 --- a/sciurus17_examples_py/package.xml +++ b/sciurus17_examples_py/package.xml @@ -7,6 +7,8 @@ RT Corporation Apache License 2.0 + chama1176 + rclpy std_msgs moveit From e1e67ba604a6548ded1baf8a8cd49a87026f9d9e Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 24 Dec 2024 13:51:46 +0900 Subject: [PATCH 46/46] =?UTF-8?q?README=E3=81=AB=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 8fadce3..ca97ad4 100644 --- a/README.md +++ b/README.md @@ -104,6 +104,9 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' - sciurus17_examples - [README](./sciurus17_examples/README.md) - Sciurus17のサンプルコード集です +- sciurus17_examples_py + - [README](./sciurus17_examples_py/README.md) + - Sciurus17のPythonサンプルコード集です - sciurus17_gazebo - Sciurus17のGazeboシミュレーションパッケージです - sciurus17_moveit_config