def on_destroy(self): # Create explosion for s in self.body.shapes: bodydef = Body() bodydef.ccd = True debris = self.melee.world.append_body(bodydef) debris.linear_velocity = Vec2(randrange(-100.0, 100.0), randrange(-100.0, 100.0)) junk = Debris(self.melee, debris) if isinstance(s, BoundPolygon): # Polygon debris.position = self.body.position + s.centroid polydef = Polygon() polydef.density = 10 polydef.vertices = s.vertices shape = debris.append_shape(polydef) # Register shapes for collision callbacks self.melee.contact_register[hash(shape)] = junk else: # Circle debris.position = self.body.position + s.local_position circdef = Circle() circdef.density = 10 circle.radis = s.radius shape = debris.append_shape(circdef) self.melee.contact_register[hash(shape)] = junk debris.set_mass_from_shapes()
def __init__(self, width, height, pose): """Creates a Polygon with the orientation and size inputted Keywords: width -> float height -> float pose -> 3-item list """ self.pose = pose self.width = width self.height = height # define the geometry halfwidth_x = width * 0.5 halfwidth_y = height * 0.5 vertexes = [[halfwidth_x, halfwidth_y], [halfwidth_x, -halfwidth_y], [-halfwidth_x, -halfwidth_y], [-halfwidth_x, halfwidth_y]] self.geometry = Polygon(vertexes) self.global_geometry = Polygon(vertexes).get_transformation_to_pose( self.pose)
def __init__(self, melee): Ship.__init__(self, melee) ## ## Manually Create turret ## TODO: Eventually this will be autogenerated via SVG from utils.squirtle import SVG svg_turret = SVG('data/ships/nemesis-turret.svg') svg_turret.init((105.908, 106.821), self.scale) # Create body bodydef = Body() bodydef.ccd = True bodydef.position = self.body.position self.turret = melee.world.append_body(bodydef) self.turret.angular_velocity = self.body.angular_velocity self.turret.linear_velocity = self.body.linear_velocity # Create shapes self.radius = 0.85 density = 2.0 # Base base = Circle() base.collision_group = self.group base.radius = self.radius base.density = density # Barrel verts = [Vec2(0.15, -2), Vec2(0.15, 0), Vec2(-0.15, 0), Vec2( -0.15, -2)] barrel = Polygon() barrel.vertices = verts barrel.collision_group = self.group barrel.density = density s1 = self.turret.append_shape(base) s2 = self.turret.append_shape(barrel) self.turret.set_mass_from_shapes() # Create secondary SecondaryWeapon(self, melee, self.turret, svg_turret)
def fire(self): # This is a specalized function.. if not self.primary_time() or self.battery <= self.pEnergy: return # Drain battery self.battery_cost(self.pEnergy) # Create body and shape bodydef = Body() bodydef.ccd = True bodydef.angle = self.body.angle + (pi * 0.5) + self.turret_angle bodydef.position = self.turret.get_world_point(Vec2(0, -3)) shell = self.melee.world.append_body(bodydef) angle = vforangle(bodydef.angle) velocity = rotate(angle, (0.0, -150.0)) vb = self.body.linear_velocity shell.linear_velocity = Vec2(velocity[0]+vb.x, velocity[1]+vb.y) polydef = Polygon() verts = [Vec2(0.5, 0.8), Vec2(-0.5, 0.8), Vec2(-0.5, -0.8), Vec2(0.5, -0.8)] polydef.vertices = verts polydef.density = 5 polydef.collision_group = self.group shell.append_shape(polydef) shell.set_mass_from_shapes() # Create projectile projectile = PrimaryWeapon(self, self.melee, shell) projectile.group = self.group projectile.lifetime = 2.5 projectile.damage = 10 projectile.health = 5 projectile.shapes = verts
def __init__(self, melee): Actor.__init__(self, melee) # Set max linear and angular velocity self.max_linear_velocity = 50 self.max_angular_velocity = pi # Physics (based on SVG shapes) self.translate = calc_center(self.lines[self.parts.index(self.center_part)]) self.svg.init(self.translate, self.scale) bodydef = Body() bodydef.ccd = True bodydef.position = self.initial_position self.body = melee.world.append_body(bodydef) self.body.linear_velocity = self.initial_velocity self.body.angular_velocity = self.initial_ang_vel for p in self.lines: polygondef = Polygon() polygondef.density = self.density # Ensure points are oriented ccw ccw = convex_hull(p) # Translate and scale points verts = [] for v in ccw: x = (v[0] - self.translate[0]) * self.scale y = (v[1] - self.translate[1]) * self.scale verts.append(Vec2(x, y)) polygondef.vertices = verts polygondef.collision_group = self.group shape = self.body.append_shape(polygondef) # Register shapes for collision callbacks melee.contact_register[hash(shape)] = self self.body.set_mass_from_shapes()
def build_0(self): pig1 = Pig(960, 110, self.space) pig2 = Pig(960, 192, self.space) self.pigs.append(pig1) self.pigs.append(pig2) self.beams.append(Polygon((960, 110), 85, 20, self.space)) self.beams.append(Polygon((960, 20), 85, 20, self.space)) self.columns.append(Polygon((930, -50), 20, 85, self.space)) self.columns.append(Polygon((990, -50), 20, 85, self.space)) self.columns.append(Polygon((930, 70), 20, 85, self.space)) self.columns.append(Polygon((990, 70), 20, 85, self.space)) self.number_of_birds = 4 if self.bool_space: self.number_of_birds = 8
def custom_map(self): """Generate a map based on dictionary of obstacle and goal locations""" # OBSTACLE PARAMS obs_params = [{ 'w': 0.5, 'h': 1.2, 'x': 1.0, 'y': 0.0, 'deg': 0 }, { 'w': 0.3, 'h': 0.6, 'x': 0.5, 'y': 1.0, 'deg': 25 }] # BUILD CUSTOM ELEMENTS # generate the goal x = GOAL_X_DIST y = GOAL_Y_DIST goal = [x, y] # generate a proximity test geometry for the goal r = MIN_GOAL_CLEARANCE n = 6 _goal_test_geometry = [] for i in range(n): _goal_test_geometry.append( [x + r * cos(i * 2 * pi / n), y + r * sin(i * 2 * pi / n)]) goal_test_geometry = Polygon(_goal_test_geometry) # generate the obstacles obstacles = [] obs_index = 0 # collect together robot and goal geometries to check obstacles # are not on top of them test_geometries = [r.global_geometry for r in self.world.robots] + ([goal_test_geometry]) while obs_index < len(obs_params): # convert orientation to radians theta = -pi + ((obs_params[obs_index]['deg'] / 360.0) * 2 * pi) # create the obstacle obstacle = RectangleObstacle( obs_params[obs_index]['w'], obs_params[obs_index]['h'], Pose(obs_params[obs_index]['x'], obs_params[obs_index]['y'], theta)) # test if the obstacle overlaps the robots or the goal intersects = False for test_geometry in test_geometries: intersects |= self.geometrics.convex_polygon_intersect_test( test_geometry, obstacle.global_geometry) if intersects is False: obstacles.append(obstacle) else: print("Obstacle {} collides with robots or goal").format( obs_index) obs_index += 1 # update the current obstacles and goal self.current_obstacles = obstacles self.current_goal = goal # apply the new obstacles and goal to the world self.apply_to_world()
def random_map(self): """Generate a random set of obstacles and goal location""" # OBSTACLE PARAMS obs_min_dim = OBS_MIN_DIM obs_max_dim = OBS_MAX_DIM obs_max_combined_dim = OBS_MAX_COMBINED_DIM obs_min_count = OBS_MIN_COUNT obs_max_count = OBS_MAX_COUNT obs_min_dist = OBS_MIN_DIST obs_max_dist = OBS_MAX_DIST # GOAL PARAMS goal_min_dist = GOAL_MIN_DIST goal_max_dist = GOAL_MAX_DIST # BUILD RANDOM ELEMENTS # generate the goal goal_dist_range = goal_max_dist - goal_min_dist dist = goal_min_dist + (random() * goal_dist_range) phi = -pi + (random() * 2 * pi) x = dist * sin(phi) y = dist * cos(phi) goal = [x, y] # generate a proximity test geometry for the goal r = MIN_GOAL_CLEARANCE n = 6 _goal_test_geometry = [] for i in range(n): _goal_test_geometry.append( [x + r * cos(i * 2 * pi / n), y + r * sin(i * 2 * pi / n)]) goal_test_geometry = Polygon(_goal_test_geometry) # generate the obstacles obstacles = [] obs_dim_range = obs_max_dim - obs_min_dim obs_dist_range = obs_max_dist - obs_min_dist num_obstacles = randrange(obs_min_count, obs_max_count + 1) test_geometries = [r.global_geometry for r in self.world.robots] + ([goal_test_geometry]) while len(obstacles) < num_obstacles: # generate dimensions width = obs_min_dim + (random() * obs_dim_range) height = obs_min_dim + (random() * obs_dim_range) while width + height > obs_max_combined_dim: height = obs_min_dim + (random() * obs_dim_range) # generate position dist = obs_min_dist + (random() * obs_dist_range) phi = -pi + (random() * 2 * pi) x = dist * sin(phi) y = dist * cos(phi) # generate orientation theta = -pi + (random() * 2 * pi) # test if the obstacle overlaps the robots or the goal obstacle = RectangleObstacle(width, height, Pose(x, y, theta)) intersects = False for test_geometry in test_geometries: intersects |= self.geometrics.convex_polygon_intersect_test( test_geometry, obstacle.global_geometry) if intersects is False: obstacles.append(obstacle) # update the current obstacles and goal self.current_obstacles = obstacles self.current_goal = goal # apply the new obstacles and goal to the world self.apply_to_world()
class Robot(object): # Robot """Class representing a robot simultaneously in the GUI and real world. Attributes: id -> int wheel_radius -> float wheel_base_length -> float max_speed -> float trans_vel_limit -> float ang_vel_limit -> float pose -> Pose object geometry -> Polygon object global_geometry -> Polygon object left_wheel_encoder -> WheelEncoder object right_wheel_encoder -> WheelEncoder object wheel_encoders -> list proximity_sensors -> list dynamics -> DifferentialDriveDynamics object supervisor -> Supervisor object use_serial -> boolean physical_robot -> RobotPhysicalInterface object left_wheel_drive_rate -> float right_wheel_drive_rate -> float Methods: __init__(ID, x=0.0, y=0.0, deg=90.0) step_motion(dt) stop_motion() set_wheel_drive_rates(v_l, v_r) """ def __init__(self, ID, viewer, x=0.0, y=0.0, deg=0.0): """Bind robot ID and setup robot geometry, location, supervisor & comms. Keywords: id -> int x -> float y -> float deg -> float robot_params -> list """ # robot ID self.id = ID # bind the viewer of the robot self.viewer = viewer # bind the initial pose of the robot self.x = x self.y = y self.deg = deg # setup the robot self.setup_robot(self.x, self.y, self.deg) # physical robot communication self.use_serial = use_serial if self.use_serial: self.physical_robot = RobotPhysicalInterface(self) def setup_robot(self, x=0.0, y=0.0, deg=0.0): """Apply the robot and sensor config parameters""" # retrieve robot config parameters robot_params = self.viewer.get_robot_parameters() self.wheel_radius = float(robot_params[0][0][1]) # metres self.wheel_base_length = float(robot_params[0][1][1]) # metres self.ticks_per_rev = float(robot_params[0][2][1]) # unitless self.max_speed = float(robot_params[0][3][1]) # rpm self.body_width = float(robot_params[0][4][1]) # metres self.body_length = float(robot_params[0][5][1]) # metres self.payload_width = float(robot_params[0][6][1]) # metres self.payload_length = float(robot_params[0][7][1]) # metres self.payload_offset = float(robot_params[0][8][1]) # metres self.sensor_min_value = float(robot_params[1][0][1]) # millivolts self.sensor_max_value = float(robot_params[1][1][1]) # millivolts self.sensor_min_range = float(robot_params[1][2][1]) # metres self.sensor_max_range = float(robot_params[1][3][1]) # metres self.sensor_phi_range = float(robot_params[1][4][1]) # metres # x, y, theta_degrees self.sensor_poses = ([ [float(robot_params[2][0][1]), float(robot_params[2][1][1]), float(robot_params[2][2][1])], [float(robot_params[3][0][1]), float(robot_params[3][1][1]), float(robot_params[3][2][1])], [float(robot_params[4][0][1]), float(robot_params[4][1][1]), float(robot_params[4][2][1])] ]) self.robot_body = ([[self.body_length / 2, self.body_width / 2], [-self.body_length / 2, self.body_width / 2], [-self.body_length / 2, -self.body_width / 2], [self.body_length / 2, -self.body_width / 2]]) self.robot_payload = ([[self.payload_length / 2 + self.payload_offset, self.payload_width / 2], [-self.payload_length / 2 + self.payload_offset, self.payload_width / 2], [-self.payload_length / 2 + self.payload_offset, -self.payload_width / 2], [self.payload_length / 2 + self.payload_offset, -self.payload_width / 2]]) # drive rates self.max_speed *= ((2 * pi) / 60) # rpm 2 rad/s self.trans_vel_limit = self.max_speed * self.wheel_radius # m/s self.ang_vel_limit = 0.2 * self.max_speed # rad/s if debug: print("SPEED {:.3f}m/s").format(self.trans_vel_limit) # pose theta = -pi + ((deg / 360.0) * 2 * pi) self.pose = Pose(x, y, theta) # geometry self.geometry = Polygon(self.robot_body) self.global_geometry = ( self.geometry.get_transformation_to_pose(self.pose)) # wheel encoders self.left_wheel_encoder = WheelEncoder(self.ticks_per_rev) self.right_wheel_encoder = WheelEncoder(self.ticks_per_rev) self.wheel_encoders = ( [self.left_wheel_encoder, self.right_wheel_encoder]) # proximity sensors self.proximity_sensors = [] for _pose in self.sensor_poses: sensor_pose = Pose(_pose[0], _pose[1], radians(_pose[2])) self.proximity_sensors.append( ProximitySensor(self, sensor_pose, self.sensor_min_range, self.sensor_max_range, radians(self.sensor_phi_range))) # dynamics self.dynamics = DifferentialDriveDynamics(self, self.wheel_radius, self.wheel_base_length) # supervisor self.supervisor = Supervisor(RobotSupervisorInterface(self), self.wheel_radius, self.wheel_base_length, self.ticks_per_rev, self.sensor_poses, self.sensor_max_range, Pose(self.pose.x, self.pose.y, self.pose.theta)) # set wheel drive rates (rad/s) self.left_wheel_drive_rate = 0.0 self.right_wheel_drive_rate = 0.0 def step_motion(self, dt): """Simulate the robot's motion over the given time interval.""" # apply the supervisor velocities & encoder values -> determine new pose self.dynamics.apply_dynamics(self.left_wheel_drive_rate, self.right_wheel_drive_rate, dt, self.pose, self.wheel_encoders) # update global geometry (blue shell) self.global_geometry = ( self.geometry.get_transformation_to_pose(self.pose)) # update the position all of proximity the sensors for proximity_sensor in self.proximity_sensors: proximity_sensor.update_position() # send wheel speeds to physical robot and retrieve sensor values if self.physical_robot.robot_comm.connected: self.physical_robot.step() def stop_motion(self): """Stop the physical robot.""" if self.physical_robot.robot_comm.connected: self.physical_robot.stop() def set_wheel_drive_rates(self, v_l, v_r): """Set the drive rates (angular vel rad/s) for this robot's wheels.""" # simulate physical limit on drive motors self.left_wheel_drive_rate = min(self.max_speed, v_l) self.right_wheel_drive_rate = min(self.max_speed, v_r) print("left_wheel_drive_rate = {}, right_wheel_drive_rate = {}").format( self.left_wheel_drive_rate, self.right_wheel_drive_rate) def read_encoders(self): self.physical_robot.read_wheel_encoders()
def setup_robot(self, x=0.0, y=0.0, deg=0.0): """Apply the robot and sensor config parameters""" # retrieve robot config parameters robot_params = self.viewer.get_robot_parameters() self.wheel_radius = float(robot_params[0][0][1]) # metres self.wheel_base_length = float(robot_params[0][1][1]) # metres self.ticks_per_rev = float(robot_params[0][2][1]) # unitless self.max_speed = float(robot_params[0][3][1]) # rpm self.body_width = float(robot_params[0][4][1]) # metres self.body_length = float(robot_params[0][5][1]) # metres self.payload_width = float(robot_params[0][6][1]) # metres self.payload_length = float(robot_params[0][7][1]) # metres self.payload_offset = float(robot_params[0][8][1]) # metres self.sensor_min_value = float(robot_params[1][0][1]) # millivolts self.sensor_max_value = float(robot_params[1][1][1]) # millivolts self.sensor_min_range = float(robot_params[1][2][1]) # metres self.sensor_max_range = float(robot_params[1][3][1]) # metres self.sensor_phi_range = float(robot_params[1][4][1]) # metres # x, y, theta_degrees self.sensor_poses = ([ [float(robot_params[2][0][1]), float(robot_params[2][1][1]), float(robot_params[2][2][1])], [float(robot_params[3][0][1]), float(robot_params[3][1][1]), float(robot_params[3][2][1])], [float(robot_params[4][0][1]), float(robot_params[4][1][1]), float(robot_params[4][2][1])] ]) self.robot_body = ([[self.body_length / 2, self.body_width / 2], [-self.body_length / 2, self.body_width / 2], [-self.body_length / 2, -self.body_width / 2], [self.body_length / 2, -self.body_width / 2]]) self.robot_payload = ([[self.payload_length / 2 + self.payload_offset, self.payload_width / 2], [-self.payload_length / 2 + self.payload_offset, self.payload_width / 2], [-self.payload_length / 2 + self.payload_offset, -self.payload_width / 2], [self.payload_length / 2 + self.payload_offset, -self.payload_width / 2]]) # drive rates self.max_speed *= ((2 * pi) / 60) # rpm 2 rad/s self.trans_vel_limit = self.max_speed * self.wheel_radius # m/s self.ang_vel_limit = 0.2 * self.max_speed # rad/s if debug: print("SPEED {:.3f}m/s").format(self.trans_vel_limit) # pose theta = -pi + ((deg / 360.0) * 2 * pi) self.pose = Pose(x, y, theta) # geometry self.geometry = Polygon(self.robot_body) self.global_geometry = ( self.geometry.get_transformation_to_pose(self.pose)) # wheel encoders self.left_wheel_encoder = WheelEncoder(self.ticks_per_rev) self.right_wheel_encoder = WheelEncoder(self.ticks_per_rev) self.wheel_encoders = ( [self.left_wheel_encoder, self.right_wheel_encoder]) # proximity sensors self.proximity_sensors = [] for _pose in self.sensor_poses: sensor_pose = Pose(_pose[0], _pose[1], radians(_pose[2])) self.proximity_sensors.append( ProximitySensor(self, sensor_pose, self.sensor_min_range, self.sensor_max_range, radians(self.sensor_phi_range))) # dynamics self.dynamics = DifferentialDriveDynamics(self, self.wheel_radius, self.wheel_base_length) # supervisor self.supervisor = Supervisor(RobotSupervisorInterface(self), self.wheel_radius, self.wheel_base_length, self.ticks_per_rev, self.sensor_poses, self.sensor_max_range, Pose(self.pose.x, self.pose.y, self.pose.theta)) # set wheel drive rates (rad/s) self.left_wheel_drive_rate = 0.0 self.right_wheel_drive_rate = 0.0
class Robot(object): # Robot """Class representing a robot simultaneously in the GUI and real world. Attributes: id -> int wheel_radius -> float wheel_base_length -> float max_speed -> float trans_vel_limit -> float ang_vel_limit -> float pose -> Pose object geometry -> Polygon object global_geometry -> Polygon object left_wheel_encoder -> WheelEncoder object right_wheel_encoder -> WheelEncoder object wheel_encoders -> list proximity_sensors -> list dynamics -> DifferentialDriveDynamics object supervisor -> Supervisor object use_serial -> boolean physical_robot -> RobotPhysicalInterface object left_wheel_drive_rate -> float right_wheel_drive_rate -> float Methods: __init__(ID, x=0.0, y=0.0, deg=90.0) step_motion(dt) stop_motion() set_wheel_drive_rates(v_l, v_r) """ def __init__(self, ID, viewer, x=0.0, y=0.0, deg=0.0): """Bind robot ID and setup robot geometry, location, supervisor & comms. Keywords: id -> int x -> float y -> float deg -> float robot_params -> list """ # robot ID self.id = ID # bind the viewer of the robot self.viewer = viewer # bind the initial pose of the robot self.x = x self.y = y self.deg = deg # setup the robot self.setup_robot(self.x, self.y, self.deg) # physical robot communication self.use_serial = use_serial if self.use_serial: self.physical_robot = RobotPhysicalInterface(self) def setup_robot(self, x=0.0, y=0.0, deg=0.0): """Apply the robot and sensor config parameters""" # retrieve robot config parameters robot_params = self.viewer.get_robot_parameters() self.wheel_radius = float(robot_params[0][0][1]) # metres self.wheel_base_length = float(robot_params[0][1][1]) # metres self.ticks_per_rev = float(robot_params[0][2][1]) # unitless self.max_speed = float(robot_params[0][3][1]) # rpm self.body_width = float(robot_params[0][4][1]) # metres self.body_length = float(robot_params[0][5][1]) # metres self.payload_width = float(robot_params[0][6][1]) # metres self.payload_length = float(robot_params[0][7][1]) # metres self.payload_offset = float(robot_params[0][8][1]) # metres self.sensor_min_value = float(robot_params[1][0][1]) # millivolts self.sensor_max_value = float(robot_params[1][1][1]) # millivolts self.sensor_min_range = float(robot_params[1][2][1]) # metres self.sensor_max_range = float(robot_params[1][3][1]) # metres self.sensor_phi_range = float(robot_params[1][4][1]) # metres # x, y, theta_degrees self.sensor_poses = ([[ float(robot_params[2][0][1]), float(robot_params[2][1][1]), float(robot_params[2][2][1]) ], [ float(robot_params[3][0][1]), float(robot_params[3][1][1]), float(robot_params[3][2][1]) ], [ float(robot_params[4][0][1]), float(robot_params[4][1][1]), float(robot_params[4][2][1]) ]]) self.robot_body = ([[self.body_length / 2, self.body_width / 2], [-self.body_length / 2, self.body_width / 2], [-self.body_length / 2, -self.body_width / 2], [self.body_length / 2, -self.body_width / 2]]) self.robot_payload = ([ [ self.payload_length / 2 + self.payload_offset, self.payload_width / 2 ], [ -self.payload_length / 2 + self.payload_offset, self.payload_width / 2 ], [ -self.payload_length / 2 + self.payload_offset, -self.payload_width / 2 ], [ self.payload_length / 2 + self.payload_offset, -self.payload_width / 2 ] ]) # drive rates self.max_speed *= ((2 * pi) / 60) # rpm 2 rad/s self.trans_vel_limit = self.max_speed * self.wheel_radius # m/s self.ang_vel_limit = 0.2 * self.max_speed # rad/s if debug: print("SPEED {:.3f}m/s").format(self.trans_vel_limit) # pose theta = -pi + ((deg / 360.0) * 2 * pi) self.pose = Pose(x, y, theta) # geometry self.geometry = Polygon(self.robot_body) self.global_geometry = (self.geometry.get_transformation_to_pose( self.pose)) # wheel encoders self.left_wheel_encoder = WheelEncoder(self.ticks_per_rev) self.right_wheel_encoder = WheelEncoder(self.ticks_per_rev) self.wheel_encoders = ([ self.left_wheel_encoder, self.right_wheel_encoder ]) # proximity sensors self.proximity_sensors = [] for _pose in self.sensor_poses: sensor_pose = Pose(_pose[0], _pose[1], radians(_pose[2])) self.proximity_sensors.append( ProximitySensor(self, sensor_pose, self.sensor_min_range, self.sensor_max_range, radians(self.sensor_phi_range))) # dynamics self.dynamics = DifferentialDriveDynamics(self, self.wheel_radius, self.wheel_base_length) # supervisor self.supervisor = Supervisor( RobotSupervisorInterface(self), self.wheel_radius, self.wheel_base_length, self.ticks_per_rev, self.sensor_poses, self.sensor_max_range, Pose(self.pose.x, self.pose.y, self.pose.theta)) # set wheel drive rates (rad/s) self.left_wheel_drive_rate = 0.0 self.right_wheel_drive_rate = 0.0 def step_motion(self, dt): """Simulate the robot's motion over the given time interval.""" # apply the supervisor velocities & encoder values -> determine new pose self.dynamics.apply_dynamics(self.left_wheel_drive_rate, self.right_wheel_drive_rate, dt, self.pose, self.wheel_encoders) # update global geometry (blue shell) self.global_geometry = (self.geometry.get_transformation_to_pose( self.pose)) # update the position all of proximity the sensors for proximity_sensor in self.proximity_sensors: proximity_sensor.update_position() # send wheel speeds to physical robot and retrieve sensor values if self.physical_robot.robot_comm.connected: self.physical_robot.step() def stop_motion(self): """Stop the physical robot.""" if self.physical_robot.robot_comm.connected: self.physical_robot.stop() def set_wheel_drive_rates(self, v_l, v_r): """Set the drive rates (angular vel rad/s) for this robot's wheels.""" # simulate physical limit on drive motors self.left_wheel_drive_rate = min(self.max_speed, v_l) self.right_wheel_drive_rate = min(self.max_speed, v_r) print( "left_wheel_drive_rate = {}, right_wheel_drive_rate = {}").format( self.left_wheel_drive_rate, self.right_wheel_drive_rate) def read_encoders(self): self.physical_robot.read_wheel_encoders()
def setup_robot(self, x=0.0, y=0.0, deg=0.0): """Apply the robot and sensor config parameters""" # retrieve robot config parameters robot_params = self.viewer.get_robot_parameters() self.wheel_radius = float(robot_params[0][0][1]) # metres self.wheel_base_length = float(robot_params[0][1][1]) # metres self.ticks_per_rev = float(robot_params[0][2][1]) # unitless self.max_speed = float(robot_params[0][3][1]) # rpm self.body_width = float(robot_params[0][4][1]) # metres self.body_length = float(robot_params[0][5][1]) # metres self.payload_width = float(robot_params[0][6][1]) # metres self.payload_length = float(robot_params[0][7][1]) # metres self.payload_offset = float(robot_params[0][8][1]) # metres self.sensor_min_value = float(robot_params[1][0][1]) # millivolts self.sensor_max_value = float(robot_params[1][1][1]) # millivolts self.sensor_min_range = float(robot_params[1][2][1]) # metres self.sensor_max_range = float(robot_params[1][3][1]) # metres self.sensor_phi_range = float(robot_params[1][4][1]) # metres # x, y, theta_degrees self.sensor_poses = ([[ float(robot_params[2][0][1]), float(robot_params[2][1][1]), float(robot_params[2][2][1]) ], [ float(robot_params[3][0][1]), float(robot_params[3][1][1]), float(robot_params[3][2][1]) ], [ float(robot_params[4][0][1]), float(robot_params[4][1][1]), float(robot_params[4][2][1]) ]]) self.robot_body = ([[self.body_length / 2, self.body_width / 2], [-self.body_length / 2, self.body_width / 2], [-self.body_length / 2, -self.body_width / 2], [self.body_length / 2, -self.body_width / 2]]) self.robot_payload = ([ [ self.payload_length / 2 + self.payload_offset, self.payload_width / 2 ], [ -self.payload_length / 2 + self.payload_offset, self.payload_width / 2 ], [ -self.payload_length / 2 + self.payload_offset, -self.payload_width / 2 ], [ self.payload_length / 2 + self.payload_offset, -self.payload_width / 2 ] ]) # drive rates self.max_speed *= ((2 * pi) / 60) # rpm 2 rad/s self.trans_vel_limit = self.max_speed * self.wheel_radius # m/s self.ang_vel_limit = 0.2 * self.max_speed # rad/s if debug: print("SPEED {:.3f}m/s").format(self.trans_vel_limit) # pose theta = -pi + ((deg / 360.0) * 2 * pi) self.pose = Pose(x, y, theta) # geometry self.geometry = Polygon(self.robot_body) self.global_geometry = (self.geometry.get_transformation_to_pose( self.pose)) # wheel encoders self.left_wheel_encoder = WheelEncoder(self.ticks_per_rev) self.right_wheel_encoder = WheelEncoder(self.ticks_per_rev) self.wheel_encoders = ([ self.left_wheel_encoder, self.right_wheel_encoder ]) # proximity sensors self.proximity_sensors = [] for _pose in self.sensor_poses: sensor_pose = Pose(_pose[0], _pose[1], radians(_pose[2])) self.proximity_sensors.append( ProximitySensor(self, sensor_pose, self.sensor_min_range, self.sensor_max_range, radians(self.sensor_phi_range))) # dynamics self.dynamics = DifferentialDriveDynamics(self, self.wheel_radius, self.wheel_base_length) # supervisor self.supervisor = Supervisor( RobotSupervisorInterface(self), self.wheel_radius, self.wheel_base_length, self.ticks_per_rev, self.sensor_poses, self.sensor_max_range, Pose(self.pose.x, self.pose.y, self.pose.theta)) # set wheel drive rates (rad/s) self.left_wheel_drive_rate = 0.0 self.right_wheel_drive_rate = 0.0