task_name: CloseBox
category_name: Articulated

tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
object_keypoint_name_list: [articulated_object_head, articulated_object_tail]

constraint_list:
# ensure the gripper to be in contact with the box lid with whole fingers
- keypoint_name: tool_head
  target_keypoint_name: articulated_object_head
  tolerance: 0.000001
  type: point2point_constraint
  name: fingers_contact_with_link0

# ensure surface of the gripper to be parallel to the box lid
- axis_from_keypoint_name: tool_head
  axis_to_keypoint_name: tool_side
  target_axis: object_axis
  target_axis_frame: object
  tolerance: 0.05
  target_inner_product: 1
  type: frame_axis_parallel
  name: hand_parallel_to_link0_edge

# - axis_from_keypoint_name: tool_head
#   axis_to_keypoint_name: tool_tail
#   target_axis: [1.0, 0, 0]
#   target_axis_frame: object
#   tolerance: 0.01
#   target_inner_product: 0
#   type: frame_axis_orthogonal

# - axis_from_keypoint_name: tool_head
#   axis_to_keypoint_name: tool_tail
#   target_axis_from_keypoint_name: articulated_object_head
#   target_axis_to_keypoint_name: articulated_object_tail
#   tolerance: 0.01
#   target_inner_product: 0
#   type: keypoint_axis_orthogonal

- axis_from_keypoint_name: tool_head
  axis_to_keypoint_name: tool_tail
  cross_target_axis1_from_keypoint_name: articulated_object_head
  cross_target_axis1_to_keypoint_name: articulated_object_tail
  target_axis: object_axis
  target_axis_frame: object    # cross_target_axis2_frame
  tolerance: 0.05
  target_inner_product: 1
  type: frame_axis_parallel
  name: fingers_orthogonal_to_link0



# for pre-actuation and post-actuation motions.
# Each motion is represented in the [mode,value] format
# mode: translate or rotate
# value: [x,y,z] in the tool frame for translate or radian for rotate
# Units are in meters and radians respectively.

contact_pose_index: 2
# pre-actuation pose list.
pre_actuation_motions: [ ["translate_z", -0.03]]  #   ["translate_z", -0.0001] , ["translate_x", -0.01],
# post-actuation pose list
post_actuation_motions: [ ["rotate",0.3],["rotate",0.2], ["rotate",0.1], ["rotate",0.1], ["rotate",0.1]] # ["translate_z", 0.08],

# trajectory time
# actuation_time: 24 # time to reach task goal pose
# pre_actuation_times: [16, 20]
# post_actuation_times: [26, 32]

actuation_time: 6 # time to reach task goal pose
# pre_actuation_times: [4, 8]
# post_actuation_times: [16, 20]
pre_actuation_times: [4] # 6
post_actuation_times: [10,12] # 6,