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/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/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,82 @@
+# 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 sciurus17_examples_py.utils import plan_and_execute
+
+from moveit.planning import (
+    MoveItPy,
+    PlanRequestParameters,
+)
+
+import rclpy
+from rclpy.logging import get_logger
+
+
+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',
+    )
+
+    # 動作速度の調整
+    arm_plan_request_params.max_acceleration_scaling_factor = 1.0  # Set 0.0 ~ 1.0
+    arm_plan_request_params.max_velocity_scaling_factor = 1.0  # Set 0.0 ~ 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

@@ -76,6 +76,15 @@
         <joint name="r_arm_joint6" value="-2.0943"/>
         <joint name="r_arm_joint7" value="0"/>
     </group_state>
+    <group_state name="r_arm_push_forward_pose" group="r_arm_group">
+        <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="0.5236"/>
+        <joint name="r_arm_joint5" value="0"/>
+        <joint name="r_arm_joint6" value="0"/>
+        <joint name="r_arm_joint7" value="0"/>
+    </group_state>
     <group_state name="l_arm_init_pose" group="l_arm_group">
         <joint name="l_arm_joint1" value="0"/>
         <joint name="l_arm_joint2" value="1.5707"/>
@@ -85,9 +94,18 @@
         <joint name="l_arm_joint6" value="2.0943"/>
         <joint name="l_arm_joint7" value="0"/>
     </group_state>
+    <group_state name="l_arm_push_forward_pose" group="l_arm_group">
+        <joint name="l_arm_joint1" value="0"/>
+        <joint name="l_arm_joint2" value="0"/>
+        <joint name="l_arm_joint3" value="0"/>
+        <joint name="l_arm_joint4" value="0"/>
+        <joint name="l_arm_joint5" value="0"/>
+        <joint name="l_arm_joint6" value="0"/>
+        <joint name="l_arm_joint7" value="0"/>
+    </group_state>
     <group_state name="r_arm_waist_init_pose" group="r_arm_waist_group">
         <joint name="r_arm_joint1" value="0"/>
-        <joint name="r_arm_joint2" value="-1.5707"/>
+        <joint name="r_arm_joint2" value="0"/>
         <joint name="r_arm_joint3" value="0"/>
         <joint name="r_arm_joint4" value="2.7262"/>
         <joint name="r_arm_joint5" value="0"/>
@@ -121,6 +139,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="-1.5707"/>
+        <joint name="l_arm_joint5" value="0"/>
+        <joint name="l_arm_joint6" value="0.5236"/>
+        <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="1.5707"/>
+        <joint name="r_arm_joint5" value="0"/>
+        <joint name="r_arm_joint6" value="-0.5236"/>
+        <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"/>