diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index a8fb9d8..6e1cd28 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -1,11 +1,11 @@ # Copyright 2020 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. @@ -20,16 +20,14 @@ from rclpy.logging import get_logger # moveit python library +from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, PlanRequestParameters, ) +from moveit.core.kinematic_constraints import construct_joint_constraint -from utils import plan_and_execute - - -def to_radians(deg_angle): - return deg_angle * math.pi / 180.0 +from crane_plus_examples_py.utils import plan_and_execute def main(args=None): @@ -43,41 +41,35 @@ def main(args=None): crane_plus = MoveItPy(node_name="moveit_py") crane_plus_arm = crane_plus.get_planning_component("arm") logger.info("MoveItPy instance created") + + # instantiate a a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + robot_state = RobotState(robot_model) - # 速度&加速度のスケーリングファクタを設定 plan_request_params = PlanRequestParameters( crane_plus, "ompl_rrtc", ) - plan_request_params.max_acceleration_scaling_factor(1.0) # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor(1.0) # Set 0.0 ~ 1.0 - - # 現在の位置から"vertical"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=plan_request_params, - ) - - # 現在の位置からarmの関節を全て順番に45度動かす - joint_values = crane_plus_arm.get_named_target_state_values("arm") - target_joint_value = math.radians(45.0) - # joint_valuesはDict型なので、keyの数(=jointの数)だけ関節を動かす - for key in enumerate(joint_values.keys()): - joint_values[key] = target_joint_value + # armの関節のjoint1〜4を順番に45[deg]ずつ動かす + joint_names = ["crane_plus_joint1", "crane_plus_joint2", "crane_plus_joint3", "crane_plus_joint4"] + target_joint_value = math.radians(45) + for joint_name in joint_names: + joint_values = {joint_name: target_joint_value} + robot_state.joint_positions = joint_values + joint_constraint = construct_joint_constraint( + robot_state=robot_state, + joint_model_group=crane_plus.get_robot_model().get_joint_model_group("arm"), + ) crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") + crane_plus_arm.set_goal_state(motion_plan_constraints=[joint_constraint]) plan_and_execute( crane_plus, crane_plus_arm, logger, single_plan_parameters=plan_request_params, ) - + # 現在の位置から"vertical"ポジションに動かす crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state(configuration_name="vertical") @@ -91,6 +83,9 @@ def main(args=None): # MoveItPyの終了 crane_plus.shutdown() + # rclpyの終了 + rclpy.shutdown() + if __name__ == "__main__": main()