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