Exemple #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
Exemple #2
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
Exemple #3
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
Exemple #4
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