def get_contacts(oracle, body_name, direction): # NOTE - Other objects will only have a fixed set of contacts and directions
#print 'direction', direction[2]
assert direction[2] == 0
contacts = []
direction = normalize(direction)
aabb = oracle.get_aabb(body_name)
radius = sqrt(oracle.get_radius2D2(body_name))
#radius = body.GetLinks()[0].GetGeometries()[0].GetCylinderRadius()
height = 2*aabb.extents()[2]
#height = body.GetLinks()[0].GetGeometries()[0].GetCylinderHeight()
distance = radius + PUSH_SEPERATION
z = -height/2 + PUSH_HEIGHT
tool_quat = quat_from_trans(get_tool_trans(oracle))
manip_point = -distance*direction + np.array([0, 0, z]) + aabb.pos()
for rotation in [0, PI]: # NOTE - 2 hand trans can push in a given direction
manip_quat = quat_dot(quat_look_at(-direction), quat_look_at(-unit_z()), quat_from_angle_vector(rotation, unit_x()), tool_quat) # Grip * Tool = Manip
contacts.append(Contact(compute_grasp(trans_from_quat_point(manip_quat, manip_point), unit_trans()), direction))
return contacts
def get_contacts(oracle, body_name, direction): # NOTE - Other objects will only have a fixed set of contacts and directions
#print 'direction', direction[2]
assert direction[2] == 0
contacts = []
direction = normalize(direction)
aabb = oracle.get_aabb(body_name)
radius = sqrt(oracle.get_radius2D2(body_name))
#radius = body.GetLinks()[0].GetGeometries()[0].GetCylinderRadius()
height = 2*aabb.extents()[2]
#height = body.GetLinks()[0].GetGeometries()[0].GetCylinderHeight()
distance = radius + PUSH_SEPERATION
z = -height/2 + PUSH_HEIGHT
tool_quat = quat_from_trans(get_tool_trans(oracle))
manip_point = -distance*direction + np.array([0, 0, z]) + aabb.pos()
for rotation in [0, PI]: # NOTE - 2 hand trans can push in a given direction
manip_quat = quat_dot(quat_look_at(-direction), quat_look_at(-unit_z()), quat_from_angle_vector(rotation, unit_x()), tool_quat) # Grip * Tool = Manip
contacts.append(Contact(compute_grasp(trans_from_quat_point(manip_quat, manip_point), unit_trans()), direction))
return contacts
# def box_contacts(oracle, body_name): # TODO - push boxes along their faces
def approach_vector_from_pose_contact(pose, contact): # TODO - universal way of inferring approach_vector from manip_trans (probably not possible) approach_vector = quat_transform_point(quat_from_trans(manip_trans_from_pose_contact(pose, contact)), APPROACH_VECTOR) if contact.grasp_trans[0, 3] > 0: approach_vector[:2] *= -1 return approach_vector
def approach_vector_from_pose_contact(pose, contact): # TODO - universal way of inferring approach_vector from manip_trans (probably not possible) approach_vector = quat_transform_point(quat_from_trans(manip_trans_from_pose_contact(pose, contact)), APPROACH_VECTOR) if contact.grasp_trans[0, 3] > 0: approach_vector[:2] *= -1 return approach_vector
