diff --git a/sciurus17_examples_py/sciurus17_examples_py/waist_control.py b/sciurus17_examples_py/sciurus17_examples_py/waist_control.py new file mode 100755 index 0000000..4796f9f --- /dev/null +++ b/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() diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index c3c0b5e..8915f0c 100644 --- a/sciurus17_examples_py/setup.py +++ b/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', ], }, )