init commit

workflows/simbox/solver/kpam/SE3_utils.py

@@ -0,0 +1,131 @@
+import numpy as np
+
+
+def transform_point_cloud(T_homo, pc):
+    # type: (np.ndarray, np.ndarray) -> np.ndarray
+    """
+    Transform a point cloud using homogeous transform
+    :param T_homo: 4x4 transformation
+    :param pc: (N, 3) point cloud
+    :return: (N, 3) point cloud
+    """
+    transformed_pc = T_homo[0:3, 0:3].dot(pc.T)
+    transformed_pc = transformed_pc.T
+    transformed_pc[:, 0] += T_homo[0, 3]
+    transformed_pc[:, 1] += T_homo[1, 3]
+    transformed_pc[:, 2] += T_homo[2, 3]
+    return transformed_pc
+
+
+def xyzrpy_to_matrix(xyzrpy):
+    """
+    Create 4x4 homogeneous transform matrix from pos and rpy
+    """
+    xyz = xyzrpy[0:3]
+    rpy = xyzrpy[3:6]
+
+    T = np.zeros([4, 4], dtype=xyzrpy.dtype)
+    T[0:3, 0:3] = rpy_to_rotation_matrix(rpy)
+    T[3, 3] = 1
+    T[0:3, 3] = xyz
+    return T
+
+
+def rpy_to_rotation_matrix(rpy):
+    """
+    Creates 3x3 rotation matrix from rpy
+    See http://danceswithcode.net/engineeringnotes/rotations_in_3d/rotations_in_3d_part1.html
+    """
+    u = rpy[0]
+    v = rpy[1]
+    w = rpy[2]
+
+    R = np.zeros([3, 3], dtype=rpy.dtype)
+
+    # first row
+    R[0, 0] = np.cos(v) * np.cos(w)
+    R[0, 1] = np.sin(u) * np.sin(v) * np.cos(w) - np.cos(u) * np.sin(w)
+    R[0, 2] = np.sin(u) * np.sin(w) + np.cos(u) * np.sin(v) * np.cos(w)
+
+    # second row
+    R[1, 0] = np.cos(v) * np.sin(w)
+    R[1, 1] = np.cos(u) * np.cos(w) + np.sin(u) * np.sin(v) * np.sin(w)
+    R[1, 2] = np.cos(u) * np.sin(v) * np.sin(w) - np.sin(u) * np.cos(w)
+
+    # third row
+    R[2, 0] = -np.sin(v)
+    R[2, 1] = np.sin(u) * np.cos(v)
+    R[2, 2] = np.cos(u) * np.cos(v)
+
+    return R
+
+
+def transform_point(T, p):
+    """
+    Transform a point via a homogeneous transform matrix T
+
+    :param: T 4x4 numpy array
+    """
+
+    p_homog = np.concatenate((p, np.array([1])))
+    q = np.dot(T, p_homog)
+    q = q.squeeze()
+    q = q[0:3]
+    return q
+
+
+def transform_vec(T, v):
+    """
+    Transform a vector via a homogeneous transform matrix T
+
+    :param: T 4x4 numpy array
+    """
+    v = np.array(v)
+    R = T[:3, :3]
+    u = np.dot(R, v)
+    return u
+
+
+def xyzrpy_to_matrix_symbolic(xyzrpy):
+    """
+    Create 4x4 homogeneous transform matrix from pos and rpy
+    """
+    xyz = xyzrpy[0:3]
+    rpy = xyzrpy[3:6]
+
+    T = np.zeros([4, 4], dtype=xyzrpy.dtype)
+    T[0:3, 0:3] = rpy_to_rotation_matrix_symbolic(rpy)
+    T[3, 3] = 1
+    T[0:3, 3] = xyz
+    return T
+
+
+def rpy_to_rotation_matrix_symbolic(rpy):
+    """
+    Creates 3x3 rotation matrix from rpy
+    See http://danceswithcode.net/engineeringnotes/rotations_in_3d/rotations_in_3d_part1.html
+    """
+    from pydrake.math import cos, sin
+
+    u = rpy[0]
+    v = rpy[1]
+    w = rpy[2]
+
+    R = np.zeros([3, 3], dtype=rpy.dtype)
+
+    # first row
+    R[0, 0] = cos(v) * cos(w)
+    R[0, 1] = sin(u) * sin(v) * cos(w) - cos(u) * sin(w)
+    R[0, 2] = sin(u) * sin(w) + cos(u) * sin(v) * cos(w)
+
+    # second row
+    R[1, 0] = cos(v) * sin(w)
+    R[1, 1] = cos(u) * cos(w) + sin(u) * sin(v) * sin(w)
+    R[1, 2] = cos(u) * sin(v) * sin(w) - sin(u) * cos(w)
+
+    # third row
+    R[2, 0] = -sin(v)
+    R[2, 1] = sin(u) * cos(v)
+    R[2, 2] = cos(u) * cos(v)
+
+    return R

workflows/simbox/solver/kpam/config/CloseBox.yaml

@@ -0,0 +1,75 @@
+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,

workflows/simbox/solver/kpam/config/CloseBox_drawer.yaml

@@ -0,0 +1,83 @@
+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: link_contact_point
+    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_link0_contact_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_tail
+    axis_to_keypoint_name: tool_head
+    target_axis: object_link0_move_axis
+    target_axis_frame: object
+    tolerance: 0.05
+    target_inner_product: 1
+    type: frame_axis_parallel
+    name: hand_parallel_to_link0_move_axis
+
+  # - 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: 1
+# pre-actuation pose list.
+pre_actuation_motions: [["translate_z", -0.05]] #   ["translate_z", -0.0001] , ["translate_x", -0.01],
+# post-actuation pose list
+post_actuation_motions: [["translate_z", 0.3],["translate_z", 0.1],["translate_z", 0.05],["translate_z", 0.05]] # ["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,

workflows/simbox/solver/kpam/config/CloseBox_laptop.yaml

@@ -0,0 +1,81 @@
+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: link0_contact_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: link0_contact_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],
+    ["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,

workflows/simbox/solver/kpam/config/CloseBox_storagefurniture.yaml

@@ -0,0 +1,76 @@
+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: link0_contact_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: link0_contact_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.7],["rotate",0.3], ["rotate",0.2], ["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,

workflows/simbox/solver/kpam/config/CloseLaptop.yaml

@@ -0,0 +1,46 @@
+task_name: CloseLaptop
+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:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [1.0, 0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+- 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
+
+
+
+# for pre-actuation and post-actuation poses relative to the tool. 
+# Z axis positive points from gripper to fingers and X axis points to the front direction. 
+# Each pose is represented in the [[x,y,z,roll,pitch,yaw]] format
+# Units are in meters and radians respectively.
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_x", 0.06], ["translate_z", -0.15]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [["translate_z", 0.05], ["translate_x", -0.25]] # [press lid down]
+
+# trajectory time
+actuation_time: 20 # time to reach task goal pose
+pre_actuation_times: [12, 16]
+post_actuation_times: [22, 30]

workflows/simbox/solver/kpam/config/CloseMicrowave.yaml

@@ -0,0 +1,42 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 0
+  - 1.0
+  - 0
+  target_axis_frame: object
+  target_inner_product: -1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-right
+- move-forward
+- move-forward
+pre_actuation_motions:
+- - translate_x
+  - -0.1
+- - translate_z
+  - -0.15
+task_name: CloseMicrowave
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/CloseRefrigeratorDoor.yaml

@@ -0,0 +1,42 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 0
+  - 1.0
+  - 0
+  target_axis_frame: object
+  target_inner_product: -1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-right
+- move-forward
+- move-forward
+pre_actuation_motions:
+- - translate_x
+  - -0.05
+- - translate_z
+  - -0.15
+task_name: CloseRefrigeratorDoor
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/CloseSafe.yaml

@@ -0,0 +1,41 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 0
+  - 0
+  - 1.0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 0
+  - 1.0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- - translate_x
+  - 0.2
+pre_actuation_motions:
+- - translate_x
+  - -0.1
+- - translate_z
+  - -0.2
+task_name: CloseSafe
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/CloseSuitcaseLid.yaml

@@ -0,0 +1,56 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: -1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- 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
+  target_inner_product: -1
+  tolerance: 0.01
+  type: keypoint_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis_from_keypoint_name: articulated_object_head
+  target_axis_to_keypoint_name: articulated_object_tail
+  target_inner_product: 0
+  tolerance: 0.01
+  type: keypoint_axis_orthogonal
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+object_keypoint_name_list:
+- articulated_object_head
+- articulated_object_tail
+post_actuation_motions:
+- move-down
+- move-backward
+pre_actuation_motions:
+- - translate_x
+  - 0.1
+- - translate_z
+  - -0.15
+task_name: CloseSuitcaseLid
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/LiftBucketUpright.yaml

@@ -0,0 +1,41 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_tail
+  axis_to_keypoint_name: tool_head
+  target_axis:
+  - 0
+  - 0
+  - -1
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 0
+  - 0
+  - 1
+  target_axis_frame: world
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-up
+- move-up
+pre_actuation_motions:
+- - translate_z
+  - -0.2
+- - translate_x
+  - -0.1
+task_name: LiftBucketUpright
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/MoveAboveBucket.yaml

@@ -0,0 +1,40 @@
+task_name: MoveAboveBucket
+category_name: Articulated
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [articulated_object_above]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_above
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 1.0, 0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: -1
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.1], ["translate_y", 0.2],] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 30 # time to reach task goal pose
+pre_actuation_times: [15, 20]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/MoveBagForward.yaml

@@ -0,0 +1,41 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 0
+  - 1.0
+  - 0
+  target_axis_frame: object
+  target_inner_product: -1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-forward
+- move-up
+pre_actuation_motions:
+- - translate_x
+  - -0.05
+- - translate_z
+  - -0.15
+task_name: MoveBagForward
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/MoveInMicrowave.yaml

@@ -0,0 +1,40 @@
+task_name: MoveInMicrowave
+category_name: Articulated
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [articulated_object_inside_base]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_inside_base
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 0.707
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 1.0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: 0
+  type: frame_axis_orthogonal
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_x", -0.1], ["translate_z", -0.2]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 30 # time to reach task goal pose
+pre_actuation_times: [20, 25]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/MoveInsideBox.yaml

@@ -0,0 +1,40 @@
+task_name: MoveInsideBox
+category_name: Articulated
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [articulated_object_inside_base]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_inside_base
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 1.0, 0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: -1
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.1], ["translate_y", 0.2],] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 30 # time to reach task goal pose
+pre_actuation_times: [15, 20]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/MoveOnLaptop.yaml

@@ -0,0 +1,40 @@
+task_name: MoveOnLaptop
+category_name: Articulated
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [articulated_object_surface_base]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_surface_base
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 1.0, 0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: -1
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.1], ["translate_y", 0.2],] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 30 # time to reach task goal pose
+pre_actuation_times: [15, 20]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/MoveToDrawer.yaml

@@ -0,0 +1,40 @@
+task_name: MoveToDrawer
+category_name: Articulated
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [articulated_object_inside]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_inside
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 1.0, 0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: -1
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.1], ["translate_y", 0.2],] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 30 # time to reach task goal pose
+pre_actuation_times: [15, 20]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/MoveToRefrigerator.yaml

@@ -0,0 +1,40 @@
+task_name: MoveToRefrigerator
+category_name: Articulated
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [articulated_object_inside_base]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_inside_base
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 1.0, 0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: -1
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_x", -0.2], ["translate_y", 0.2],] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 30 # time to reach task goal pose
+pre_actuation_times: [15, 20]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/MoveUnderFaucet.yaml

@@ -0,0 +1,40 @@
+task_name: MoveUnderFaucet
+category_name: Articulated
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [articulated_object_bottom_base]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_bottom_base
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 0
+  type: frame_axis_orthogonal
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [1.0, 0, 0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_y", -0.15], ["translate_x", -0.2]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 30 # time to reach task goal pose
+pre_actuation_times: [20, 25]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/OpenBox.yaml

@@ -0,0 +1,62 @@
+task_name: OpenBox
+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:
+# make sure the robot gripper hits the box lid
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# 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: [1.0, 0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+- 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: -1
+  type: keypoint_axis_parallel
+  
+# ensure gripper to point directly to the direction of the box
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  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
+  target_axis: [1.0, 0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: 0
+  type: frame_axis_orthogonal
+
+# for pre-actuation and post-actuation poses relative to the tool. 
+# Z axis positive points from gripper to fingers and X axis points to the front direction. 
+# Each pose is represented in the [[x,y,z,roll,pitch,yaw]] format
+# Units are in meters and radians respectively.
+
+# pre-actuation notion list. 
+pre_actuation_motions: [["translate_x", -0.05], ["translate_z", -0.15]] # [move gripper below lid, adjust gripper direction and approach]
+
+# post-actuation motion list. pushing down
+post_actuation_motions: [["rotate", -0.5]] # [lift lid up]
+
+# trajectory time
+actuation_time: 24 # time to reach task goal pose
+pre_actuation_times: [16, 20]
+post_actuation_times: [32]

workflows/simbox/solver/kpam/config/OpenDrawer.yaml

@@ -0,0 +1 @@
+{'task_name': 'OpenDrawer', 'category_name': 'Articulated', 'tool_keypoint_name_list': ['tool_head', 'tool_tail', 'tool_side'], 'object_keypoint_name_list': ['articulated_object_head'], 'constraint_list': [{'keypoint_name': 'tool_head', 'target_keypoint_name': 'articulated_object_head', 'tolerance': 0.0001, 'type': 'point2point_constraint'}, {'axis_from_keypoint_name': 'tool_head', 'axis_to_keypoint_name': 'tool_side', 'target_axis': [1.0, 0, 0], 'target_axis_frame': 'object', 'tolerance': 0.01, 'target_inner_product': 1, 'type': 'frame_axis_parallel'}, {'axis_from_keypoint_name': 'tool_head', 'axis_to_keypoint_name': 'tool_tail', 'target_axis': [0, 0, 1.0], 'target_axis_frame': 'object', 'tolerance': 0.01, 'target_inner_product': 0, 'type': 'frame_axis_orthogonal'}], 'pre_actuation_motions': [['translate_x', 0.1], ['translate_z', -0.1]], 'post_actuation_motions': ['move-backward']}

workflows/simbox/solver/kpam/config/OpenLaptop.yaml

@@ -0,0 +1,62 @@
+task_name: OpenLaptop
+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:
+
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [1.0, 0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+- 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: -1
+  type: keypoint_axis_parallel
+
+# ensure gripper to point directly to the direction of the laptop
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  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
+  target_axis: [1.0, 0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: 0
+  type: frame_axis_orthogonal
+
+# for pre-actuation and post-actuation poses relative to the tool. 
+# Z axis positive points from gripper to fingers and X axis points to the front direction. 
+# Each pose is represented in the [[x,y,z,roll,pitch,yaw]] format
+# Units are in meters and radians respectively.
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_x", -0.05], ["translate_z", -0.15]] # [adjust gripper direction and approach, move gripper below lid]
+
+# post-actuation pose list. pushing down
+post_actuation_motions: [["translate_z", 0.05], ["translate_x", 0.15]] # [lift lid up]
+
+# trajectory time
+actuation_time: 20 # time to reach task goal pose
+pre_actuation_times: [12, 16]
+post_actuation_times: [22, 30]

workflows/simbox/solver/kpam/config/OpenRefrigeratorDoor.yaml

@@ -0,0 +1,9 @@
+{category_name: Articulated, constraint_list: [{keypoint_name: tool_tail, target_keypoint_name: articulated_object_head,
+tolerance: 0.0001, type: point2point_constraint}, {axis_from_keypoint_name: tool_head,
+axis_to_keypoint_name: tool_side, target_axis: [0, 1.0, 0], target_axis_frame: object,
+target_inner_product: -1, tolerance: 0.01, type: frame_axis_parallel}, {axis_from_keypoint_name: tool_head,
+axis_to_keypoint_name: tool_tail, target_axis: [1.0, 0, 0], target_axis_frame: object,
+target_inner_product: 1, tolerance: 0.01, type: frame_axis_parallel}], object_keypoint_name_list: [
+articulated_object_head], post_actuation_motions: [[translate_x, -0.5]], pre_actuation_motions: [
+[translate_x, 0.1], [translate_z, -0.15]], task_name: OpenRefrigeratorDoor, tool_keypoint_name_list: [
+tool_head, tool_tail, tool_side]}

workflows/simbox/solver/kpam/config/OpenSafe.yaml

@@ -0,0 +1,41 @@
+category_name: Articulated
+constraint_list:
+-   keypoint_name: tool_tail
+    target_keypoint_name: articulated_object_head
+    tolerance: 0.0001
+    type: point2point_constraint
+-   axis_from_keypoint_name: tool_head
+    axis_to_keypoint_name: tool_side
+    target_axis:
+    - 0
+    - 1.0
+    - 0
+    target_axis_frame: object
+    target_inner_product: 1
+    tolerance: 0.01
+    type: frame_axis_parallel
+-   axis_from_keypoint_name: tool_head
+    axis_to_keypoint_name: tool_tail
+    target_axis:
+    - 1.0
+    - 0
+    - 0
+    target_axis_frame: object
+    target_inner_product: -1
+    tolerance: 0.01
+    type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-right
+- move-right
+pre_actuation_motions:
+-   - translate_x
+    - 0.1
+-   - translate_z
+    - -0.15
+task_name: OpenSafe
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/PressToasterLever.yaml

@@ -0,0 +1 @@
+{'task_name': 'PressToasterLever', 'category_name': 'Articulated', 'tool_keypoint_name_list': ['tool_head', 'tool_tail', 'tool_side'], 'object_keypoint_name_list': ['articulated_object_head'], 'constraint_list': [{'keypoint_name': 'tool_side', 'target_keypoint_name': 'articulated_object_head', 'tolerance': 0.0001, 'type': 'point2point_constraint'}, {'axis_from_keypoint_name': 'tool_head', 'axis_to_keypoint_name': 'tool_side', 'target_axis': [1.0, 0, 0], 'target_axis_frame': 'object', 'tolerance': 0.01, 'target_inner_product': 1, 'type': 'frame_axis_parallel'}, {'axis_from_keypoint_name': 'tool_head', 'axis_to_keypoint_name': 'tool_tail', 'target_axis': [0, 0, 1.0], 'target_axis_frame': 'object', 'tolerance': 0.01, 'target_inner_product': 0, 'type': 'frame_axis_orthogonal'}], 'pre_actuation_motions': [['translate_z', -0.15]], 'post_actuation_motions': ['move-down', 'move-down']}

workflows/simbox/solver/kpam/config/PushBox.yaml

@@ -0,0 +1,42 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 0
+  - 0
+  - 1.0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-forward
+- move-left
+- move-forward
+pre_actuation_motions:
+- - translate_x
+  - -0.05
+- - translate_z
+  - -0.15
+task_name: PushBox
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/PushDrawerClose.yaml

@@ -0,0 +1 @@
+{'task_name': 'PushDrawerClose', 'category_name': 'Articulated', 'tool_keypoint_name_list': ['tool_head', 'tool_tail', 'tool_side'], 'object_keypoint_name_list': ['articulated_object_head'], 'constraint_list': [{'keypoint_name': 'tool_head', 'target_keypoint_name': 'articulated_object_head', 'tolerance': 0.0001, 'type': 'point2point_constraint'}, {'axis_from_keypoint_name': 'tool_head', 'axis_to_keypoint_name': 'tool_side', 'target_axis': [1.0, 0, 0], 'target_axis_frame': 'object', 'tolerance': 0.01, 'target_inner_product': 1, 'type': 'frame_axis_parallel'}, {'axis_from_keypoint_name': 'tool_head', 'axis_to_keypoint_name': 'tool_tail', 'target_axis': [0, 0, 1.0], 'target_axis_frame': 'object', 'tolerance': 0.01, 'target_inner_product': 0, 'type': 'frame_axis_orthogonal'}], 'pre_actuation_motions': [['translate_x', -0.05], ['translate_z', -0.05]], 'post_actuation_motions': [['translate_x', 0.13]], 'actuation_time': 14, 'pre_actuation_times': [7, 9], 'post_actuation_times': [17]}

workflows/simbox/solver/kpam/config/PushOvenClose.yaml

@@ -0,0 +1,41 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 0
+  - 0
+  - 1.0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-forward
+- move-forward
+pre_actuation_motions:
+- - translate_x
+  - -0.05
+- - translate_z
+  - -0.1
+task_name: push-oven-close
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/PushToasterForward.yaml

@@ -0,0 +1,41 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 0
+  - 0
+  - 1.0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-forward
+- move-backward
+pre_actuation_motions:
+- - translate_x
+  - -0.05
+- - translate_z
+  - -0.15
+task_name: PushToasterForward
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/ReachBall.yaml

@@ -0,0 +1,42 @@
+
+
+task_name: ReachBall
+category_name: RigidBody
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [rigidbody_object_head]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: rigidbody_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 1.0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: -1
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.3]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 20 # time to reach task goal pose
+pre_actuation_times: [12]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/ReachCuboid.yaml

@@ -0,0 +1,42 @@
+
+
+task_name: ReachCuboid
+category_name: RigidBody
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [rigidbody_object_head]
+
+constraint_list:
+
+- keypoint_name: tool_tail
+  target_keypoint_name: rigidbody_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [1.0, 0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: -1
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.2]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 20 # time to reach task goal pose
+pre_actuation_times: [12]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/ReachRigidBody.yaml

@@ -0,0 +1,42 @@
+
+
+task_name: ReachRigidBody
+category_name: RigidBody
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [rigidbody_object_head]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: rigidbody_object_head
+  tolerance: 0.001
+  type: point2point_constraint
+
+# # ensure surface of the gripper to be parallel to the laptop lid
+# - axis_from_keypoint_name: tool_head
+#   axis_to_keypoint_name: tool_side
+#   target_axis: [1.0, 0, 0]
+#   target_axis_frame: object
+#   tolerance: 0.01
+#   target_inner_product: -1
+#   type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.2]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [["translate_z", 0.05]] # [press lid down]
+
+# trajectory time
+actuation_time: 20 # time to reach task goal pose
+pre_actuation_times: [12]
+post_actuation_times: [24]

workflows/simbox/solver/kpam/config/ReachRigidBodyEdge.yaml

@@ -0,0 +1,42 @@
+
+
+task_name: ReachRigidBody
+category_name: RigidBody
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [rigidbody_object_head]
+
+constraint_list:
+
+- keypoint_name: tool_side
+  target_keypoint_name: rigidbody_object_head
+  tolerance: 0.001
+  type: point2point_constraint
+
+# # ensure surface of the gripper to be parallel to the laptop lid
+# - axis_from_keypoint_name: tool_head
+#   axis_to_keypoint_name: tool_side
+#   target_axis: [1.0, 0, 0]
+#   target_axis_frame: object
+#   tolerance: 0.01
+#   target_inner_product: -1
+#   type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.2]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 20 # time to reach task goal pose
+pre_actuation_times: [12]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/ReachRigidBodyHorizontal.yaml

@@ -0,0 +1,49 @@
+
+
+task_name: ReachRigidBody
+category_name: RigidBody
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [rigidbody_object_head]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: rigidbody_object_head
+  tolerance: 0.001
+  type: point2point_constraint
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 0
+  type: frame_axis_orthogonal
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 0
+  type: frame_axis_orthogonal
+
+# - axis_from_keypoint_name: tool_head
+#   axis_to_keypoint_name: tool_side
+#   target_axis: [0, 1.0, 0]
+#   target_axis_frame: object
+#   tolerance: 0.01
+#   target_inner_product: 1
+#   type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.05]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [["translate_z", 0.05]] # [press lid down]
+
+# trajectory time
+actuation_time: 20 # time to reach task goal pose
+pre_actuation_times: [15]
+post_actuation_times: [25]

workflows/simbox/solver/kpam/config/ReachThinObject.yaml

@@ -0,0 +1,41 @@
+
+
+task_name: ReachThinObject
+category_name: RigidBody
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [rigidbody_object_head]
+
+constraint_list:
+
+- keypoint_name: tool_head
+  target_keypoint_name: rigidbody_object_head
+  tolerance: 0.001
+  type: point2point_constraint
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [1.0, 0, 0.0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: 0
+  type: frame_axis_orthogonal
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.2]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [["translate_z", 0.02]] # [press lid down]
+
+# trajectory time
+actuation_time: 20 # time to reach task goal pose
+pre_actuation_times: [12]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/ReachXAxis.yaml

@@ -0,0 +1,42 @@
+
+
+task_name: ReachXAxis
+category_name: RigidBody
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [rigidbody_object_head]
+
+constraint_list:
+
+- keypoint_name: tool_tail
+  target_keypoint_name: rigidbody_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure surface of the gripper to be parallel to the laptop lid
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [1.0, 0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  target_inner_product: -1
+  type: frame_axis_parallel
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_z", -0.2]] # [adjust gripper direction and approach, move gripper above lid]
+
+# post-actuation pose list.
+post_actuation_motions: [] # [press lid down]
+
+# trajectory time
+actuation_time: 20 # time to reach task goal pose
+pre_actuation_times: [12]
+post_actuation_times: []

workflows/simbox/solver/kpam/config/RelocateSuitcase.yaml

@@ -0,0 +1,40 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_tail
+  axis_to_keypoint_name: tool_head
+  target_axis:
+  - 0
+  - 1
+  - 0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 0
+  - 1
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-down
+- move-forward
+- move-up
+pre_actuation_motions:
+- - translate_z
+  - -0.05
+task_name: RelocateSuitcase
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/RotateMicrowaveDoor.yaml

@@ -0,0 +1,41 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 0
+  - 0
+  - 1.0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-left
+- move-backward
+pre_actuation_motions:
+- - translate_x
+  - 0.12
+- - translate_z
+  - -0.2
+task_name: RotateMicrowaveDoor
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/RotateOvenKnob.yaml

@@ -0,0 +1,40 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 0
+  - 0
+  - 1.0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-down
+pre_actuation_motions:
+- - translate_x
+  - 0.1
+- - translate_z
+  - -0.15
+task_name: RotateOvenKnob
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/SwayBagStrap.yaml

@@ -0,0 +1,38 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 0
+  - 1.0
+  - 0
+  target_axis_frame: object
+  target_inner_product: -1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-forward
+pre_actuation_motions:
+- - translate_z
+  - -0.05
+task_name: sway-bag-strap
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/SwingBucketHandle.yaml

@@ -0,0 +1,47 @@
+actuation_time: 20
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_tail
+  axis_to_keypoint_name: tool_head
+  target_axis:
+  - 0
+  - 0
+  - 1.0
+  target_axis_frame: world
+  target_inner_product: -1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 0
+  - 1.0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- - translate_z
+  - 0.05
+- - translate_x
+  - 0.1
+post_actuation_times:
+- 24
+- 28
+pre_actuation_motions:
+- - translate_z
+  - -0.1
+pre_actuation_times:
+- 15
+task_name: SwingBucketHandle
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/SwingDoorOpen.yaml

@@ -0,0 +1,39 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 0
+  - 1
+  - 0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 1
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-right
+- move-right
+pre_actuation_motions:
+- - translate_z
+  - -0.05
+task_name: SwingDoorOpen
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/SwingSuitcaseLidOpen.yaml

@@ -0,0 +1,55 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: -1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- 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
+  target_inner_product: -1
+  tolerance: 0.01
+  type: keypoint_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis_from_keypoint_name: articulated_object_head
+  target_axis_to_keypoint_name: articulated_object_tail
+  target_inner_product: 0
+  tolerance: 0.01
+  type: keypoint_axis_orthogonal
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 0
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+object_keypoint_name_list:
+- articulated_object_head
+- articulated_object_tail
+post_actuation_motions:
+- move-forward
+pre_actuation_motions:
+- - translate_x
+  - -0.05
+- - translate_z
+  - -0.1
+task_name: swing-suitcase-lid-open
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/ToggleDoorClose.yaml

@@ -0,0 +1,40 @@
+category_name: Articulated
+constraint_list:
+- keypoint_name: tool_tail
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis:
+  - 0
+  - 1.0
+  - 0
+  target_axis_frame: object
+  target_inner_product: 1
+  tolerance: 0.01
+  type: frame_axis_parallel
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_tail
+  target_axis:
+  - 1.0
+  - 0
+  - 0
+  target_axis_frame: object
+  target_inner_product: -1
+  tolerance: 0.01
+  type: frame_axis_parallel
+object_keypoint_name_list:
+- articulated_object_head
+post_actuation_motions:
+- move-forward
+- move-left
+- move-backward
+pre_actuation_motions:
+- - translate_z
+  - -0.05
+task_name: toggle-door-close
+tool_keypoint_name_list:
+- tool_head
+- tool_tail
+- tool_side

workflows/simbox/solver/kpam/config/TurnOffFaucet.yaml

@@ -0,0 +1,65 @@
+task_name: TurnOffFaucet
+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:
+
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure the gripper to point straight to the table
+- axis_from_keypoint_name: tool_head 
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  type: frame_axis_parallel
+  target_inner_product: 1
+
+# ensure the axis between gripper finger tips to be parallel to the table
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: frame_axis_parallel
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+  target_inner_product: 0
+
+# ensure the axis between gripper finger tips to be orthogonal to the faucet handle
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: object
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+  target_inner_product: 0
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [1.0, 0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  type: frame_axis_parallel
+  target_inner_product: 1
+
+
+
+# for pre-actuation and post-actuation poses relative to the tool. 
+# Z axis positive points from gripper to fingers and X axis points to the front direction. 
+# Each pose is represented in the [[x,y,z,roll,pitch,yaw]] format
+# Units are in meters and radians respectively.
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_y", -0.1], ["translate_z", -0.15]] # [move above handle]
+
+# post-actuation pose list. pushing down
+post_actuation_motions: [["translate_z", 0.05], ["translate_y", 0.08]] # [turn the handle by 90 degree from left to right, turn the handle by 180 degree from left to right]
+
+# trajectory time
+actuation_time: 16  # time to reach task goal pose
+pre_actuation_times: [8, 12]
+post_actuation_times: [20, 24]

workflows/simbox/solver/kpam/config/TurnOnFaucet.yaml

@@ -0,0 +1,66 @@
+task_name: TurnOnFaucet
+category_name: Articulated
+
+tool_keypoint_name_list: [tool_head, tool_tail, tool_side]
+object_keypoint_name_list: [articulated_object_head]
+
+constraint_list:
+# ensure the gripper to hold the faucet handle with fingers
+- keypoint_name: tool_head
+  target_keypoint_name: articulated_object_head
+  tolerance: 0.0001
+  type: point2point_constraint
+
+# ensure the gripper to point straight to the table
+- axis_from_keypoint_name: tool_head 
+  axis_to_keypoint_name: tool_tail
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  target_inner_product: 1
+  type: frame_axis_parallel
+
+# ensure the axis between gripper finger tips to be parallel to the table
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: world
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+  target_inner_product: 0
+
+# ensure the axis between gripper finger tips to be orthogonal to the faucet handle
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [0, 0, 1.0]
+  target_axis_frame: object
+  tolerance: 0.01
+  type: frame_axis_orthogonal
+  target_inner_product: 0
+
+- axis_from_keypoint_name: tool_head
+  axis_to_keypoint_name: tool_side
+  target_axis: [1.0, 0, 0]
+  target_axis_frame: object
+  tolerance: 0.01
+  type: frame_axis_parallel
+  target_inner_product: 1
+
+
+
+# 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.
+
+# pre-actuation pose list. 
+pre_actuation_motions: [["translate_y", 0.12], ["translate_z", -0.15]] # [move above handle]
+
+# post-actuation pose list. pushing down
+post_actuation_motions: [["translate_z", 0.05], ["translate_y", -0.08], ["translate_x", 0.05]] # [turn the handle by 90 degree from left to right, turn the handle by 180 degree from left to right]
+
+# trajectory time
+actuation_time: 16  # time to reach task goal pose
+pre_actuation_times: [8, 12]
+post_actuation_times: [20, 24, 28]

workflows/simbox/solver/kpam/config/examples/constraint_lib.json

@@ -0,0 +1,277 @@
+[
+    {
+        "constraint_list": [
+            {
+                "keypoint_name": "tool_tail",
+                "target_keypoint_name": "articulated_object_head",
+                "tolerance": 0.0001,
+                "type": "point2point_constraint"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_side",
+                "target_axis": [
+                    1.0,
+                    0,
+                    0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": 1,
+                "type": "frame_axis_parallel"
+            },
+            {
+                "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": -1,
+                "type": "keypoint_axis_parallel"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_side",
+                "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",
+                "target_axis": [
+                    1.0,
+                    0,
+                    0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": 0,
+                "type": "frame_axis_orthogonal"
+            }
+        ]
+    },
+
+    {
+        "constraint_list": [
+            {
+                "keypoint_name": "tool_head",
+                "target_keypoint_name": "articulated_object_head",
+                "tolerance": 0.0001,
+                "type": "point2point_constraint"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_side",
+                "target_axis": [
+                    1.0,
+                    0,
+                    0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": 1,
+                "type": "frame_axis_parallel"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_tail",
+                "target_axis": [
+                    0,
+                    0,
+                    1.0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": 0,
+                "type": "frame_axis_orthogonal"
+            }
+        ]
+    },
+
+    {
+        "constraint_list": [
+            {
+                "keypoint_name": "tool_tail",
+                "target_keypoint_name": "articulated_object_head",
+                "tolerance": 0.0001,
+                "type": "point2point_constraint"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_side",
+                "target_axis": [
+                    1.0,
+                    0,
+                    0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": 1,
+                "type": "frame_axis_parallel"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_tail",
+                "target_axis": [
+                    0,
+                    0,
+                    1.0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": 0,
+                "type": "frame_axis_orthogonal"
+            }
+        ]
+    },
+
+    {
+        "constraint_list": [
+            {
+                "keypoint_name": "tool_head",
+                "target_keypoint_name": "rigidbody_object_head",
+                "tolerance": 0.0001,
+                "type": "point2point_constraint"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_side",
+                "target_axis": [
+                    1.0,
+                    0,
+                    0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": -1,
+                "type": "frame_axis_parallel"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_tail",
+                "target_axis": [
+                    0,
+                    0,
+                    1.0
+                ],
+                "target_axis_frame": "world",
+                "tolerance": 0.01,
+                "target_inner_product": 1,
+                "type": "frame_axis_parallel"
+            }
+        ]
+    },
+
+    {
+        "constraint_list": [
+            {
+                "keypoint_name": "tool_head",
+                "target_keypoint_name": "rigidbody_object_head",
+                "tolerance": 0.001,
+                "type": "point2point_constraint"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_side",
+                "target_axis": [
+                    0,
+                    1.0,
+                    0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": -1,
+                "type": "frame_axis_parallel"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_tail",
+                "target_axis": [
+                    0,
+                    0,
+                    1.0
+                ],
+                "target_axis_frame": "world",
+                "tolerance": 0.01,
+                "target_inner_product": 1,
+                "type": "frame_axis_parallel"
+            }
+        ]
+    },
+    {
+        "constraint_list": [
+            {
+                "keypoint_name": "tool_tail",
+                "target_keypoint_name": "articulated_object_head",
+                "tolerance": 0.0001,
+                "type": "point2point_constraint"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_side",
+                "target_axis": [
+                    0,
+                    1.0,
+                    0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": -1,
+                "type": "frame_axis_parallel"
+            },
+            {
+                "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": 1,
+                "type": "frame_axis_parallel"
+            }
+        ]
+    },
+    {
+        "constraint_list": [
+            {
+                "keypoint_name": "tool_tail",
+                "target_keypoint_name": "articulated_object_head",
+                "tolerance": 0.0001,
+                "type": "point2point_constraint"
+            },
+            {
+                "axis_from_keypoint_name": "tool_head",
+                "axis_to_keypoint_name": "tool_side",
+                "target_axis": [
+                    0,
+                    1.0,
+                    0
+                ],
+                "target_axis_frame": "object",
+                "tolerance": 0.01,
+                "target_inner_product": 1,
+                "type": "frame_axis_parallel"
+            },
+            {
+                "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": -1,
+                "type": "frame_axis_parallel"
+            }
+        ]
+    }
+    
+]

workflows/simbox/solver/kpam/config/examples/post_actuation_lib.json

@@ -0,0 +1,13 @@
+[
+    {
+        "post_actuation_motions": ["move-backward", "move-left"]
+    },
+
+    {
+        "post_actuation_motions": ["move-backward"]
+    },
+
+    {
+        "post_actuation_motions": ["move-forward", "move-up"]
+    }
+]

workflows/simbox/solver/kpam/config/examples/pre_actuation_lib.json

@@ -0,0 +1,21 @@
+[
+    {
+        "pre_actuation_motions": [["translate_x", -0.1], ["translate_z", -0.15]]
+    },
+
+    {
+        "pre_actuation_motions": [["translate_x", 0.1], ["translate_z", -0.1]]
+    },
+
+    {
+        "pre_actuation_motions": [["translate_z", -0.1]]
+    },
+
+    {
+        "pre_actuation_motions": [["translate_z", -0.15], ["translate_x", -0.1]]
+    },
+
+    {
+        "pre_actuation_motions": [["translate_z", -0.15], ["translate_x", 0.1]]
+    }
+]

workflows/simbox/solver/kpam/mp_builder.py

@@ -0,0 +1,319 @@
+import functools
+
+import numpy as np
+from solver.kpam import mp_terms, term_spec
+from solver.kpam.optimization_problem import OptimizationProblemkPAM
+from solver.kpam.transformations import affine_matrix_from_points
+
+
+class OptimizationBuilderkPAM(object):
+    def __init__(self, specification):
+        self._specification = specification
+
+    def _check_and_get_keypoint_idx(self, keypoint_name, input_idx=-1):
+        # type: (str, int) -> int
+        """
+        Given the keypoint name, return the index for that keypoint.
+        If input_idx is valid, also verify two index match each other
+        :param keypoint_name:
+        :param input_idx:
+        :return:
+        """
+        assert keypoint_name in self._specification.keypoint_name2idx
+        idx = self._specification.keypoint_name2idx[keypoint_name]
+
+        if input_idx >= 0:
+            assert idx == input_idx
+        return idx
+
+    def build_optimization(self, keypoint_observation):  # type: (np.ndarray) -> OptimizationProblemkPAM
+        """
+        Given the observed keypoint, build a mathematical program whose
+        solution is the transform that map keypoint_observation to target
+        :param keypoint_observation: (N, 3) keypoint location
+        :return:
+        """
+        # Basic check
+        assert keypoint_observation.shape[1] == 3
+        assert keypoint_observation.shape[0] == len(self._specification.keypoint_name2idx)
+
+        # Empty initialization of optimization problem
+        problem = OptimizationProblemkPAM()
+        problem.T_init = np.eye(4)
+        problem.mp = None
+        problem.xyzrpy = None
+        problem.has_solution = False
+
+        # Build the initial transformation
+        problem.T_init = self._build_initial_transformation(keypoint_observation)
+
+        # Build the mathematical_program with decision variable
+        problem.build_empty_mp()
+
+        # Process the cost terms
+        for cost in self._specification.cost_list:
+            self._process_cost_term(problem, keypoint_observation, cost)
+
+        # Process the constraint terms
+        for constraint in self._specification.constraint_list:
+            self._process_constraint_term(problem, keypoint_observation, constraint)
+
+        return problem
+
+    def _build_initial_transformation(self, keypoint_observation):  # type: (np.ndarray) -> np.ndarray
+        # The container for all keypoint that has nominal target
+        from_keypoint = []
+        to_keypoint = []
+
+        # Get all the keypoint that has nominal target
+        keypoint_name2idx = self._specification.keypoint_name2idx
+        for keypoint_name in keypoint_name2idx:
+            exist_target, location = self._specification.get_nominal_target_position(keypoint_name)
+            if exist_target:
+                to_keypoint.append(location)
+                idx = keypoint_name2idx[keypoint_name]
+                from_keypoint.append(
+                    [
+                        keypoint_observation[idx, 0],
+                        keypoint_observation[idx, 1],
+                        keypoint_observation[idx, 2],
+                    ]
+                )
+
+        # Depends on whether we have enough keypoints
+        assert len(from_keypoint) == len(to_keypoint)
+        if len(from_keypoint) < 3:
+            return np.eye(4)
+
+        # There exists enough keypoint, let's compute using karbas algorithm
+        # The np array are (3, N) format of keypoints
+        from_keypoint_np = np.zeros(shape=(3, len(from_keypoint)))
+        to_keypoint_np = np.zeros_like(from_keypoint_np)
+        for i in range(len(from_keypoint)):
+            for j in range(3):
+                from_keypoint_np[j, i] = from_keypoint[i][j]
+                to_keypoint_np[j, i] = to_keypoint[i][j]
+        T = affine_matrix_from_points(from_keypoint_np, to_keypoint_np, shear=False, scale=False, usesvd=True)
+        return T
+
+    def _process_cost_term(self, problem, keypoint_observation, cost):
+        # type: (OptimizationProblemkPAM, np.ndarray, term_spec.OptimizationTermSpec) -> None
+        """
+        Global dispatcher for the cost term
+        :param problem:
+        :param cost:
+        :return:
+        """
+        if isinstance(cost, term_spec.Point2PointCostL2Spec):
+            self._process_keypoint_l2_cost_term(problem, keypoint_observation, cost)
+        elif isinstance(cost, term_spec.Point2PlaneCostSpec):
+            self._process_point2plane_cost_term(problem, keypoint_observation, cost)
+        else:
+            raise RuntimeError("The cost term type is unknown/not implemented")
+
+    def _process_constraint_term(self, problem, keypoint_observation, constraint):
+        # type: (OptimizationProblemkPAM, np.ndarray, term_spec.OptimizationTermSpec) -> None
+        """
+        Global dispatcher for constraint term
+        :param problem:
+        :param keypoint_observation:
+        :param constraint:
+        :return:
+        """
+        if isinstance(constraint, term_spec.Point2PointConstraintSpec):
+            self._process_point2point_constraint_term(problem, keypoint_observation, constraint)
+        elif isinstance(constraint, term_spec.AxisAlignmentConstraintSpec):
+            self._process_axis_alignment_constraint_term(problem, constraint)
+        elif isinstance(constraint, term_spec.KeypointAxisAlignmentConstraintSpec):
+            self._process_keypoint_axisalign_constraint_term(problem, keypoint_observation, constraint)
+        elif isinstance(constraint, term_spec.KeypointAxisOrthogonalConstraintSpec):
+            self._process_keypoint_axisorthogonal_constraint_term(problem, keypoint_observation, constraint)
+        else:
+            raise RuntimeError("The constraint type is unknown/not implemented")
+
+    def _process_keypoint_l2_cost_term(self, problem, keypoint_observation, cost):
+        # type: (OptimizationProblemkPAM, np.ndarray, term_spec.Point2PointCostL2Spec) -> None
+        idx = self._check_and_get_keypoint_idx(cost.keypoint_name, cost.keypoint_idx)
+
+        # The current location
+        from_keypoints = keypoint_observation[idx, :]
+        from_keypoints = np.reshape(from_keypoints, (1, 3))
+
+        # The target location
+        to_keypoints = np.zeros(shape=(1, 3))
+        to_keypoints[0, 0] = cost.target_position[0]
+        to_keypoints[0, 1] = cost.target_position[1]
+        to_keypoints[0, 2] = cost.target_position[2]
+
+        # The cost function
+        # It is wired that the cost term cannot use symbolic expression
+        cost_func = functools.partial(
+            mp_terms.keypoint_l2_cost,
+            problem,
+            from_keypoints,
+            to_keypoints,
+            cost.penalty_weight,
+        )
+        problem.mp.AddCost(cost_func, problem.xyzrpy)
+
+    def _process_point2plane_cost_term(self, problem, keypoint_observation, cost):
+        # type: (OptimizationProblemkPAM, np.ndarray, term_spec.Point2PlaneCostSpec) -> None
+        idx = self._check_and_get_keypoint_idx(cost.keypoint_name, cost.keypoint_idx)
+
+        # The current location
+        from_keypoints = keypoint_observation[idx, :]
+        from_keypoints = np.reshape(from_keypoints, (1, 3))
+
+        # The target location
+        to_keypoints = np.zeros(shape=(1, 3))
+        to_keypoints[0, 0] = cost.target_position[0]
+        to_keypoints[0, 1] = cost.target_position[1]
+        to_keypoints[0, 2] = cost.target_position[2]
+
+        # The cost function
+        # It is wired that the cost term cannot use symbolic expression
+        cost_func = functools.partial(
+            mp_terms.point2plane_cost,
+            problem,
+            from_keypoints,
+            to_keypoints,
+            cost.penalty_weight,
+            cost.plane_normal,
+        )
+        problem.mp.AddCost(cost_func, problem.xyzrpy)
+
+    def _process_point2point_constraint_term(self, problem, keypoint_observation, constraint):
+        # type: (OptimizationProblemkPAM, np.ndarray, term_spec.Point2PointConstraintSpec) -> None
+        idx = self._check_and_get_keypoint_idx(constraint.keypoint_name, constraint.keypoint_idx)
+
+        # The current location
+        from_keypoint = keypoint_observation[idx, :]
+        transformed_point_symbolic_val = mp_terms.transform_point_symbolic(problem, from_keypoint, problem.xyzrpy)
+
+        # The target location
+        to_keypoints = np.asarray(constraint.target_position).copy()
+        to_keypoints_lb = to_keypoints - constraint.tolerance
+        to_keypoints_ub = to_keypoints + constraint.tolerance
+        for j in range(3):
+            problem.mp.AddConstraint(transformed_point_symbolic_val[j] >= to_keypoints_lb[j])
+            problem.mp.AddConstraint(transformed_point_symbolic_val[j] <= to_keypoints_ub[j])
+
+    @staticmethod
+    def _process_axis_alignment_constraint_term(problem, constraint):
+        # type: (OptimizationProblemkPAM, np.ndarray, term_spec.AxisAlignmentConstraintSpec) -> None
+        assert constraint.tolerance < 1
+        assert constraint.tolerance > 0
+
+        # Check the axis
+        from_axis = np.asarray(constraint.from_axis, dtype=np.float32).copy()  # type: np.ndarray
+        to_axis = np.asarray(constraint.target_axis, dtype=np.float32).copy()  # type: np.ndarray
+        norm_from = np.linalg.norm(from_axis)
+        norm_to = np.linalg.norm(to_axis)
+        if norm_from < 1e-4 or norm_to < 1e-4:
+            print("Warning: the axis is zero, the constraint is not added to optimization.")
+            return
+
+        # Do normalization
+        from_axis *= 1.0 / norm_from
+        to_axis *= 1.0 / norm_to
+
+        # Build the term
+        vector_dot_symbolic_val = mp_terms.vector_dot_symbolic(problem, from_axis, to_axis, problem.xyzrpy)
+
+        # Add to mp
+        problem.mp.AddConstraint(
+            vector_dot_symbolic_val >= constraint.target_inner_product - constraint.tolerance
+        )  # - constraint.target_inner_product   # ← One space after #
+        problem.mp.AddConstraint(
+            vector_dot_symbolic_val <= constraint.target_inner_product + constraint.tolerance
+        )  # - constraint.target_inner_product   # ← One space after #
+
+    def _process_keypoint_axisalign_constraint_term(self, problem, keypoint_observation, constraint):
+        # type: (OptimizationProblemkPAM, np.ndarray, term_spec.KeypointAxisAlignmentConstraintSpec) -> None
+        from_idx = self._check_and_get_keypoint_idx(
+            constraint.axis_from_keypoint_name, constraint.axis_from_keypoint_idx
+        )
+        to_idx = self._check_and_get_keypoint_idx(constraint.axis_to_keypoint_name, constraint.axis_to_keypoint_idx)
+
+        # Compute the axis
+        from_point = keypoint_observation[from_idx, :]
+        to_point = keypoint_observation[to_idx, :]
+        vector = to_point - from_point
+        norm_vec = np.linalg.norm(vector)
+        if norm_vec < 1e-4:
+            print("Warning: the axis is zero, the constraint is not added to optimization.")
+            return
+        vector *= 1.0 / norm_vec
+
+        # Build axis alignment constraint
+        axis_constraint = term_spec.AxisAlignmentConstraintSpec()
+        axis_constraint.from_axis = [vector[0], vector[1], vector[2]]
+        axis_constraint.target_axis = constraint.target_axis
+        axis_constraint.tolerance = constraint.tolerance
+        axis_constraint.target_inner_product = constraint.target_inner_product
+
+        # OK
+        self._process_axis_alignment_constraint_term(problem, axis_constraint)
+
+    @staticmethod
+    def _process_axis_orthogonal_constraint_term(problem, constraint):
+        # make the axis orthogonal as the constraint
+        # type: (OptimizationProblemkPAM, np.ndarray, term_spec.AxisAlignmentConstraintSpec) -> None
+        assert constraint.tolerance < 1
+        assert constraint.tolerance > 0
+
+        # Check the axis
+        from_axis = np.asarray(constraint.from_axis, dtype=np.float32).copy()  # type: np.ndarray
+        to_axis = np.asarray(constraint.target_axis, dtype=np.float32).copy()  # type: np.ndarray
+        norm_from = np.linalg.norm(from_axis)
+        norm_to = np.linalg.norm(to_axis)
+        if norm_from < 1e-4 or norm_to < 1e-4:
+            print("Warning: the axis is zero, the constraint is not added to optimization.")
+            return
+
+        # Do normalization
+        from_axis *= 1.0 / norm_from
+        to_axis *= 1.0 / norm_to
+
+        # Build the term
+        vector_dot_symbolic_val = mp_terms.vector_dot_symbolic(problem, from_axis, to_axis, problem.xyzrpy)
+
+        # Add to mp
+        problem.mp.AddConstraint(
+            vector_dot_symbolic_val - constraint.target_inner_product <= constraint.tolerance
+        )  # 1.0 -
+        problem.mp.AddConstraint(
+            vector_dot_symbolic_val - constraint.target_inner_product >= -constraint.tolerance
+        )  # 1.0 -
+
+    def _process_keypoint_axisorthogonal_constraint_term(self, problem, keypoint_observation, constraint):
+        # type: (OptimizationProblemkPAM, np.ndarray, term_spec.KeypointAxisAlignmentConstraintSpec) -> None
+        from_idx = self._check_and_get_keypoint_idx(
+            constraint.axis_from_keypoint_name, constraint.axis_from_keypoint_idx
+        )
+        to_idx = self._check_and_get_keypoint_idx(constraint.axis_to_keypoint_name, constraint.axis_to_keypoint_idx)
+
+        # Compute the axis
+        from_point = keypoint_observation[from_idx, :]
+        to_point = keypoint_observation[to_idx, :]
+        vector = to_point - from_point
+        norm_vec = np.linalg.norm(vector)
+        if norm_vec < 1e-4:
+            print("Warning: the axis is zero, the constraint is not added to optimization.")
+            return
+        vector *= 1.0 / norm_vec
+
+        # Build axis alignment constraint
+        axis_constraint = term_spec.AxisAlignmentConstraintSpec()
+        axis_constraint.from_axis = [vector[0], vector[1], vector[2]]
+        axis_constraint.target_axis = constraint.target_axis
+        axis_constraint.tolerance = constraint.tolerance
+        axis_constraint.target_inner_product = constraint.target_inner_product
+
+        # OK
+        self._process_axis_orthogonal_constraint_term(problem, axis_constraint)
+
+
+def test_builder():
+    """Legacy test entry; no longer used."""
+    raise RuntimeError("test_builder is deprecated and should not be called.")

workflows/simbox/solver/kpam/mp_terms.py

@@ -0,0 +1,75 @@
+import numpy as np
+from solver.kpam import SE3_utils
+from solver.kpam.optimization_problem import OptimizationProblemkPAM
+
+
+def keypoint_l2_cost(problem, from_keypoints, goal_keypoints, weight, xyzrpy):
+    # type: (OptimizationProblemkPAM, np.ndarray, np.ndarray, float, np.ndarray) -> np.ndarray
+    """
+    :param from_keypoints: (N, 3) np.ndarray with float
+    :param goal_keypoints: (N, 3) np.ndarray with float
+    :param weight: (N, 3) np.ndarray with float
+    :param xyzrpy: np.ndarray with potentially symbolic variable
+    :return: The cost value
+    """
+    cost = 0.0
+    T_eps = SE3_utils.xyzrpy_to_matrix(xyzrpy)
+    T = np.dot(problem.T_init, T_eps)
+
+    # The iteration on keypoints
+    num_keypoints = from_keypoints.shape[0]
+    for i in range(num_keypoints):
+        q = SE3_utils.transform_point(T, from_keypoints[i, :])
+        q_goal = goal_keypoints[i, :]
+        delta = q - q_goal
+        cost += np.dot(delta, delta)
+
+    return weight * cost
+
+
+def point2plane_cost(problem, from_keypoints, goal_keypoints, weight, plane_normal, xyzrpy):
+    cost = 0.0
+    T_eps = SE3_utils.xyzrpy_to_matrix(xyzrpy)
+    T = np.dot(problem.T_init, T_eps)
+
+    # The iteration on keypoints
+    num_keypoints = from_keypoints.shape[0]
+    for i in range(num_keypoints):
+        q = SE3_utils.transform_point(T, from_keypoints[i, :])
+        q_goal = goal_keypoints[i, :]
+        delta = q - q_goal
+        delta_normal = np.dot(delta, plane_normal)
+        cost += np.dot(delta_normal, delta_normal)
+
+    return weight * cost
+
+
+def vector_dot_symbolic(problem, from_axis, to_axis, xyzrpy):
+    # type: (OptimizationProblemkPAM, np.ndarray, np.ndarray, np.ndarray) -> np.ndarray
+    """
+    dot(T.dot(from_axis), to_axis)
+    :param problem:
+    :param from_axis:
+    :param to_axis:
+    :param xyzrpy:
+    :return:
+    """
+    T_eps = SE3_utils.xyzrpy_to_matrix_symbolic(xyzrpy)
+    T = np.dot(problem.T_init, T_eps)
+    R = T[0:3, 0:3]
+    transformed_from = np.dot(R, from_axis)
+    return np.dot(transformed_from, to_axis)
+
+
+def transform_point_symbolic(problem, from_keypoint, xyzrpy):
+    # type: (OptimizationProblemkPAM, np.ndarray, np.ndarray) -> np.ndarray
+    """
+    Transform the point using symbolic expression
+    :param problem:
+    :param from_keypoint: np.ndarray with float
+    :param xyzrpy: np.ndarray with symbolic variable
+    :return:
+    """
+    T_eps = SE3_utils.xyzrpy_to_matrix_symbolic(xyzrpy)
+    T = np.dot(problem.T_init, T_eps)
+    return SE3_utils.transform_point(T, from_keypoint)

workflows/simbox/solver/kpam/optimization_problem.py

@@ -0,0 +1,77 @@
+import numpy as np
+from pydrake.all import MathematicalProgram, Solve
+from solver.kpam import SE3_utils
+
+
+class OptimizationProblemkPAM(object):
+    # The property about initial transformation
+    # There is always an initial guess, either identity or from keypoint matching
+    T_init = np.ndarray(shape=(4, 4))  # type: np.ndarray
+
+    # The final mathematical program
+    mp = None  # type: MathematicalProgram
+    xyzrpy = None  # type: np.ndarray
+
+    # The solution to the program
+    has_solution = False
+    xyzrpy_sol = np.ndarray(shape=(6,))
+    # T_action.dot(observed_keypoint) = target_keypoint
+    T_action = np.ndarray(shape=(4, 4))
+
+    # Build empty problem
+    def build_empty_mp(self):
+        # Construct the problem
+        mp = MathematicalProgram()
+        xyz = mp.NewContinuousVariables(3, "xyz")
+        rpy = mp.NewContinuousVariables(3, "rpy")
+        xyz_rpy = np.concatenate((xyz, rpy))
+        mp.SetInitialGuessForAllVariables(np.zeros(6))
+
+        # Store the result to problem
+        self.mp = mp
+        self.xyzrpy = xyz_rpy
+
+
+class OptimizationProblemkPAMJoint(object):
+    # The property about initial transformation
+    # There is always an initial guess, either identity or from keypoint matching
+    T_init = np.ndarray(shape=(4, 4))  # type: np.ndarray
+
+    # The final mathematical program
+    mp = None  # type: MathematicalProgram
+    xyzrpy = None  # type: np.ndarray
+
+    # The solution to the program
+    has_solution = False
+    xyzrpy_sol = np.ndarray(shape=(6,))
+    # T_action.dot(observed_keypoint) = target_keypoint
+    T_action = np.ndarray(shape=(4, 4))
+
+    # Build empty problem
+    def build_empty_mp(self):
+        # Construct the problem
+        mp = MathematicalProgram()
+        xyz = mp.NewContinuousVariables(3, "xyz")
+        rpy = mp.NewContinuousVariables(3, "rpy")
+        xyz_rpy = np.concatenate((xyz, rpy))
+        mp.SetInitialGuessForAllVariables(np.zeros(6))
+
+        # Store the result to problem
+        self.mp = mp
+        self.xyzrpy = xyz_rpy
+
+
+def solve_kpam(problem):  # type: (OptimizationProblemkPAM) -> bool
+    result = Solve(problem.mp)  # mp.
+    if not result.is_success():
+        problem.has_solution = False
+        return False
+
+    # Save the result to problem
+    problem.xyzrpy_sol = result.get_x_val()  # problem.mp.GetSolution(problem.xyzrpy)
+    T_eps = SE3_utils.xyzrpy_to_matrix(xyzrpy=problem.xyzrpy_sol)
+    problem.T_action = np.dot(problem.T_init, T_eps)
+    problem.has_solution = True
+
+    # OK
+    return True

workflows/simbox/solver/kpam/optimization_spec.py

@@ -0,0 +1,219 @@
+import copy
+from typing import List
+
+import yaml
+from solver.kpam import term_spec
+from solver.kpam.term_spec import OptimizationTermSpec
+
+
+class OptimizationProblemSpecification(object):
+    """
+    The class serves as the interface between the
+    config file and solver
+    """
+
+    def __init__(
+        self,
+        task_name="",  # type: str
+        category_name="",  # type: str
+        tool_keypoint_name_list=None,  # type: List[str]
+        object_keypoint_name_list=None,  # type: List[str]
+    ):
+        """
+        The optimization spec can be constructed in python code
+        or load from yaml config path. In latter case, these
+        parameters can left default and use load_from_config method
+        :param task_name:
+        :param category_name:
+        :param keypoint_name_list:
+        """
+        self._task_name = task_name  # type: str
+        self._category_name = category_name  # type: str
+
+        # The default construction of list
+        if tool_keypoint_name_list is not None:
+            self._tool_keypoint_name_list = tool_keypoint_name_list  # type: List[str]
+        else:
+            self._tool_keypoint_name_list = []
+
+        if object_keypoint_name_list is not None:
+            self._object_keypoint_name_list = object_keypoint_name_list  # type: List[str]
+        else:
+            self._object_keypoint_name_list = []
+
+        # By default, nothing here
+        self._cost_list = []  # type: List[OptimizationTermSpec]
+        self._constraint_list = []  # type: List[OptimizationTermSpec]
+
+        # # Build from keypoint name list
+        # self._keypoint_name2idx = OrderedDict()  # type: Dict[str, int]
+        # self.setup_keypoint_mapping()
+
+        # The container for explicit nominal position
+        # The nominal position can either explicit declared
+        # Or implicit added using Point2PointCost/Constraint
+        # self._keypoint_nominal_target_position = OrderedDict()  # type: Dict[str, List[float]]
+
+    # def setup_keypoint_mapping(self):
+    #     self._keypoint_name2idx.clear()
+    #     for i in range(len(self._keypoint_name_list)):
+    #         name = self._keypoint_name_list[i]
+    #         self._keypoint_name2idx[name] = i
+
+    # The access interface
+    @property
+    def task_name(self):
+        return self._task_name
+
+    @property
+    def category_name(self):
+        return self._category_name
+
+    # @property
+    # def keypoint_name2idx(self):
+    #     return self._keypoint_name2idx
+
+    @property
+    def cost_list(self):
+        return self._cost_list
+
+    @property
+    def constraint_list(self):
+        return self._constraint_list
+
+    # # The method to manipulate nominal target
+    # def add_nominal_target_position(self, keypoint_name, nominal_target):  # type: (str, List[float]) -> bool
+    #     # Check the existence of keypoint
+    #     if keypoint_name not in self._keypoint_name2idx:
+    #         return False
+
+    #     # Check the shape of target
+    #     if len(nominal_target) != 3:
+    #         return False
+
+    #     # OK
+    #     self._keypoint_nominal_target_position[keypoint_name] = nominal_target
+    #     return True
+
+    # def get_nominal_target_position(self, keypoint_name):  # type: (str) -> (bool, List[float])
+    #     # If explicitly defined
+    #     if keypoint_name in self._keypoint_nominal_target_position:
+    #         return True, self._keypoint_nominal_target_position[keypoint_name]
+
+    #     # Else search for constraints
+    #     for cost_term in self.cost_list:
+    #         if isinstance(cost_term, Point2PointCostL2Spec):
+    #             if cost_term.keypoint_name == keypoint_name:
+    #                 return True, cost_term.target_position
+    #     for constraint_term in self.constraint_list:
+    #         if isinstance(constraint_term, Point2PointConstraintSpec):
+    #             if constraint_term.keypoint_name == keypoint_name:
+    #                 return True, constraint_term.target_position
+
+    #     # Not available
+    #     return False, []
+
+    # The method to modify the specification from python
+    def add_cost(self, cost_term):  # type: (OptimizationTermSpec) -> bool
+        if not cost_term.is_cost():
+            return False
+        copied = copy.deepcopy(cost_term)
+        self._cost_list.append(copied)
+        return True
+
+    def add_constraint(self, constraint_term):  # type: (OptimizationTermSpec) -> bool
+        if constraint_term.is_cost():
+            return False
+        copied = copy.deepcopy(constraint_term)
+        self._constraint_list.append(copied)
+        return True
+
+    def add_optimization_term(self, optimization_term):  # type: (OptimizationTermSpec) -> bool
+        if optimization_term.is_cost():
+            return self.add_cost(optimization_term)
+        else:
+            return self.add_constraint(optimization_term)
+
+    # The interface from/to yaml
+    def write_to_yaml(self, yaml_save_path):  # type: (str) -> None
+        data_map = {}
+        data_map["task_name"] = self._task_name
+        data_map["category_name"] = self._category_name
+        data_map["tool_keypoint_name_list"] = self._tool_keypoint_name_list
+        data_map["object_keypoint_name_list"] = self._object_keypoint_name_list
+
+        # # For cost terms
+        # cost_map_list = []
+        # for cost in self._cost_list:
+        #     cost_i_map = cost.to_dict()
+        #     cost_i_map["type"] = cost.type_name()
+        #     cost_map_list.append(cost_i_map)
+        # data_map["cost_list"] = cost_map_list
+
+        # For constraint terms
+        constraint_map_list = []
+        for constraint in self._constraint_list:
+            constraint_i_map = constraint.to_dict()
+            constraint_i_map["type"] = constraint.type_name()
+            constraint_map_list.append(constraint_i_map)
+        data_map["constraint_list"] = constraint_map_list
+
+        # Save to yaml
+        with open(yaml_save_path, mode="w", encoding="utf-8") as save_file:
+            yaml.dump(data_map, save_file)
+
+    def load_from_config(self, data_map):  # type: (str) -> bool
+        # Basic meta
+        self._task_name = data_map["task_name"]
+        self._category_name = data_map["category_name"]
+        self._tool_keypoint_name_list = data_map["tool_keypoint_name_list"]
+        self._object_keypoint_name_list = data_map["object_keypoint_name_list"]
+        # self._keypoint_name2idx.clear()
+        # self.setup_keypoint_mapping()
+
+        # For cost terms
+
+        # cost_map_list = data_map["cost_list"]
+        # self._cost_list = []
+        # for cost in cost_map_list:
+
+        #     cost_type = cost["type"]  # type: str
+        #     if cost_type == term_spec.Point2PointCostL2Spec.type_name():
+        #         cost_spec = term_spec.Point2PointCostL2Spec()
+        #         cost_spec.from_dict(cost)
+        #         self._cost_list.append(cost_spec)
+        #     elif cost_type == term_spec.Point2PlaneCostSpec.type_name():
+        #         cost_spec = term_spec.Point2PlaneCostSpec()
+        #         cost_spec.from_dict(cost)
+        #         self._cost_list.append(cost_spec)
+        #     else:
+        #         raise RuntimeError("Unknown cost type %s" % cost_type)
+
+        # For constraint terms
+        constraint_map_list = data_map["constraint_list"]
+        self._constraint_list = []
+        for constraint in constraint_map_list:
+            constraint_type = constraint["type"]
+            if constraint_type == term_spec.Point2PointConstraintSpec.type_name():
+                constraint_spec = term_spec.Point2PointConstraintSpec()
+                constraint_spec.from_dict(constraint)
+                self._constraint_list.append(constraint_spec)
+            elif constraint_type == term_spec.KeypointAxisParallelConstraintSpec.type_name():
+                constraint_spec = term_spec.KeypointAxisParallelConstraintSpec()
+                constraint_spec.from_dict(constraint)
+                self._constraint_list.append(constraint_spec)
+            elif constraint_type == term_spec.KeypointAxisOrthogonalConstraintSpec.type_name():
+                constraint_spec = term_spec.KeypointAxisOrthogonalConstraintSpec()
+                constraint_spec.from_dict(constraint)
+                self._constraint_list.append(constraint_spec)
+            elif constraint_type == term_spec.FrameAxisParallelConstraintSpec.type_name():
+                constraint_spec = term_spec.FrameAxisParallelConstraintSpec()
+                constraint_spec.from_dict(constraint)
+                self._constraint_list.append(constraint_spec)
+            elif constraint_type == term_spec.FrameAxisOrthogonalConstraintSpec.type_name():
+                constraint_spec = term_spec.FrameAxisOrthogonalConstraintSpec()
+                constraint_spec.from_dict(constraint)
+                self._constraint_list.append(constraint_spec)
+            else:
+                raise RuntimeError(f"Unknown constraint type {constraint_type}")
+        return True

workflows/simbox/solver/kpam/term_spec.py

@@ -0,0 +1,294 @@
+from typing import List
+
+import attr
+
+
+# A very thin base class for all
+# the specification of the terms
+class OptimizationTermSpec(object):
+    # The type of this method
+    @staticmethod
+    def type_name():  # type: () -> str
+        raise NotImplementedError
+
+    @staticmethod
+    def is_cost():  # type: () -> bool
+        raise NotImplementedError
+
+    # The method for serialization
+    def to_dict(self):
+        raise NotImplementedError
+
+    def from_dict(self, data_map):
+        raise NotImplementedError
+
+
+# For the specification of constraint types:
+# @attr.s
+# class Point2PointConstraintSpec(OptimizationTermSpec):
+#     # 1 - tolerance <= dot(from, to) <= 1
+#     keypoint_name = ""  # type: str
+#     keypoint_idx = -1  # type: int
+#     target_position = [0, 0, 0]  # type: List[float]
+#     tolerance = 0.001  # type: float
+
+#     @staticmethod
+#     def type_name():
+#         return "point2point_constraint"
+
+#     @staticmethod
+#     def is_cost():  # type: () -> bool
+#         return False
+
+#     def to_dict(self):
+#         constraint_dict = dict()
+#         constraint_dict["keypoint_name"] = self.keypoint_name
+#         constraint_dict["keypoint_idx"] = self.keypoint_idx
+#         constraint_dict["target_position"] = self.target_position
+#         constraint_dict["tolerance"] = self.tolerance
+#         return constraint_dict
+
+#     def from_dict(self, data_map):
+#         self.keypoint_name = data_map["keypoint_name"]
+#         self.keypoint_idx = data_map["keypoint_idx"]
+#         self.target_position = data_map["target_position"]
+#         self.tolerance = data_map["tolerance"]
+
+
+@attr.s
+class Point2PointConstraintSpec(OptimizationTermSpec):
+    # 1 - tolerance <= dot(from, to) <= 1
+    keypoint_name = ""  # type: str
+    keypoint_idx = -1  # type: int
+    target_keypoint_name = ""  # type: List[float]
+    target_keypoint_idx = -1  # type: float
+    tolerance = 0.001  # type: float
+
+    @staticmethod
+    def type_name():
+        return "point2point_constraint"
+
+    @staticmethod
+    def is_cost():  # type: () -> bool
+        return False
+
+    def to_dict(self):
+        constraint_dict = {}
+        constraint_dict["keypoint_name"] = self.keypoint_name
+        constraint_dict["target_keypoint_name"] = self.target_keypoint_name
+        constraint_dict["tolerance"] = self.tolerance
+        return constraint_dict
+
+    def from_dict(self, data_map):
+        self.keypoint_name = data_map["keypoint_name"]
+        self.target_keypoint_name = data_map["target_keypoint_name"]
+        self.tolerance = data_map["tolerance"]
+
+
+@attr.s
+class KeypointAxisParallelConstraintSpec(OptimizationTermSpec):
+    axis_from_keypoint_name = ""  # type: str
+    axis_to_keypoint_name = ""  # type: str
+    target_axis_from_keypoint_name = ""  # type: str
+    target_axis_to_keypoint_name = ""  # type: str
+    tolerance = 0.1  # type: float
+    target_inner_product = 1
+
+    @staticmethod
+    def type_name():
+        return "keypoint_axis_parallel"
+
+    @staticmethod
+    def is_cost():  # type: () -> bool
+        return False
+
+    def to_dict(self):
+        constraint_dict = {}
+        constraint_dict["axis_from_keypoint_name"] = self.axis_from_keypoint_name
+        constraint_dict["axis_to_keypoint_name"] = self.axis_to_keypoint_name
+        constraint_dict["target_axis_from_keypoint_name"] = self.target_axis_from_keypoint_name
+        constraint_dict["target_axis_to_keypoint_name"] = self.target_axis_to_keypoint_name
+        constraint_dict["tolerance"] = self.tolerance
+        constraint_dict["target_inner_product"] = self.target_inner_product
+        return constraint_dict
+
+    def from_dict(self, data_map):
+        self.axis_from_keypoint_name = data_map["axis_from_keypoint_name"]
+        self.axis_to_keypoint_name = data_map["axis_to_keypoint_name"]
+        self.target_axis_from_keypoint_name = data_map["target_axis_from_keypoint_name"]
+        self.target_axis_to_keypoint_name = data_map["target_axis_to_keypoint_name"]
+        self.tolerance = data_map["tolerance"]
+        self.target_inner_product = data_map["target_inner_product"]
+
+
+@attr.s
+class KeypointAxisOrthogonalConstraintSpec(OptimizationTermSpec):
+    axis_from_keypoint_name = ""  # type: str
+    axis_to_keypoint_name = ""  # type: str
+    target_axis_from_keypoint_name = ""  # type: str
+    target_axis_to_keypoint_name = ""  # type: str
+    tolerance = 0.01  # type: float
+    target_inner_product = 0
+
+    @staticmethod
+    def type_name():
+        return "keypoint_axis_orthogonal"
+
+    @staticmethod
+    def is_cost():  # type: () -> bool
+        return False
+
+    def to_dict(self):
+        constraint_dict = {}
+        constraint_dict["axis_from_keypoint_name"] = self.axis_from_keypoint_name
+        constraint_dict["axis_to_keypoint_name"] = self.axis_to_keypoint_name
+        constraint_dict["target_axis_from_keypoint_name"] = self.target_axis_from_keypoint_name
+        constraint_dict["target_axis_to_keypoint_name"] = self.target_axis_to_keypoint_name
+        constraint_dict["tolerance"] = self.tolerance
+        constraint_dict["target_inner_product"] = self.target_inner_product
+        return constraint_dict
+
+    def from_dict(self, data_map):
+        self.axis_from_keypoint_name = data_map["axis_from_keypoint_name"]
+        self.axis_to_keypoint_name = data_map["axis_to_keypoint_name"]
+        self.target_axis_from_keypoint_name = data_map["target_axis_from_keypoint_name"]
+        self.target_axis_to_keypoint_name = data_map["target_axis_to_keypoint_name"]
+        self.tolerance = data_map["tolerance"]
+        self.target_inner_product = data_map["target_inner_product"]
+
+
+@attr.s
+class FrameAxisOrthogonalConstraintSpec(OptimizationTermSpec):
+    axis_from_keypoint_name = ""  # type: str
+    axis_to_keypoint_name = ""  # type: str
+    target_axis = [0, 0, 1]  # type: List[float]
+    target_axis_frame = ""
+    tolerance = 0.01  # type: float
+    target_inner_product = 0
+
+    @staticmethod
+    def type_name():
+        return "frame_axis_orthogonal"
+
+    @staticmethod
+    def is_cost():  # type: () -> bool
+        return False
+
+    def to_dict(self):
+        constraint_dict = {}
+        constraint_dict["axis_from_keypoint_name"] = self.axis_from_keypoint_name
+        constraint_dict["axis_to_keypoint_name"] = self.axis_to_keypoint_name
+        constraint_dict["target_axis"] = self.target_axis
+        constraint_dict["target_axis_frame"] = self.target_axis_frame
+        constraint_dict["tolerance"] = self.tolerance
+        constraint_dict["target_inner_product"] = self.target_inner_product
+        return constraint_dict
+
+    def from_dict(self, data_map):
+        self.axis_from_keypoint_name = data_map["axis_from_keypoint_name"]
+        self.axis_to_keypoint_name = data_map["axis_to_keypoint_name"]
+        self.target_axis = data_map["target_axis"]
+        self.target_axis_frame = data_map["target_axis_frame"]
+        self.tolerance = data_map["tolerance"]
+        self.target_inner_product = data_map["target_inner_product"]
+
+
+@attr.s
+class FrameAxisParallelConstraintSpec(OptimizationTermSpec):
+    axis_from_keypoint_name = ""  # type: str
+    axis_to_keypoint_name = ""  # type: str
+    target_axis = [0, 0, 1]  # type: List[float]
+    target_axis_frame = ""
+    tolerance = 0.1  # type: float
+    target_inner_product = 0
+
+    @staticmethod
+    def type_name():
+        return "frame_axis_parallel"
+
+    @staticmethod
+    def is_cost():  # type: () -> bool
+        return False
+
+    def to_dict(self):
+        constraint_dict = {}
+        constraint_dict["axis_from_keypoint_name"] = self.axis_from_keypoint_name
+        constraint_dict["axis_to_keypoint_name"] = self.axis_to_keypoint_name
+        constraint_dict["target_axis"] = self.target_axis
+        constraint_dict["target_axis_frame"] = self.target_axis_frame
+        constraint_dict["tolerance"] = self.tolerance
+        constraint_dict["target_inner_product"] = self.target_inner_product
+        return constraint_dict
+
+    def from_dict(self, data_map):
+        self.axis_from_keypoint_name = data_map["axis_from_keypoint_name"]
+        self.axis_to_keypoint_name = data_map["axis_to_keypoint_name"]
+        self.target_axis = data_map["target_axis"]
+        self.target_axis_frame = data_map["target_axis_frame"]
+        self.tolerance = data_map["tolerance"]
+        self.target_inner_product = data_map["target_inner_product"]
+
+
+# For the specification of cost terms
+@attr.s
+class Point2PointCostL2Spec(OptimizationTermSpec):
+    keypoint_name = ""  # type: str
+    keypoint_idx = -1  # type: int
+    target_position = [0, 0, 0]  # type: List[float]
+    penalty_weight = 1.0  # type: float
+
+    @staticmethod
+    def type_name():
+        return "point2point_cost"
+
+    @staticmethod
+    def is_cost():  # type: () -> bool
+        return True
+
+    def to_dict(self):
+        constraint_dict = {}
+        constraint_dict["keypoint_name"] = self.keypoint_name
+        constraint_dict["keypoint_idx"] = self.keypoint_idx
+        constraint_dict["target_position"] = self.target_position
+        constraint_dict["penalty_weight"] = self.penalty_weight
+        return constraint_dict
+
+    def from_dict(self, data_map):
+        self.keypoint_name = data_map["keypoint_name"]
+        self.keypoint_idx = data_map["keypoint_idx"]
+        self.target_position = data_map["target_position"]
+        self.penalty_weight = data_map["penalty_weight"]
+
+
+# For the specification of point2plane cost
+@attr.s
+class Point2PlaneCostSpec(OptimizationTermSpec):
+    keypoint_name = ""  # type: str
+    keypoint_idx = -1  # type: int
+    target_position = [0.0, 0.0, 0.0]  # type: List[float]
+    plane_normal = [0.0, 0.0, 1.0]  # type: List[float]
+    penalty_weight = 1.0  # type: float
+
+    @staticmethod
+    def type_name():  # type: () -> str
+        return "point2plane_cost"
+
+    @staticmethod
+    def is_cost():  # type: () -> bool
+        return True
+
+    def to_dict(self):
+        constraint_dict = {}
+        constraint_dict["keypoint_name"] = self.keypoint_name
+        constraint_dict["keypoint_idx"] = self.keypoint_idx
+        constraint_dict["target_position"] = self.target_position
+        constraint_dict["plane_normal"] = self.plane_normal
+        constraint_dict["penalty_weight"] = self.penalty_weight
+        return constraint_dict
+
+    def from_dict(self, data_map):
+        self.keypoint_name = data_map["keypoint_name"]
+        self.keypoint_idx = data_map["keypoint_idx"]
+        self.target_position = data_map["target_position"]
+        self.plane_normal = data_map["plane_normal"]
+        self.penalty_weight = data_map["penalty_weight"]