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 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_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 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 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 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}})
