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
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
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
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