add waist sample

sciurus17_examples_py/sciurus17_examples_py/waist_control.py

@@ -0,0 +1,107 @@
+# Copyright 2023 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.
+import math
+
+
+# moveit python library
+from moveit.core.robot_state import RobotState
+from moveit.planning import (
+    MoveItPy,
+    PlanRequestParameters,
+)
+# generic ros libraries
+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('moveit_py.waist_control')
+
+    # instantiate MoveItPy instance and get planning component
+    sciurus17 = MoveItPy(node_name='waist_control')
+    logger.info('MoveItPy instance created')
+
+    # 腰制御用 planning component
+    waist = sciurus17.get_planning_component('waist_group')
+    planning_scene_monitor = sciurus17.get_planning_scene_monitor()
+
+    robot_model = sciurus17.get_robot_model()
+
+    plan_request_params = PlanRequestParameters(
+        sciurus17,
+        'ompl_rrtc_default',
+    )
+    # 動作速度の調整
+    plan_request_params.max_acceleration_scaling_factor = 0.1    # Set 0.0 ~ 1.0
+    plan_request_params.max_velocity_scaling_factor = 0.1    # Set 0.0 ~ 1.0
+
+    # SRDFに定義されている'waist_init_pose'の姿勢にする
+    waist.set_start_state_to_current_state()
+    waist.set_goal_state(configuration_name='waist_init_pose')
+    plan_and_execute(
+        sciurus17,
+        waist,
+        logger,
+        single_plan_parameters=plan_request_params,
+    )
+
+    # 現在角度をベースに、目標角度を作成する
+    joint_values = []
+    with planning_scene_monitor.read_only() as scene:
+        robot_state = scene.current_state
+        joint_values = robot_state.get_joint_group_positions('waist_group')
+
+    joint_values[0] = math.radians(45.0)
+    waist.set_start_state_to_current_state()
+    robot_state = RobotState(robot_model)
+    robot_state.set_joint_group_positions('waist_group', joint_values)
+    waist.set_goal_state(robot_state=robot_state)
+    plan_and_execute(
+        sciurus17,
+        waist,
+        logger,
+        single_plan_parameters=plan_request_params,
+    )
+
+    joint_values[0] = math.radians(-45.0)
+    waist.set_start_state_to_current_state()
+    robot_state = RobotState(robot_model)
+    robot_state.set_joint_group_positions('waist_group', joint_values)
+    waist.set_goal_state(robot_state=robot_state)
+    plan_and_execute(
+        sciurus17,
+        waist,
+        logger,
+        single_plan_parameters=plan_request_params,
+    )
+
+    waist.set_start_state_to_current_state()
+    waist.set_goal_state(configuration_name='waist_init_pose')
+    plan_and_execute(
+        sciurus17,
+        waist,
+        logger,
+        single_plan_parameters=plan_request_params,
+    )
+
+    # Finishe with error. Related Issue
+    # https://github.com/moveit/moveit2/issues/2693
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

sciurus17_examples_py/setup.py

@@ -25,9 +25,10 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'gripper_control = sciurus17_examples_py.gripper_control:main',
             'neck_control = sciurus17_examples_py.neck_control:main',
+            'waist_control = sciurus17_examples_py.waist_control:main',
             'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main',
-            'gripper_control = sciurus17_examples_py.gripper_control:main',
         ],
     },
 )