def start_test(self,challenge): vals = self.parseChallenge(challenge) if 'v' not in vals or 'theta' not in vals: raise CourseraException("Unknown challenge format. Please contact developers for assistance.") v = vals['v'] theta = vals['theta'] from supervisors.week2 import QuickBotSupervisor from robots.quickbot import QuickBot from pose import Pose from helpers import Struct from math import pi bot = QuickBot(Pose()) info = bot.get_info() info.color = 0 s = QuickBotSupervisor(Pose(),info) params = Struct() params.goal = theta*180/pi params.velocity = v params.pgain = 1 s.set_parameters(params) tc = 0.033 # 0.033 sec' is the SimIAm time step for step in range(25): # 25 steps bot.move(tc) bot.set_inputs(s.execute(bot.get_info(), tc)) xe,ye,te = s.pose_est xr,yr,tr = bot.get_pose() self.testsuite.respond("{:0.3f},{:0.3f},{:0.3f}".format(abs((xr-xe)/xr), abs((yr-ye)/yr), abs(abs(tr-te)%(2*pi)/tr)))
def init_default_parameters(self): """Sets the default PID parameters, goal, and velocity""" p = Struct() p.goal = 45.0 p.velocity = 0.2 p.pgain = 3.0 self.parameters = p
def get_parameters(self): """Return a structure with current parameters""" p = Struct() p.wall = Struct() p.wall.direction = self.parameters.direction p.wall.distance = self.parameters.distance p.velocity = self.parameters.velocity p.gains = self.parameters.gains return p
def get_struct(self): p = Struct() for key, leaf in self.leafs.items(): if isinstance(key, tuple): if key[0] not in p.__dict__: p.__dict__[key[0]] = {} p.__dict__[key[0]][key[1]] = leaf.get_struct() else: p.__dict__[key] = leaf.get_struct() return p
def get_default_parameters(self): """Sets the default PID parameters, goal, and velocity""" p = Struct() p.goal = Struct() p.goal.x = 0.0 p.goal.y = 0.5 p.velocity = Struct() p.velocity.v = 0.2 p.gains = Struct() p.gains.kp = 10.0 p.gains.ki = 2.0 p.gains.kd = 0.0 return p
def init_default_parameters(self): """Sets the default PID parameters, goal, and velocity""" p = Struct() p.goal = Struct() p.goal.x = 1.0 p.goal.y = 1.0 p.velocity = Struct() p.velocity.v = 0.2 p.gains = Struct() p.gains.kp = 10.0 p.gains.ki = 2.0 p.gains.kd = 0.0 self.parameters = p
def start_test(self, challenge): m = self.RX.match(challenge) if m is None: raise CourseraException( "Unknown challenge format. Please contact developers for assistance." ) try: v = float(m.group('v')) theta = float(m.group('theta')) except ValueError: raise CourseraException( "Unknown challenge format. Please contact developers for assistance." ) from supervisors.week2 import QuickBotSupervisor from robots.quickbot import QuickBot from pose import Pose from helpers import Struct from math import pi bot = QuickBot(Pose()) info = bot.get_info() info.color = 0 s = QuickBotSupervisor(Pose(), info) params = Struct() params.goal = theta * 180 / pi params.velocity = v params.pgain = 1 s.set_parameters(params) tc = 0.033 # 0.033 sec' is the SimIAm time step for step in range(25): # 25 steps bot.move(tc) bot.set_inputs(s.execute(bot.get_info(), tc)) xe, ye, te = s.pose_est xr, yr, tr = bot.get_pose() if xr == 0: xr = 0.0000001 if yr == 0: yr = 0.0000001 if tr == 0: tr = 0.0000001 self.testsuite.respond("{:0.3f},{:0.3f},{:0.3f}".format( abs((xr - xe) / xr), abs((yr - ye) / yr), abs(abs(tr - te) % (2 * pi) / tr)))
def init_default_parameters(self): """Sets the default PID parameters, goal, and velocity""" p = Struct() p.goal = Struct() p.goal.x = 1.0 p.goal.y = 1.0 p.velocity = Struct() p.velocity.v = 0.2 p.gains = Struct() p.gains.kp = 4.0 p.gains.ki = 0.1 p.gains.kd = 0.0 p.ga_path = [] p.point_cnt = 0 self.parameters = p
def start_test(self,challenge): m = self.RX.match(challenge) if m is None: raise CourseraException("Unknown challenge format. Please contact developers for assistance.") try: v = float(m.group('v')) theta = float(m.group('theta')) except ValueError: raise CourseraException("Unknown challenge format. Please contact developers for assistance.") from supervisors.week2 import QuickBotSupervisor from robots.quickbot import QuickBot from pose import Pose from helpers import Struct from math import pi bot = QuickBot(Pose()) info = bot.get_info() info.color = 0 s = QuickBotSupervisor(Pose(),info) params = Struct() params.goal = theta*180/pi params.velocity = v params.pgain = 1 s.set_parameters(params) tc = 0.033 # 0.033 sec' is the SimIAm time step for step in range(25): # 25 steps bot.move(tc) bot.set_inputs(s.execute(bot.get_info(), tc)) xe,ye,te = s.pose_est xr,yr,tr = bot.get_pose() if xr == 0: xr = 0.0000001 if yr == 0: yr = 0.0000001 if tr == 0: tr = 0.0000001 self.testsuite.respond("{:0.3f},{:0.3f},{:0.3f}".format(abs((xr-xe)/xr), abs((yr-ye)/yr), abs(abs(tr-te)%(2*pi)/tr)))
def init_default_parameters(self): """Sets the default PID parameters, goal, and velocity""" self.parameters = Struct({ "joystick": { "i_j": 0, "i_x": 0, "i_y": 1, "inv_x": True, "inv_y": True } })
def init_default_parameters(self): """Sets the default PID parameters, goal, and velocity""" p = Struct() p.goal = Struct() p.goal.x = 0.50 p.goal.y = 0.0 p.velocity = Struct() p.velocity.v = 0.2 p.gains = Struct() p.gains.kp = 3.0 p.gains.ki = 2.0 p.gains.kd = 0.0 self.parameters = p
def start_test(self, challenge): vals = self.parseChallenge(challenge) if 'v' not in vals or 'theta' not in vals: raise CourseraException( "Unknown challenge format. Please contact developers for assistance." ) v = vals['v'] theta = vals['theta'] from supervisors.week2 import QuickBotSupervisor from robots.quickbot import QuickBot from pose import Pose from helpers import Struct from math import pi bot = QuickBot(Pose()) info = bot.get_info() info.color = 0 s = QuickBotSupervisor(Pose(), info) params = Struct() params.goal = theta * 180 / pi params.velocity = v params.pgain = 1 s.set_parameters(params) tc = 0.033 # 0.033 sec' is the SimIAm time step for step in range(25): # 25 steps bot.move(tc) bot.set_inputs(s.execute(bot.get_info(), tc)) xe, ye, te = s.pose_est xr, yr, tr = bot.get_pose() self.testsuite.respond("{:0.3f},{:0.3f},{:0.3f}".format( abs((xr - xe) / xr), abs((yr - ye) / yr), abs(abs(tr - te) % (2 * pi) / tr)))
def init_default_parameters(self): """Sets the default PID parameters, goal, and velocity""" self.parameters = Struct({"velocity":{"v":0.2}, \ "gains":{"kp":4.0, "ki": 0.1, "kd": 0.0}})
def __init__(self, pose, color = 0xFFFFFF): Robot.__init__(self, pose, color) # create shape self._shapes = Struct() self._shapes.base_plate = np.array([[ 0.0335, 0.0534, 1], [ 0.0429, 0.0534, 1], [ 0.0639, 0.0334, 1], [ 0.0686, 0.0000, 1], [ 0.0639,-0.0334, 1], [ 0.0429,-0.0534, 1], [ 0.0335,-0.0534, 1], [-0.0465,-0.0534, 1], [-0.0815,-0.0534, 1], [-0.1112,-0.0387, 1], [-0.1112, 0.0387, 1], [-0.0815, 0.0534, 1], [-0.0465, 0.0534, 1]]) self._shapes.bbb = np.array([[-0.0914,-0.0406, 1], [-0.0944,-0.0376, 1], [-0.0944, 0.0376, 1], [-0.0914, 0.0406, 1], [-0.0429, 0.0406, 1], [-0.0399, 0.0376, 1], [-0.0399,-0.0376, 1], [-0.0429,-0.0406, 1]]) self._shapes.bbb_rail_l = np.array([[-0.0429, -0.0356,1], [-0.0429, 0.0233,1], [-0.0479, 0.0233,1], [-0.0479,-0.0356,1]]) self._shapes.bbb_rail_r = np.array([[-0.0914,-0.0356,1], [-0.0914, 0.0233,1], [-0.0864, 0.0233,1], [-0.0864,-0.0356,1]]) self._shapes.bbb_eth = np.array([[-0.0579, 0.0436, 1], [-0.0579, 0.0226, 1], [-0.0739, 0.0226, 1], [-0.0739, 0.0436, 1]]) self._shapes.left_wheel = np.array([[ 0.0254, 0.0595, 1], [ 0.0254, 0.0335, 1], [-0.0384, 0.0335, 1], [-0.0384, 0.0595, 1]]) self._shapes.left_wheel_ol = np.array([[ 0.0254, 0.0595, 1], [ 0.0254, 0.0335, 1], [-0.0384, 0.0335, 1], [-0.0384, 0.0595, 1]]) self._shapes.right_wheel_ol = np.array([[ 0.0254,-0.0595, 1], [ 0.0254,-0.0335, 1], [-0.0384,-0.0335, 1], [-0.0384,-0.0595, 1]]) self._shapes.right_wheel = np.array([[ 0.0254,-0.0595, 1], [ 0.0254,-0.0335, 1], [-0.0384,-0.0335, 1], [-0.0384,-0.0595, 1]]) self._shapes.ir_1 = np.array([[-0.0732, 0.0534, 1], [-0.0732, 0.0634, 1], [-0.0432, 0.0634, 1], [-0.0432, 0.0534, 1]]) self._shapes.ir_2 = np.array([[ 0.0643, 0.0214, 1], [ 0.0714, 0.0285, 1], [ 0.0502, 0.0497, 1], [ 0.0431, 0.0426, 1]]) self._shapes.ir_3 = np.array([[ 0.0636,-0.0042, 1], [ 0.0636, 0.0258, 1], [ 0.0736, 0.0258, 1], [ 0.0736,-0.0042, 1]]) self._shapes.ir_4 = np.array([[ 0.0643,-0.0214, 1], [ 0.0714,-0.0285, 1], [ 0.0502,-0.0497, 1], [ 0.0431,-0.0426, 1]]) self._shapes.ir_5 = np.array([[-0.0732,-0.0534, 1], [-0.0732,-0.0634, 1], [-0.0432,-0.0634, 1], [-0.0432,-0.0534, 1]]) self._shapes.bbb_usb = np.array([[-0.0824,-0.0418, 1], [-0.0694,-0.0418, 1], [-0.0694,-0.0278, 1], [-0.0824,-0.0278, 1]]) # create IR sensors self.ir_sensors = [] ir_sensor_poses = [ Pose(-0.0474, 0.0534, np.radians(90)), Pose( 0.0613, 0.0244, np.radians(45)), Pose( 0.0636, 0.0, np.radians(0)), Pose( 0.0461,-0.0396, np.radians(-45)), Pose(-0.0690,-0.0534, np.radians(-90)) ] for pose in ir_sensor_poses: self.ir_sensors.append(QuickBot_IRSensor(pose,self)) # initialize motion self.ang_velocity = (0.0,0.0) self.info = Struct() self.info.wheels = Struct() # these were the original parameters self.info.wheels.radius = 0.0325 self.info.wheels.base_length = 0.09925 self.info.wheels.ticks_per_rev = 16.0 self.info.wheels.left_ticks = 0 self.info.wheels.right_ticks = 0 self.info.wheels.max_velocity = 8.377 self.left_revolutions = 0.0 self.right_revolutions = 0.0 self.info.ir_sensors = Struct() self.info.ir_sensors.poses = ir_sensor_poses self.info.ir_sensors.readings = None self.info.ir_sensors.rmax = 0.2 self.info.ir_sensors.rmin = 0.02
def get_parameters(self): params = Struct() params.pid = Supervisor.get_parameters(self) return params
def _parse_simulation(self): """ Parse a simulation configuration file Scope: Private Parameters: None Return: A list of the objects in the simulation. """ simulator_objects = [] # robots for robot in self._root.findall('robot'): robot_type = robot.get('type') supervisor = robot.find('supervisor') if supervisor == None: raise Exception( '[XMLReader._parse_simulation] No supervisor specified!') pose = robot.find('pose') if pose == None: raise Exception( '[XMLReader._parse_simulation] No pose specified!') try: x, y, theta = pose.get('x'), pose.get('y'), pose.get('theta') if x == None or y == None or theta == None: raise Exception( '[XMLReader._parse_simulation] Invalid pose!') robot_color = self._parse_color(robot.get('color')) simulator_objects.append( Struct({ 'type': 'robot', 'robot': { 'type': robot_type, 'pose': Pose(float(x), float(y), float(theta)), 'color': robot_color, 'options': robot.get('options', None) }, 'supervisor': { 'type': supervisor.attrib['type'], 'options': supervisor.get('options', None) } })) except ValueError: raise Exception( '[XMLReader._parse_simulation] Invalid robot (bad value)!') # obstacles for obstacle in self._root.findall('obstacle'): pose = obstacle.find('pose') if pose == None: raise Exception( '[XMLReader._parse_simulation] No pose specified!') geometry = obstacle.find('geometry') if geometry == None: raise Exception( '[XMLReader._parse_simulation] No geometry specified!') try: points = [] for point in geometry.findall('point'): x, y = point.get('x'), point.get('y') if x == None or y == None: raise Exception( '[XMLReader._parse_simulation] Invalid point!') points.append((float(x), float(y))) if len(points) < 3: raise Exception( '[XMLReader._parse_simulation] Too few points!') x, y, theta = pose.get('x'), pose.get('y'), pose.get('theta') if x == None or y == None or theta == None: raise Exception( '[XMLReader._parse_simulation] Invalid pose!') color = self._parse_color(obstacle.get('color')) simulator_objects.append( Struct({ 'type': 'obstacle', 'polygon': { 'pose': Pose(float(x), float(y), float(theta)), 'color': color, 'points': points } })) except ValueError: raise Exception( '[XMLReader._parse_simulation] Invalid obstacle (bad value)!' ) # background for marker in self._root.findall('marker'): pose = marker.find('pose') if pose == None: raise Exception( '[XMLReader._parse_simulation] No pose specified!') geometry = marker.find('geometry') if geometry == None: raise Exception( '[XMLReader._parse_simulation] No geometry specified!') try: points = [] for point in geometry.findall('point'): x, y = point.get('x'), point.get('y') if x == None or y == None: raise Exception( '[XMLReader._parse_simulation] Invalid point!') points.append((float(x), float(y))) if len(points) < 3: raise Exception( '[XMLReader._parse_simulation] Too few points!') x, y, theta = pose.get('x'), pose.get('y'), pose.get('theta') if x == None or y == None or theta == None: raise Exception( '[XMLReader._parse_simulation] Invalid pose!') color = self._parse_color(marker.get('color')) simulator_objects.append( Struct({ 'type': 'marker', 'polygon': { 'pose': Pose(float(x), float(y), float(theta)), 'color': color, 'points': points } })) except ValueError: raise Exception( '[XMLReader._parse_simulation] Invalid marker (bad value)!' ) return simulator_objects
def __init__(self, pose, color=0xFFFFFF, options=None): # create shape self._shapes = Struct() self._shapes.base_plate = np.array([[0.0335, 0.0534, 1], [0.0429, 0.0534, 1], [0.0639, 0.0334, 1], [0.0686, 0.0000, 1], [0.0639, -0.0334, 1], [0.0429, -0.0534, 1], [0.0335, -0.0534, 1], [-0.0465, -0.0534, 1], [-0.0815, -0.0534, 1], [-0.1112, -0.0387, 1], [-0.1112, 0.0387, 1], [-0.0815, 0.0534, 1], [-0.0465, 0.0534, 1]]) self._shapes.bbb = np.array([[-0.0914, -0.0406, 1], [-0.0944, -0.0376, 1], [-0.0944, 0.0376, 1], [-0.0914, 0.0406, 1], [-0.0429, 0.0406, 1], [-0.0399, 0.0376, 1], [-0.0399, -0.0376, 1], [-0.0429, -0.0406, 1]]) self._shapes.bbb_rail_l = np.array([[-0.0429, -0.0356, 1], [-0.0429, 0.0233, 1], [-0.0479, 0.0233, 1], [-0.0479, -0.0356, 1]]) self._shapes.bbb_rail_r = np.array([[-0.0914, -0.0356, 1], [-0.0914, 0.0233, 1], [-0.0864, 0.0233, 1], [-0.0864, -0.0356, 1]]) self._shapes.bbb_eth = np.array([[-0.0579, 0.0436, 1], [-0.0579, 0.0226, 1], [-0.0739, 0.0226, 1], [-0.0739, 0.0436, 1]]) self._shapes.left_wheel = np.array([[0.0254, 0.0595, 1], [0.0254, 0.0335, 1], [-0.0384, 0.0335, 1], [-0.0384, 0.0595, 1]]) self._shapes.left_wheel_ol = np.array([[0.0254, 0.0595, 1], [0.0254, 0.0335, 1], [-0.0384, 0.0335, 1], [-0.0384, 0.0595, 1]]) self._shapes.right_wheel_ol = np.array([[0.0254, -0.0595, 1], [0.0254, -0.0335, 1], [-0.0384, -0.0335, 1], [-0.0384, -0.0595, 1]]) self._shapes.right_wheel = np.array([[0.0254, -0.0595, 1], [0.0254, -0.0335, 1], [-0.0384, -0.0335, 1], [-0.0384, -0.0595, 1]]) self._shapes.ir_1 = np.array([[-0.0732, 0.0534, 1], [-0.0732, 0.0634, 1], [-0.0432, 0.0634, 1], [-0.0432, 0.0534, 1]]) self._shapes.ir_2 = np.array([[0.0643, 0.0214, 1], [0.0714, 0.0285, 1], [0.0502, 0.0497, 1], [0.0431, 0.0426, 1]]) self._shapes.ir_3 = np.array([[0.0636, -0.0042, 1], [0.0636, 0.0258, 1], [0.0736, 0.0258, 1], [0.0736, -0.0042, 1]]) self._shapes.ir_4 = np.array([[0.0643, -0.0214, 1], [0.0714, -0.0285, 1], [0.0502, -0.0497, 1], [0.0431, -0.0426, 1]]) self._shapes.ir_5 = np.array([[-0.0732, -0.0534, 1], [-0.0732, -0.0634, 1], [-0.0432, -0.0634, 1], [-0.0432, -0.0534, 1]]) self._shapes.bbb_usb = np.array([[-0.0824, -0.0418, 1], [-0.0694, -0.0418, 1], [-0.0694, -0.0278, 1], [-0.0824, -0.0278, 1]]) ir_sensor_poses = [ Pose(-0.0474, 0.0534, np.radians(90)), Pose(0.0613, 0.0244, np.radians(45)), Pose(0.0636, 0.0, np.radians(0)), Pose(0.0461, -0.0396, np.radians(-45)), Pose(-0.0690, -0.0534, np.radians(-90)) ] self.info = Struct() self.info.wheels = Struct() self.info.wheels.radius = 0.0325 self.info.wheels.base_length = 0.09925 self.info.wheels.ticks_per_rev = 16 self.info.wheels.left_ticks = 0 self.info.wheels.right_ticks = 0 self.info.wheels.max_velocity = 2 * pi * 130 / 60 # 130 RPM self.info.wheels.min_velocity = 2 * pi * 30 / 60 # 30 RPM self.info.wheels.vel_left = 0 self.info.wheels.vel_right = 0 self.info.ir_sensors = Struct() self.info.ir_sensors.poses = ir_sensor_poses self.info.ir_sensors.rmax = 0.3 self.info.ir_sensors.rmin = 0.04 self.info.ir_sensors.readings = [133] * len(self.info.ir_sensors.poses) self.walls = Cloud(0) # Black, no readings # The constructor is called here because otherwise set_pose fails RealBot.__init__(self, pose, color) # Connect to bot... # This code will raise an exception if not able to connect if options is None: self.log("No IP/port supplied to connect to the robot") qb_comm.__init__(self, "localhost", "localhost", 5005) else: try: qb_comm.__init__(self, options.baseIP, options.robotIP, options.port) except AttributeError: self.log( "No IP/port supplied in the options to connect to the robot" ) qb_comm.__init__(self, "localhost", "localhost", 5005) self.ping() # Check if the robot is there self.pause() # Initialize self.__paused
def init_default_parameters(self): """Sets the default PID parameters, goal, and velocity""" self.parameters = Struct({'left': 0, 'right': 0})
def __construct_world(self): """Creates objects previously loaded from the world xml file. This function uses the world in ``self.__world``. All the objects will be created anew, including robots and supervisors. All of the user's code is reloaded. """ if self.__world is None: return helpers.unload_user_modules() self.__state = DRAW_ONCE self.__robots = [] self.__obstacles = [] self.__supervisors = [] self.__background = [] self.__trackers = [] self.__qtree = None for thing in self.__world: if thing.type == 'robot': try: # Create robot robot_class = helpers.load_by_name(thing.robot.type, 'robots') if thing.robot.options is not None: robot = robot_class(thing.robot.pose, options=Struct( thing.robot.options)) else: robot = robot_class(thing.robot.pose) robot.set_logqueue(self.__log_queue) if thing.robot.color is not None: robot.set_color(thing.robot.color) elif len(self.__robots) < 8: robot.set_color(self.__nice_colors[len(self.__robots)]) # Create supervisor sup_class = helpers.load_by_name(thing.supervisor.type, 'supervisors') info = robot.get_info() info.color = robot.get_color() if thing.supervisor.options is not None: supervisor = sup_class(thing.robot.pose, info, options=Struct( thing.supervisor.options)) else: supervisor = sup_class(thing.robot.pose, info) supervisor.set_logqueue(self.__log_queue) name = "Robot {}: {}".format( len(self.__robots) + 1, sup_class.__name__) if self.__supervisor_param_cache is not None and \ len(self.__supervisor_param_cache) > len(self.__supervisors): supervisor.set_parameters( self.__supervisor_param_cache[len( self.__supervisors)]) self._out_queue.put( ("make_param_window", (robot, name, supervisor.get_ui_description()))) self.__supervisors.append(supervisor) # append robot after supervisor for the case of exceptions self.__robots.append(robot) # Create trackers self.__trackers.append( simobject.Path(thing.robot.pose, robot.get_color())) except: self.log( "[Simulator.construct_world] Robot creation failed!") raise #raise Exception('[Simulator.construct_world] Unknown robot type!') elif thing.type == 'obstacle': if thing.polygon.color is None: thing.polygon.color = 0xFF0000 self.__obstacles.append( simobject.Polygon(thing.polygon.pose, thing.polygon.points, thing.polygon.color)) elif thing.type == 'marker': if thing.polygon.color is None: thing.polygon.color = 0x00FF00 self.__background.append( simobject.Polygon(thing.polygon.pose, thing.polygon.points, thing.polygon.color)) else: raise Exception( '[Simulator.construct_world] Unknown object: ' + str(thing.type)) self.__time = 0.0 if not self.__robots: raise Exception('[Simulator.construct_world] No robot specified!') else: self.__recalculate_default_zoom() if not self.__center_on_robot: self.focus_on_world() self.__supervisor_param_cache = None self.step_simulation() self._out_queue.put(('reset', ()))
jet_maps = pickle.load(infile) print 'build jet by jet map from flat tree' flat_map = {} inputFile = io.root_open('trees/CombinedSV_ALL.root') flat_tree = inputFile.tree for entry in flat_tree: evtid = (entry.run, entry.lumi, entry.evt) if evtid not in jet_maps: continue if (entry.jetPt, entry.jetEta) not in jet_maps[evtid]: continue if evtid not in flat_map: flat_map[evtid] = {} flat_map[evtid][(entry.jetPt, entry.jetEta)] = Struct.from_entry(entry) handle = Handle('std::vector<pat::Jet>') vtx_handle = Handle('vector<reco::Vertex>') print 'analyzing pat output' same_mva, same_inputs = 0, 0 n_analyzed_jets = 0 different_jets = {} for evt in events: evtid = (evt.eventAuxiliary().run(), evt.eventAuxiliary().luminosityBlock(), evt.eventAuxiliary().event()) evt.getByLabel('selectedPatJets', handle) jets = handle.product() ext_jets = [] for jet in jets:
def __init__(self, pose, color = 0xFFFFFF): Robot.__init__(self, pose, color) # create shape self._p1 = np.array([[-0.031, 0.043, 1], [-0.031, -0.043, 1], [ 0.033, -0.043, 1], [ 0.052, -0.021, 1], [ 0.057, 0 , 1], [ 0.052, 0.021, 1], [ 0.033, 0.043, 1]]) self._p2 = np.array([[-0.024, 0.064, 1], [ 0.033, 0.064, 1], [ 0.057, 0.043, 1], [ 0.074, 0.010, 1], [ 0.074, -0.010, 1], [ 0.057, -0.043, 1], [ 0.033, -0.064, 1], [-0.025, -0.064, 1], [-0.042, -0.043, 1], [-0.048, -0.010, 1], [-0.048, 0.010, 1], [-0.042, 0.043, 1]]) # create IR sensors self.ir_sensors = [] ir_sensor_poses = [ Pose(-0.038, 0.048, np.radians(128)), Pose( 0.019, 0.064, np.radians(75)), Pose( 0.050, 0.050, np.radians(42)), Pose( 0.070, 0.017, np.radians(13)), Pose( 0.070, -0.017, np.radians(-13)), Pose( 0.050, -0.050, np.radians(-42)), Pose( 0.019, -0.064, np.radians(-75)), Pose(-0.038, -0.048, np.radians(-128)), Pose(-0.048, 0.000, np.radians(180)) ] for pose in ir_sensor_poses: self.ir_sensors.append(Khepera3_IRSensor(pose,self)) # initialize motion self.ang_velocity = (0.0,0.0) self.info = Struct() self.info.wheels = Struct() # these were the original parameters self.info.wheels.radius = 0.021 self.info.wheels.base_length = 0.0885 self.info.wheels.ticks_per_rev = 2764.8 self.info.speed_factor = 6.2953e-6 self.info.wheels.left_ticks = 0 self.info.wheels.right_ticks = 0 self.left_revolutions = 0.0 self.right_revolutions = 0.0 self.info.ir_sensors = Struct() self.info.ir_sensors.poses = ir_sensor_poses self.info.ir_sensors.readings = None self.info.ir_sensors.rmax = 0.2 self.info.ir_sensors.rmin = 0.02
def init_default_parameters(self): """Init parameters with default values""" self.parameters = Struct({"direction":'left', \ "distance":0.1, \ "velocity":{"v":0.2}, \ "gains":{"kp":3.0, "ki": 0.1, "kd": 0.0}})