diff --git a/crane_x7_examples_py/crane_x7_examples_py/joint_values.py b/crane_x7_examples_py/crane_x7_examples_py/joint_values.py index f02317ab..8b551ad8 100644 --- a/crane_x7_examples_py/crane_x7_examples_py/joint_values.py +++ b/crane_x7_examples_py/crane_x7_examples_py/joint_values.py @@ -48,7 +48,9 @@ def main(args=None): ) # 動作速度の調整 - arm_plan_request_params.max_acceleration_scaling_factor = 0.5 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_acceleration_scaling_factor = ( + 0.5 # Set 0.0 ~ 1.0 + ) arm_plan_request_params.max_velocity_scaling_factor = 0.5 # Set 0.0 ~ 1.0 # SRDFに定義されている'vertical'の姿勢にする @@ -69,19 +71,30 @@ def main(args=None): 'crane_x7_lower_arm_fixed_part_joint', 'crane_x7_lower_arm_revolute_part_joint', 'crane_x7_wrist_joint', - ] + ] target_joint_value = math.radians(-45.0) - # 各関節角度を順番に-45[deg]ずつ動かす + # 現在角度をベースに、目標角度を作成する + current_state = arm.get_start_state() + joint_values = current_state.get_joint_group_positions('arm') + + # jointのリストを辞書型の形式に変換する + joint_values_dict = dict(zip(joint_names, joint_values)) + + # 各関節角度を順番に-45[deg]に動かす for joint_name in joint_names: - arm.set_start_state_to_current_state() + # 対象のjointに目標値を設定する + joint_values_dict[joint_name] = target_joint_value + robot_state.joint_positions = joint_values_dict - 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_x7.get_robot_model().get_joint_model_group('arm'), + joint_model_group=crane_x7.get_robot_model().get_joint_model_group( + 'arm' + ), ) + + arm.set_start_state_to_current_state() arm.set_goal_state(motion_plan_constraints=[joint_constraint]) plan_and_execute(