diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt
index d4ed8da6..7b51d925 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
+ pose_groupstate
neck_control
waist_control
pick_and_place_right_arm_waist
diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md
index e585da49..5990080b 100644
--- a/sciurus17_examples/README.md
+++ b/sciurus17_examples/README.md
@@ -15,6 +15,7 @@
- [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合)
- [Examples](#examples)
- [gripper\_control](#gripper_control)
+ - [pose\_groustate](#pose_groupstate)
- [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)
+- [pose\_groustate](#pose_groupstate)
- [neck\_control](#neck_control)
- [waist\_control](#waist_control)
- [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)
@@ -131,6 +133,22 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control'
---
+### pose_groupstate
+
+group_stateを使うコード例です。
+
+SRDFファイル[sciurus17_moveit_config/config/sciurus17.srdf](../sciurus17_moveit_config/config/sciurus17.srdf)に記載されている`two_arm_init_pose`と`two_arm_push_forward_pose`の姿勢に移行します。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples example.launch.py example:='pose_groupstate'
+```
+
+[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..b95ee44f 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, pose_groupstate, neck_control, waist_control,'
'pick_and_place_right_arm_waist, pick_and_place_left_arm]')
)
diff --git a/sciurus17_examples/src/pose_groupstate.cpp b/sciurus17_examples/src/pose_groupstate.cpp
new file mode 100644
index 00000000..2104ded9
--- /dev/null
+++ b/sciurus17_examples/src/pose_groupstate.cpp
@@ -0,0 +1,53 @@
+// 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
+// /5c15da709e9ea8529b54b313dc570f164f9a713e/doc/examples/subframes
+// /src/subframes_tutorial.cpp
+
+#include "moveit/move_group_interface/move_group_interface.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface;
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("pose_groupstate");
+
+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);
+ // For current state monitor
+ rclcpp::executors::SingleThreadedExecutor executor;
+ executor.add_node(move_group_arm_node);
+ std::thread([&executor]() {executor.spin();}).detach();
+
+ MoveGroupInterface move_group_arm(move_group_arm_node, "two_arm_group");
+ move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0
+ move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0
+
+ move_group_arm.setNamedTarget("two_arm_init_pose");
+ move_group_arm.move();
+
+ move_group_arm.setNamedTarget("two_arm_push_forward_pose");
+ move_group_arm.move();
+
+ move_group_arm.setNamedTarget("two_arm_init_pose");
+ move_group_arm.move();
+
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md
index 78f2f77c..75ccd822 100644
--- a/sciurus17_examples_py/README.md
+++ b/sciurus17_examples_py/README.md
@@ -8,6 +8,7 @@
- [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合)
- [Examples](#examples)
- [gripper\_control](#gripper_control)
+ - [pose\_groustate](#pose_groupstate)
- [neck\_control](#neck_control)
- [waist\_control](#waist_control)
- [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)
@@ -40,6 +41,7 @@ ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control' u
`demo.launch`を実行している状態で各サンプルを実行できます。
- [gripper\_control](#gripper_control)
+- [pose\_groustate](#pose_groupstate)
- [neck\_control](#neck_control)
- [waist\_control](#waist_control)
- [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)
@@ -65,6 +67,22 @@ ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control'
---
+### pose_groupstate
+
+group_stateを使うコード例です。
+
+SRDFファイル[sciurus17_moveit_config/config/sciurus17.srdf](../sciurus17_moveit_config/config/sciurus17.srdf)に記載されている`two_arm_init_pose`と`two_arm_push_forward_pose`の姿勢に移行します。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples_py example.launch.py example:='pose_groupstate'
+```
+
+[back to example list](#examples)
+
+---
+
### neck_control
首を上下左右へ動かすコード例です。
diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py
index 4c560243..b741bbe5 100644
--- a/sciurus17_examples_py/launch/example.launch.py
+++ b/sciurus17_examples_py/launch/example.launch.py
@@ -47,7 +47,7 @@ def generate_launch_description():
'example',
default_value='gripper_control',
description=('Set an example executable name: '
- '[gripper_control, neck_control, waist_control, '
+ '[gripper_control, pose_groupstate, neck_control, waist_control, '
'pick_and_place_right_arm_waist, pick_and_place_left_arm]'))
declare_use_sim_time = DeclareLaunchArgument(
diff --git a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py
new file mode 100644
index 00000000..036c21fb
--- /dev/null
+++ b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py
@@ -0,0 +1,81 @@
+# Copyright 2025 RT Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from moveit.planning import MoveItPy
+from moveit.planning import PlanRequestParameters
+
+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('pose_groupstate')
+
+ # instantiate MoveItPy instance and get planning component
+ sciurus17 = MoveItPy(node_name='pose_groupstate')
+ logger.info('MoveItPy instance created')
+
+ # アーム制御用 planning component
+ arm = sciurus17.get_planning_component('two_arm_group')
+
+ arm_plan_request_params = PlanRequestParameters(
+ sciurus17,
+ 'ompl_rrtc_default',
+ )
+
+ # 動作速度の調整
+ # Set 0.0 ~ 1.0
+ arm_plan_request_params.max_acceleration_scaling_factor = 1.0
+ arm_plan_request_params.max_velocity_scaling_factor = 1.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=arm_plan_request_params,
+ )
+
+ # SRDFに定義されている'two_arm_push_forward_pose'の姿勢にする
+ arm.set_start_state_to_current_state()
+ arm.set_goal_state(configuration_name='two_arm_push_forward_pose')
+ plan_and_execute(
+ sciurus17,
+ arm,
+ logger,
+ single_plan_parameters=arm_plan_request_params,
+ )
+
+ # 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=arm_plan_request_params,
+ )
+
+ # Finish 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 8ee83fb0..9ffc05a3 100644
--- a/sciurus17_examples_py/setup.py
+++ b/sciurus17_examples_py/setup.py
@@ -31,6 +31,7 @@
'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',
+ 'pose_groupstate = sciurus17_examples_py.pose_groupstate:main',
],
},
)
diff --git a/sciurus17_moveit_config/config/sciurus17.srdf b/sciurus17_moveit_config/config/sciurus17.srdf
index b77822dc..10cbdaa1 100644
--- a/sciurus17_moveit_config/config/sciurus17.srdf
+++ b/sciurus17_moveit_config/config/sciurus17.srdf
@@ -121,6 +121,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+