From c4ffa784d73f7fc29716589b1291865185c137eb Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 16 Jul 2025 17:55:16 +0900 Subject: [PATCH 1/7] =?UTF-8?q?=E5=B7=A6=E3=82=A2=E3=83=BC=E3=83=A0?= =?UTF-8?q?=E3=81=A7=E5=86=86=E9=81=8B=E5=8B=95=E3=82=92=E8=A1=8C=E3=81=86?= =?UTF-8?q?cpp=E3=83=97=E3=83=AD=E3=82=B0=E3=83=A9=E3=83=A0=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 1 + sciurus17_examples/launch/example.launch.py | 2 +- sciurus17_examples/src/cartesian_path.cpp | 108 ++++++++++++++++++++ 3 files changed, 110 insertions(+), 1 deletion(-) create mode 100644 sciurus17_examples/src/cartesian_path.cpp diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index d4ed8da6..654e9e6b 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -113,6 +113,7 @@ install( # Build and install node executables set(executable_list gripper_control + cartesian_path neck_control waist_control pick_and_place_right_arm_waist diff --git a/sciurus17_examples/launch/example.launch.py b/sciurus17_examples/launch/example.launch.py index b2fd1206..a82aa344 100644 --- a/sciurus17_examples/launch/example.launch.py +++ b/sciurus17_examples/launch/example.launch.py @@ -59,7 +59,7 @@ def generate_launch_description(): declare_example_name = DeclareLaunchArgument( 'example', default_value='gripper_control', description=('Set an example executable name: ' - '[gripper_control, neck_control, waist_control,' + '[gripper_control, cartesian_path, neck_control, waist_control,' 'pick_and_place_right_arm_waist, pick_and_place_left_arm]') ) diff --git a/sciurus17_examples/src/cartesian_path.cpp b/sciurus17_examples/src/cartesian_path.cpp new file mode 100644 index 00000000..92ff36bb --- /dev/null +++ b/sciurus17_examples/src/cartesian_path.cpp @@ -0,0 +1,108 @@ +// Copyright 2025 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Reference: +// https://github.com/ros-planning/moveit2_tutorials/blob +// /a547cf49ff7d1fe16a93dfe020c6027bcb035b51/doc/move_group_interface +// /src/move_group_interface_tutorial.cpp + +#include +#include + +#include "angles/angles.h" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "moveit/move_group_interface/move_group_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + + +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("cartesian_path"); + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options); + auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); + // For current state monitor + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_arm_node); + executor.add_node(move_group_gripper_node); + std::thread([&executor]() {executor.spin();}).detach(); + + MoveGroupInterface move_group_arm(move_group_arm_node, "l_arm_group"); + move_group_arm.setMaxVelocityScalingFactor(0.1); // Set 0.0 ~ 1.0 + move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + + MoveGroupInterface move_group_gripper(move_group_gripper_node, "l_gripper_group"); + move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); + + // SRDFに定義されている"l_arm_init_pose"の姿勢にする + move_group_arm.setNamedTarget("l_arm_init_pose"); + move_group_arm.move(); + + // ハンドを開く + gripper_joint_values[0] = angles::from_degrees(-40); + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + // 座標(x=0.3, y=0.0161 z=0.1)を中心に、XY平面上に半径0.1 mの円を3回描くように手先を動かす + std::vector waypoints; + float num_of_waypoints = 30; + int repeat = 3; + float radius = 0.1; + + geometry_msgs::msg::Point center_position; + center_position.x = 0.3; + center_position.y = 0.161; + center_position.z = 0.1; + + geometry_msgs::msg::Pose target_pose; + tf2::Quaternion q; + q.setRPY(angles::from_degrees(-90) ,0, 0); + target_pose.orientation = tf2::toMsg(q); + + for (int r = 0; r < repeat; r++) { + for (int i = 0; i < num_of_waypoints; i++) { + float theta = 2.0 * M_PI * (i / static_cast(num_of_waypoints)); + target_pose.position.x = center_position.x + radius * std::cos(theta); + target_pose.position.y = center_position.y + radius * std::sin(theta); + target_pose.position.z = center_position.z; + waypoints.push_back(target_pose); + } + } + + moveit_msgs::msg::RobotTrajectory trajectory; + const double eef_step = 0.01; + move_group_arm.computeCartesianPath(waypoints, eef_step, trajectory); + move_group_arm.execute(trajectory); + + // SRDFに定義されている"l_arm_init_pose"の姿勢にする + move_group_arm.setNamedTarget("l_arm_init_pose"); + move_group_arm.move(); + + // ハンドを閉じる + gripper_joint_values[0] = 0; + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + rclcpp::shutdown(); + return 0; +} From a0bcdca68905ad94e1140c201d0516512ebdd33f Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 16 Jul 2025 17:59:48 +0900 Subject: [PATCH 2/7] =?UTF-8?q?README=E3=81=AB=E3=82=B5=E3=83=B3=E3=83=97?= =?UTF-8?q?=E3=83=AB=E3=81=AE=E6=89=8B=E9=A0=86=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index e585da49..9cc5a652 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -15,6 +15,7 @@ - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合) - [Examples](#examples) - [gripper\_control](#gripper_control) + - [cartesian\_path](#cartesian_path) - [neck\_control](#neck_control) - [waist\_control](#waist_control) - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) @@ -131,6 +132,21 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' --- +### cartesian_path + +[Cartesian Path](https://moveit.picknik.ai/humble/doc/examples/move_group_interface/move_group_interface_tutorial.html#cartesian-paths) +を生成し、手先で円を描くコード例です。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples example.launch.py example:='cartesian_path' +``` + +[back to example list](#examples) + +--- + ### neck_control 首を上下左右へ動かすコード例です。 From 2fb5e4e251716dbc7c5ce6785da224216111c467 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 16 Jul 2025 18:21:02 +0900 Subject: [PATCH 3/7] =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88=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/src/cartesian_path.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/sciurus17_examples/src/cartesian_path.cpp b/sciurus17_examples/src/cartesian_path.cpp index 92ff36bb..581ca07c 100644 --- a/sciurus17_examples/src/cartesian_path.cpp +++ b/sciurus17_examples/src/cartesian_path.cpp @@ -27,7 +27,6 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; static const rclcpp::Logger LOGGER = rclcpp::get_logger("cartesian_path"); @@ -43,14 +42,14 @@ int main(int argc, char ** argv) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_arm_node); executor.add_node(move_group_gripper_node); - std::thread([&executor]() {executor.spin();}).detach(); + std::thread([&executor]() { executor.spin(); }).detach(); MoveGroupInterface move_group_arm(move_group_arm_node, "l_arm_group"); - move_group_arm.setMaxVelocityScalingFactor(0.1); // Set 0.0 ~ 1.0 + move_group_arm.setMaxVelocityScalingFactor(0.1); // Set 0.0 ~ 1.0 move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 MoveGroupInterface move_group_gripper(move_group_gripper_node, "l_gripper_group"); - move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); @@ -76,7 +75,7 @@ int main(int argc, char ** argv) geometry_msgs::msg::Pose target_pose; tf2::Quaternion q; - q.setRPY(angles::from_degrees(-90) ,0, 0); + q.setRPY(angles::from_degrees(-90), 0, 0); target_pose.orientation = tf2::toMsg(q); for (int r = 0; r < repeat; r++) { From 8e72219770d17eb6879857d30dcf248b4d803747 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 16 Jul 2025 18:31:03 +0900 Subject: [PATCH 4/7] =?UTF-8?q?ament=5Funcrustify=E3=81=AE=E3=83=95?= =?UTF-8?q?=E3=82=A9=E3=83=BC=E3=83=9E=E3=83=83=E3=83=88=E3=82=92=E9=81=A9?= =?UTF-8?q?=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/cartesian_path.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/src/cartesian_path.cpp b/sciurus17_examples/src/cartesian_path.cpp index 581ca07c..aab7f27f 100644 --- a/sciurus17_examples/src/cartesian_path.cpp +++ b/sciurus17_examples/src/cartesian_path.cpp @@ -42,7 +42,7 @@ int main(int argc, char ** argv) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_arm_node); executor.add_node(move_group_gripper_node); - std::thread([&executor]() { executor.spin(); }).detach(); + std::thread([&executor]() {executor.spin();}).detach(); MoveGroupInterface move_group_arm(move_group_arm_node, "l_arm_group"); move_group_arm.setMaxVelocityScalingFactor(0.1); // Set 0.0 ~ 1.0 From 2f743fd5d634d90da0a87f890fd039eaf7566f25 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Wed, 30 Jul 2025 13:22:14 +0900 Subject: [PATCH 5/7] =?UTF-8?q?[suggestion]MoveIt=E3=81=AE=E3=83=AA?= =?UTF-8?q?=E3=83=B3=E3=82=AF=E3=82=92humble=E3=81=8B=E3=82=89jazzy?= =?UTF-8?q?=E3=81=AB=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Kuwamai --- sciurus17_examples/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 9cc5a652..fc494993 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -134,7 +134,7 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' ### cartesian_path -[Cartesian Path](https://moveit.picknik.ai/humble/doc/examples/move_group_interface/move_group_interface_tutorial.html#cartesian-paths) +[Cartesian Path](https://moveit.picknik.ai/main/doc/examples/move_group_interface/move_group_interface_tutorial.html#cartesian-paths) を生成し、手先で円を描くコード例です。 次のコマンドを実行します。 From 2bf0478d5a9615b5e9e3373f040cc8ee6113994f Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 30 Jul 2025 13:26:35 +0900 Subject: [PATCH 6/7] =?UTF-8?q?cartesian=5Fpath=E3=81=AE=E8=AA=AC=E6=98=8E?= =?UTF-8?q?=E3=82=92Example=E3=81=AE=E7=9B=AE=E6=AC=A1=E3=81=AB=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index fc494993..544d6507 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -104,6 +104,7 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_ `demo.launch`を実行している状態で各サンプルを実行できます。 - [gripper\_control](#gripper_control) +- [cartesian\_path](#cartesian_path) - [neck\_control](#neck_control) - [waist\_control](#waist_control) - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) From 16b2dec667777e048c4b4ddfe635b15a33728b69 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 30 Jul 2025 16:33:59 +0900 Subject: [PATCH 7/7] =?UTF-8?q?=E6=8F=8F=E3=81=8F=E5=86=86=E3=81=AE?= =?UTF-8?q?=E4=B8=AD=E5=BF=83=E5=BA=A7=E6=A8=99=E3=82=92=E3=82=AD=E3=83=AA?= =?UTF-8?q?=E3=81=AE=E8=89=AF=E3=81=84=E6=95=B0=E5=80=A4=E3=81=AB=E5=A4=89?= =?UTF-8?q?=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/cartesian_path.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sciurus17_examples/src/cartesian_path.cpp b/sciurus17_examples/src/cartesian_path.cpp index aab7f27f..812e3962 100644 --- a/sciurus17_examples/src/cartesian_path.cpp +++ b/sciurus17_examples/src/cartesian_path.cpp @@ -62,7 +62,7 @@ int main(int argc, char ** argv) move_group_gripper.setJointValueTarget(gripper_joint_values); move_group_gripper.move(); - // 座標(x=0.3, y=0.0161 z=0.1)を中心に、XY平面上に半径0.1 mの円を3回描くように手先を動かす + // 座標(x=0.3, y=0.2 z=0.1)を中心に、XY平面上に半径0.1 mの円を3回描くように手先を動かす std::vector waypoints; float num_of_waypoints = 30; int repeat = 3; @@ -70,7 +70,7 @@ int main(int argc, char ** argv) geometry_msgs::msg::Point center_position; center_position.x = 0.3; - center_position.y = 0.161; + center_position.y = 0.2; center_position.z = 0.1; geometry_msgs::msg::Pose target_pose;