def start_test(self, challenge):
vals = self.parseChallenge(challenge)
if 'v' not in vals or 'w' not in vals:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
vd = vals['v']
wd = vals['w']
QuickBotSupervisor = helpers.load_by_name('week3.QBGTGSupervisor',
'supervisors')
QuickBot = helpers.load_by_name('QuickBot', 'robots')
from pose import Pose
bot = QuickBot(Pose())
info = bot.get_info()
info.color = 0
s = QuickBotSupervisor(Pose(), info)
vld, vrd = s.uni2diff((vd, wd))
vl, vr = s.ensure_w((vld, vrd))
# Clamp to robot maxima
vl = max(-info.wheels.max_velocity, min(info.wheels.max_velocity, vl))
vr = max(-info.wheels.max_velocity, min(info.wheels.max_velocity, vr))
v, w = bot.diff2uni((vl, vr))
self.testsuite.respond("{:0.3f}".format(abs(w - wd) / wd))
def start_test(self,challenge):
vals = self.parseChallenge(challenge)
if 'v' not in vals or 'w' not in vals:
raise CourseraException("Unknown challenge format. Please contact developers for assistance.")
vd = vals['v']
wd = vals['w']
QuickBotSupervisor = helpers.load_by_name('week3.QBGTGSupervisor','supervisors')
QuickBot = helpers.load_by_name('QuickBot','robots')
from pose import Pose
bot = QuickBot(Pose())
info = bot.get_info()
info.color = 0
s = QuickBotSupervisor(Pose(),info)
vld, vrd = s.uni2diff((vd,wd))
vl, vr = s.ensure_w((vld,vrd))
# Clamp to robot maxima
vl = max(-info.wheels.max_velocity, min(info.wheels.max_velocity, vl))
vr = max(-info.wheels.max_velocity, min(info.wheels.max_velocity, vr))
v, w = bot.diff2uni((vl,vr))
self.testsuite.respond("{:0.3f}".format(abs(w-wd)/wd))
def start_test(self,challenge):
vals = self.parseChallenge(challenge)
if 'dist_1' not in vals or 'x' not in vals or 'y' not in vals or 'theta' not in vals:
raise CourseraException("Unknown challenge format. Please contact developers for assistance.")
rpose = Pose(vals['x'],vals['y'],vals['theta'])
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))
]
params = helpers.Struct()
params.sensor_poses = [p >> rpose for p in ir_sensor_poses]
params.gains = helpers.Struct({'kp':0, 'ki':0, 'kd':0})
state = helpers.Struct()
state.sensor_distances = [vals['dist_1'], 0.3, 0.3, vals['dist_1'], 0.3]
AvoidObstacles = helpers.load_by_name('week4.AvoidObstacles','controllers')
testAO = AvoidObstacles(params)
testAO.get_heading(state)
vs = testAO.vectors
error_1 = math.sqrt((vs[0][0] - 0.3637)**2 + (vs[0][1] + 0.0545)**2)
error_2 = math.sqrt((vs[3][0] + 0.0895)**2 + (vs[3][1] + 0.2932)**2)
self.testsuite.respond("{:0.3f},{:0.3f}".format(error_1,error_2))
def add_controller(self, module_string, parameters):
"""Create and return a controller instance for a given controller class.
:param module_string: a string specifying a class in a module. See :ref:`module-string`
:type module_string: string
:param parameters: a parameter structure to be passed to the controller constructor
:type paramaters: :class:`~helpers.Struct`
"""
module, controller_class = helpers.load_by_name(module_string, 'controllers')
return controller_class(parameters)
def create_controller(self, module_string, parameters):
"""Create and return a controller instance for a given controller class.
:param module_string: a string specifying a class in a module.
See :ref:`module-string`
:type module_string: string
:param parameters: a parameter structure to be passed to the controller constructor
:type paramaters: :class:`~helpers.Struct`
"""
controller_class = helpers.load_by_name(module_string, 'controllers')
return controller_class(parameters)
def update_robot(self,type,pose):
robot_class = helpers.load_by_name(type,'robots')
robot = robot_class(pose)
robot.set_logqueue(self.__log_queue)
#Empty list
del self.__robots[:]
# append robot after supervisor for the case of exceptions
self.__robots.append(robot)
# Create trackers
self.__trackers.append(simobject.Path(pose,robot.get_color()))
if self._sim_server is not None:
self._sim_server.set_robot(robot)
def start_test(self, challenge):
print(challenge)
vals = self.parseChallenge(challenge)
print(vals)
if 'dist_1' not in vals or 'x' not in vals or 'y' not in vals or 'theta' not in vals:
raise CourseraException(
"Unknown challenge format. Please contact developers for assistance."
)
rpose = Pose(vals['x'], vals['y'], vals['theta'])
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))
]
params = helpers.Struct()
params.sensor_poses = [p >> rpose for p in ir_sensor_poses]
params.gains = helpers.Struct({'kp': 0, 'ki': 0, 'kd': 0})
state = helpers.Struct()
state.sensor_distances = [
vals['dist_1'], 0.3, 0.3, vals['dist_1'], 0.3
]
AvoidObstacles = helpers.load_by_name('week4_solved.AvoidObstacles',
'controllers')
testAO = AvoidObstacles(params)
testAO.get_heading(state)
vs = testAO.vectors
error_1 = math.sqrt((vs[0][0] - 0.3637)**2 + (vs[0][1] + 0.0545)**2)
error_2 = math.sqrt((vs[3][0] + 0.0895)**2 + (vs[3][1] + 0.2932)**2)
self.testsuite.respond("{:0.3f},{:0.3f}".format(error_1, error_2))
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:
thing_type = thing[0]
if thing_type == 'robot':
robot_type, supervisor_type, robot_pose, robot_color = thing[
1:5]
try:
# Create robot
robot_class = helpers.load_by_name(robot_type, 'robots')
robot = robot_class(pose.Pose(robot_pose))
if robot_color is not None:
robot.set_color(robot_color)
elif len(self.__robots) < 8:
robot.set_color(self.__nice_colors[len(self.__robots)])
# Create supervisor
sup_class = helpers.load_by_name(supervisor_type,
'supervisors')
info = robot.get_info()
info.color = robot.get_color()
supervisor = sup_class(robot.get_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(robot.get_pose(), robot))
self.__trackers[-1].set_color(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':
obstacle_pose, obstacle_coords, obstacle_color = thing[1:4]
if obstacle_color is None:
obstacle_color = 0xFF0000
self.__obstacles.append(
simobject.Polygon(pose.Pose(obstacle_pose),
obstacle_coords, obstacle_color))
elif thing_type == 'marker':
obj_pose, obj_coords, obj_color = thing[1:4]
if obj_color is None:
obj_color = 0x00FF00
self.__background.append(
simobject.Polygon(pose.Pose(obj_pose), obj_coords,
obj_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', ()))
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:
thing_type = thing[0]
if thing_type == 'robot':
robot_type, supervisor_type, robot_pose, robot_color = thing[1:5]
try:
# Create robot
robot_class = helpers.load_by_name(robot_type,'robots')
robot = robot_class(pose.Pose(robot_pose))
if robot_color is not None:
robot.set_color(robot_color)
elif len(self.__robots) < 8:
robot.set_color(self.__nice_colors[len(self.__robots)])
# Create supervisor
sup_class = helpers.load_by_name(supervisor_type,'supervisors')
info = robot.get_info()
info.color = robot.get_color()
supervisor = sup_class(robot.get_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(robot.get_pose(),robot))
self.__trackers[-1].set_color(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':
obstacle_pose, obstacle_coords, obstacle_color = thing[1:4]
if obstacle_color is None:
obstacle_color = 0xFF0000
self.__obstacles.append(
simobject.Polygon(pose.Pose(obstacle_pose),
obstacle_coords,
obstacle_color))
elif thing_type == 'marker':
obj_pose, obj_coords, obj_color = thing[1:4]
if obj_color is None:
obj_color = 0x00FF00
self.__background.append(
simobject.Polygon(pose.Pose(obj_pose),
obj_coords,
obj_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',()))
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
if self.__robot is not None:
del self.__robot
self.__robot = None
del self.__supervisor
self.__supervisor = None
del self.tracker
self.__background = []
for thing in self.__world:
if thing.type == 'robot' and self.__robot is None:
try:
robot_class = helpers.load_by_name(thing.robot.type,'robots')
if thing.robot.options is not None:
self.__robot = robot_class(thing.robot.pose, options = Struct(thing.robot.options))
else:
self.__robot = robot_class(thing.robot.pose)
self.__robot.set_logqueue(self.__log_queue)
if thing.robot.color is not None:
self.__robot.set_color(thing.robot.color)
else:
self.__robot.set_color(self.__nice_colors[0])
# Create supervisor
sup_class = helpers.load_by_name(thing.supervisor.type,'supervisors')
info = self.__robot.get_info()
info.color = self.__robot.get_color()
if thing.supervisor.options is not None:
self.__supervisor = sup_class(thing.robot.pose, info, options = Struct(thing.supervisor.options))
else:
self.__supervisor = sup_class(thing.robot.pose, info)
self.__supervisor.set_logqueue(self.__log_queue)
name = "Robot {}".format(sup_class.__name__)
if self.__supervisor_param_cache is not None:
self.__supervisor.set_parameters(self.__supervisor_param_cache)
self._out_queue.put(("make_param_window",
(self.__robot, name,
self.__supervisor.get_ui_description())))
# Create trackers
self.__tracker = simobject.Path(thing.robot.pose,self.__robot.get_color())
except:
self.log("[PCLoop.construct_world] Robot creation failed!")
if self.__robot is not None:
del self.__robot
self.__robot = None
self.__supervisor = None
gc.collect()
raise
#raise Exception('[PCLoop.construct_world] Unknown robot type!')
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('[PCLoop.construct_world] Unknown object: '
+ str(thing.type))
self.__time = 0.0
if self.__robot is None:
raise Exception('[PCLoop.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.__state = DRAW_ONCE
self._out_queue.put(('reset',()))
