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/README.md b/sciurus17_examples/README.md index e585da49..544d6507 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) @@ -103,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) @@ -131,6 +133,21 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' --- +### cartesian_path + +[Cartesian Path](https://moveit.picknik.ai/main/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 首を上下左右へ動かすコード例です。 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..812e3962 --- /dev/null +++ b/sciurus17_examples/src/cartesian_path.cpp @@ -0,0 +1,107 @@ +// 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.2 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.2; + 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; +}