pose_groupstateサンプルの追加

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

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
 
 首を上下左右へ動かすコード例です。

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]')
     )
 

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;
+}

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
 
 首を上下左右へ動かすコード例です。

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(

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()

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',
         ],
     },
 )

sciurus17_moveit_config/config/sciurus17.srdf

@@ -121,6 +121,22 @@
         <joint name="r_arm_joint6" value="-2.0943"/>
         <joint name="r_arm_joint7" value="0"/>
     </group_state>
+    <group_state name="two_arm_push_forward_pose" group="two_arm_group">
+        <joint name="l_arm_joint1" value="-0.5236"/>
+        <joint name="l_arm_joint2" value="1.5707"/>
+        <joint name="l_arm_joint3" value="0"/>
+        <joint name="l_arm_joint4" value="-2.0944"/>
+        <joint name="l_arm_joint5" value="0"/>
+        <joint name="l_arm_joint6" value="1.0472"/>
+        <joint name="l_arm_joint7" value="0"/>
+        <joint name="r_arm_joint1" value="0.5236"/>
+        <joint name="r_arm_joint2" value="-1.5707"/>
+        <joint name="r_arm_joint3" value="0"/>
+        <joint name="r_arm_joint4" value="2.0944"/>
+        <joint name="r_arm_joint5" value="0"/>
+        <joint name="r_arm_joint6" value="-1.0472"/>
+        <joint name="r_arm_joint7" value="0"/>
+    </group_state>
     <group_state name="neck_init_pose" group="neck_group">
         <joint name="neck_pitch_joint" value="0"/>
         <joint name="neck_yaw_joint" value="0"/>