Example #1
0
def get_handle_pose(door, side):
  x_axis = kdl.Vector(1,0,0)
  
  dist = math.sqrt(
      math.pow(door.frame_p1.x - door.handle.x,2)
      +math.pow(door.frame_p1.y - door.handle.y,2))

  if door.hinge == Door.HINGE_P2:
    dist = math.sqrt(math.pow(door.frame_p2.x - door.handle.x,2)+math.pow(door.frame_p2.y - door.handle.y,2))

  angle = get_door_angle(door)

  # get hinge point

  if door.hinge == Door.HINGE_P1:
    hinge = point2kdl(door.door_p1)
    frame_vec = point2kdl(door.frame_p2) - point2kdl(door.frame_p1)
  else:
    hinge = point2kdl(door.door_p2)
    frame_vec = point2kdl(door.frame_p1) - point2kdl(door.frame_p2)

  # get gripper pos
  frame_vec.Normalize()
  frame_vec = frame_vec * dist
  rot_angle = kdl.Rotation.RotZ(angle)
  handle_pos = hinge + (rot_angle * frame_vec)
  
  # Construct handle pose
  frame_normal = get_frame_normal(door)
  if side == -1:
    frame_normal = -frame_normal

  handle_pose = kdl.Frame(
      kdl.Rotation.RPY(0,0,get_yaw_angle(x_axis, frame_normal)),
      handle_pos)

  gripper_rotate = kdl.Frame(
      kdl.Rotation.RPY(math.pi/2,0.0,0.0),
      kdl.Vector(0,0,0))
  
  handle_pose = handle_pose * gripper_rotate

  handle_pose_msg = PoseStamped()
  handle_pose_msg.header.frame_id = door.header.frame_id
  handle_pose_msg.header.stamp = door.header.stamp
  handle_pose_msg.pose = conversions.toMsg(handle_pose)

  return handle_pose_msg
Example #2
0
def get_robot_pose(door, dist):
  """Get a robot pose some distance from the door.
  @type door: door_msgs.msg.Door
  @rtype: geometry_msgs.msg.PoseStamped
  """
  x_axis = kdl.Vector(1,0,0)

  frame_1 = kdl.Vector(door.frame_p1.x, door.frame_p1.y, door.frame_p1.z)
  frame_2 = kdl.Vector(door.frame_p2.x, door.frame_p2.y, door.frame_p2.z)
  frame_center = (frame_1+frame_2)/2.0

  frame_normal = get_frame_normal(door)
  robot_pos = frame_center + (frame_normal * dist)

  robot_pose = kdl.Frame(
      kdl.Rotation.RPY(0,0,get_yaw_angle(x_axis, frame_normal)),
      robot_pos)

  robot_pose_msg = PoseStamped() 
  robot_pose_msg.header.frame_id = door.header.frame_id
  robot_pose_msg.header.stamp = door.header.stamp
  robot_pose_msg.pose = conversions.toMsg(robot_pose)
  
  return robot_pose_msg