def calculateObjectPosition(x_pixel, y_pixel): # x_pixel offset = +5 and y_pixel offset = -5 # this is not done and should be set somewhere # Robots world coordinates seen from camera frame x_coordinate_robot_position = 34.7 y_coordinate_robot_position = 33.9 # Resolution in cm per pixel resolution_width = 70.0/611.0 resolution_height = 39.5/344.0 # Robots position seen from camera frame T1 = Affine.translation((x_coordinate_robot_position, y_coordinate_robot_position)) # Robots rotating frame opposite to camera frame - degrees rot = Affine.rotation(180) # Getting robot frame robot_frame = T1*rot # Actual distance seen from camera frame actual_distance_width = resolution_width*x_pixel actual_distance_height = resolution_height*y_pixel # Object position seen from camera frame P_yx_camera_frame = Vec2(actual_distance_height, actual_distance_width) # Object position seen from the robot frame P_xyz_robot_frame = ~robot_frame*P_yx_camera_frame return P_xyz_robot_frame
def main(): start_pos = Vec2(0.25,1.75) start_speed = Vec2(0.0,-0.1) start = RobotState(start_pos,start_speed,0.0) goal_pos = Vec2(2.75,1.75) goal_speed = Vec2(0.0,0.0) goal = RobotState(goal_pos,goal_speed,0.0) square_vertices = [Vec2(0,0),Vec2(0.5,0),Vec2(0.5,0.5),Vec2(0,0.5)] square = Polygon(square_vertices,True,True) obs1 = Affine.translation(Vec2(0.5,1.0)) * square obs2 = Affine.translation(Vec2(2.0,0.5)) * square env = Environment(Globals.PLAYGROUND_BORDER,[],[obs1,obs2]) #if poly_collides_poly(obs1,obs2): # print "(=" #else: # print ")=" #with PyCallGraph(output = GraphvizOutput()): path = aStar(start,goal,env) #print "start:\n {s}".format(s = start) #print "goal:\n {g}".format(g = goal) #for node in path: # print node while True: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() sys.exit() screen.fill(black) pygame.draw.lines(screen,white,True,convert_to_px_coords(env.border_as_tuple_list()),1) for obs in env.obstacles_as_tuples(): pygame.draw.lines(screen,red,True,convert_to_px_coords(obs),1) pygame.draw.lines(screen,green,False,convert_to_px_coords(Util.path_to_tuples(path)),1) for node in path: draw_state(node) pygame.display.update()
def get_orientation_ray(cls, perspective, landmark): standard_direction = Vec2(0, 1) top_primary_axes = landmark.get_top_parent().get_primary_axes() our_axis = None for axis in top_primary_axes: if axis.contains_point(perspective): our_axis = axis assert (our_axis != None) new_axis = our_axis.parallel(landmark.representation.middle) new_perspective = new_axis.project(perspective) p_segment = LineSegment.from_points( [new_perspective, landmark.representation.middle]) angle = standard_direction.angle_to(p_segment.vector) rotation = Affine.rotation(angle) o = [cls.orientation] rotation.itransform(o) direction = o[0] ori_ray = Ray(p_segment.end, direction) return ori_ray
def __init__(self, perspective, landmark, poi, orientation): super(OrientationRelation, self).__init__(perspective, landmark, poi) self.standard = Vec2(0,1) self.orientation = orientation top_primary_axes = landmark.get_top_parent().get_primary_axes() our_axis = None for axis in top_primary_axes: if axis.contains_point(perspective): our_axis = axis assert( our_axis != None ) new_axis = our_axis.parallel(self.landmark.representation.middle) new_perspective = new_axis.project(perspective) p_segment = LineSegment.from_points( [new_perspective, self.landmark.representation.middle] ) angle = self.standard.angle_to(p_segment.vector) rotation = Affine.rotation(angle) o = [self.orientation] rotation.itransform(o) direction = o[0] self.ori_ray = Ray(p_segment.end, direction) self.projected = self.ori_ray.line.project(poi) self.distance = self.ori_ray.start.distance_to(self.projected) self.measurement = Measurement(self.distance, required=False)
def get_orientation_ray(cls, perspective, landmark): standard_direction = Vec2(0,1) top_primary_axes = landmark.get_top_parent().get_primary_axes() our_axis = None for axis in top_primary_axes: if axis.contains_point(perspective): our_axis = axis assert( our_axis != None ) new_axis = our_axis.parallel(landmark.representation.middle) new_perspective = new_axis.project(perspective) p_segment = LineSegment.from_points( [new_perspective, landmark.representation.middle] ) angle = standard_direction.angle_to(p_segment.vector) rotation = Affine.rotation(angle) o = [cls.orientation] rotation.itransform(o) direction = o[0] ori_ray = Ray(p_segment.end, direction) return ori_ray
def _get_transform(self, dt): # Set rotation in analogue to direction of velocity position = Affine.translation(self._position + self._velocity * dt) return position
def _get_transform(self, dt): position = Affine.translation(self._position + self._velocity * dt) return position
def _get_transform(self, dt): # Set rotation in analogue to direction of velocity rotation = Affine.rotation(self._angle) position = Affine.translation(self._position_in(dt)) return position * rotation