def __init__(self, robot_pose, robot_info):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
# Fill in poses for the controller
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
# Add controllers
self.avoidobstacles = self.create_controller(
'avoidobstacles.AvoidObstacles', self.parameters)
self.gtg = self.create_controller('gotogoal.GoToGoal', self.parameters)
self.hold = self.create_controller('hold.Hold', None)
# Transitions if at goal/obstacle
self.add_controller(self.hold)
self.add_controller(self.gtg, (self.at_goal, self.hold),
(self.at_obstacle, self.avoidobstacles))
self.add_controller(
self.avoidobstacles,
(self.at_goal, self.hold),
(self.free, self.gtg),
)
# Start in the 'go-to-goal' state
self.current = self.gtg
def __init__(self, robot_pose, robot_info):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
# The maximal distance to an obstacle (inexact)
self.distmax = robot_info.ir_sensors.rmax + robot_info.wheels.base_length / 2
# Fill in some parameters
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.parameters.ir_max = robot_info.ir_sensors.rmax
self.parameters.direction = 'left'
self.parameters.distance = self.distmax * 0.85
self.process_state_info(robot_info)
#Add controllers
self.gtg = self.create_controller('GoToGoal', self.parameters)
self.avoidobstacles = self.create_controller('AvoidObstacles',
self.parameters)
self.wall = self.create_controller('FollowWall', self.parameters)
self.hold = self.create_controller('Hold', None)
# Define transitions
self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg))
self.add_controller(self.gtg, (self.at_goal, self.hold),
(self.at_wall, self.wall))
self.add_controller(self.wall, (self.at_goal, self.hold),
(self.unsafe, self.avoidobstacles),
(self.wall_cleared, self.gtg))
self.add_controller(self.avoidobstacles, (self.at_goal, self.hold),
(self.safe, self.wall))
# Start in the 'go-to-goal' state
self.current = self.gtg
def draw(self, renderer):
K3Supervisor.draw(self,renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size*5
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
goal_angle = self.gtg.get_heading_angle(self.parameters)
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(goal_angle),
arrow_length*sin(goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0,0,
arrow_length*cos(away_angle),
arrow_length*sin(away_angle))
# Week 3
renderer.set_pen(0)
for v in self.avoidobstacles.vectors:
x,y,z = v
renderer.push_state()
renderer.translate(x,y)
renderer.rotate(atan2(y,x))
renderer.draw_line(0.01,0.01,-0.01,-0.01)
renderer.draw_line(0.01,-0.01,-0.01,0.01)
renderer.pop_state()
def draw(self, renderer):
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
goal_angle = self.gtg.get_heading_angle(self.parameters)
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle),
arrow_length * sin(goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0, 0, arrow_length * cos(away_angle),
arrow_length * sin(away_angle))
# Week 3
renderer.set_pen(0)
for v in self.avoidobstacles.vectors:
x, y, z = v
renderer.push_state()
renderer.translate(x, y)
renderer.rotate(atan2(y, x))
renderer.draw_line(0.01, 0.01, -0.01, -0.01)
renderer.draw_line(0.01, -0.01, -0.01, 0.01)
renderer.pop_state()
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
if self.current == self.gtg:
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0,
arrow_length * cos(self.gtg.heading_angle),
arrow_length * sin(self.gtg.heading_angle))
elif self.current == self.avoidobstacles:
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(
0, 0, arrow_length * cos(self.avoidobstacles.heading_angle),
arrow_length * sin(self.avoidobstacles.heading_angle))
elif self.current == self.wall:
# Draw vector to wall:
renderer.set_pen(0x0000FF)
renderer.draw_arrow(0, 0, self.wall.to_wall_vector[0],
self.wall.to_wall_vector[1])
# Draw
renderer.set_pen(0xFF00FF)
renderer.push_state()
renderer.translate(self.wall.to_wall_vector[0],
self.wall.to_wall_vector[1])
renderer.draw_arrow(0, 0, self.wall.along_wall_vector[0],
self.wall.along_wall_vector[1])
renderer.pop_state()
# Draw heading (who knows, it might not be along_wall)
renderer.set_pen(0xFF00FF)
renderer.draw_arrow(0, 0,
arrow_length * cos(self.wall.heading_angle),
arrow_length * sin(self.wall.heading_angle))
# Important sensors
renderer.set_pen(0)
for v in self.wall.vectors:
x, y, z = v
renderer.push_state()
renderer.translate(x, y)
renderer.rotate(atan2(y, x))
renderer.draw_line(0.01, 0.01, -0.01, -0.01)
renderer.draw_line(0.01, -0.01, -0.01, 0.01)
renderer.pop_state()
elif self.current == self.path:
goal_angle = self.path.get_heading_angle(self.parameters)
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle),
arrow_length * sin(goal_angle))
def __init__(self, robot_pose, robot_info):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
# The maximal distance to an obstacle (inexact)
self.distmax = robot_info.ir_sensors.rmax + robot_info.wheels.base_length/2
# Fill in some parameters
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.parameters.ir_max = robot_info.ir_sensors.rmax
self.parameters.direction = 'left'
self.parameters.distance = self.distmax*0.85
self.parameters.path = [(1.0, 0.5), (0.5, 1.0)]
self.process_state_info(robot_info)
#Add controllers
self.movingtopoint1 = self.create_controller('MovingToPoint1', self.parameters)
self.movingtopoint2 = self.create_controller('MovingToPoint2', self.parameters)
# Define transitions
self.add_controller(self.movingtopoint1,
(self.at_point1, self.movingtopoint2))
self.add_controller(self.movingtopoint2,
(self.at_point2, self.movingtopoint1))
# Start in the 'go-to-goal' state
self.current = self.movingtopoint1
def set_parameters(self, params):
"""Set parameters for itself and the controllers"""
K3Supervisor.set_parameters(self, params)
self.gtg.set_parameters(self.parameters)
self.avoidobstacles.set_parameters(self.parameters)
self.wall.set_parameters(self.parameters)
self.path.set_parameters(self.parameters)
def set_parameters(self, params):
"""Set parameters for itself and the controllers"""
K3Supervisor.set_parameters(self, params)
self.gtg.set_parameters(self.parameters)
self.avoidobstacles.set_parameters(self.parameters)
self.wall.set_parameters(self.parameters)
self.path.set_parameters(self.parameters)
def process_state_info(self, state):
"""Update state parameters for the controllers and self"""
K3Supervisor.process_state_info(self,state)
# The pose for controllers
self.parameters.pose = self.pose_est
# Distance to the goal
self.distance_from_goal = sqrt((self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2)
# Sensor readings in real units
self.parameters.sensor_distances = self.get_ir_distances()
# Smallest reading translated into distance from center of robot
vectors = \
numpy.array(
[numpy.dot(
p.get_transformation(),
numpy.array([d,0,1])
)
for d, p in zip(self.parameters.sensor_distances, self.parameters.sensor_poses) \
if abs(p.theta) < 2.4] )
self.distmin = min((sqrt(a[0]**2 + a[1]**2) for a in vectors))
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self,renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size*5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(self.blending.goal_angle),
arrow_length*sin(self.blending.goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0,0,
arrow_length*cos(self.blending.away_angle),
arrow_length*sin(self.blending.away_angle))
# Draw heading
renderer.set_pen(0x0000FF)
renderer.draw_arrow(0,0,
arrow_length*cos(self.blending.heading_angle),
arrow_length*sin(self.blending.heading_angle))
def __init__(self, robot_pose, robot_info):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
# The maximal distance to an obstacle (inexact)
self.distmax = robot_info.ir_sensors.rmax + robot_info.wheels.base_length / 2
# Fill in some parameters
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.parameters.ir_max = robot_info.ir_sensors.rmax
self.parameters.direction = 'left'
self.parameters.distance = self.distmax * 0.85
self.parameters.path = [(1.0, 0.5), (0.5, 1.0)]
self.process_state_info(robot_info)
#Add controllers
self.movingtopoint1 = self.create_controller('MovingToPoint1',
self.parameters)
self.movingtopoint2 = self.create_controller('MovingToPoint2',
self.parameters)
# Define transitions
self.add_controller(self.movingtopoint1,
(self.at_point1, self.movingtopoint2))
self.add_controller(self.movingtopoint2,
(self.at_point2, self.movingtopoint1))
# Start in the 'go-to-goal' state
self.current = self.movingtopoint1
def process_state_info(self, state):
"""Update state parameters for the controllers and self"""
K3Supervisor.process_state_info(self, state)
# The pose for controllers
self.parameters.pose = self.pose_est
# Distance to the goal
self.distance_from_goal = sqrt(
(self.pose_est.x - self.parameters.goal.x)**2 +
(self.pose_est.y - self.parameters.goal.y)**2)
# Sensor readings in real units
self.parameters.sensor_distances = self.get_ir_distances()
# Smallest reading translated into distance from center of robot
vectors = \
numpy.array(
[numpy.dot(
p.get_transformation(),
numpy.array([d,0,1])
)
for d, p in zip(self.parameters.sensor_distances, self.parameters.sensor_poses) \
if abs(p.theta) < 2.4] )
self.distmin = min((sqrt(a[0]**2 + a[1]**2) for a in vectors))
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
if self.current == self.gtg:
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(
0, 0, arrow_length * cos(self.gtg.heading_angle), arrow_length * sin(self.gtg.heading_angle)
)
elif self.current == self.avoidobstacles:
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(
0,
0,
arrow_length * cos(self.avoidobstacles.heading_angle),
arrow_length * sin(self.avoidobstacles.heading_angle),
)
elif self.current == self.wall:
# Draw vector to wall:
renderer.set_pen(0x0000FF)
renderer.draw_arrow(0, 0, self.wall.to_wall_vector[0], self.wall.to_wall_vector[1])
# Draw
renderer.set_pen(0xFF00FF)
renderer.push_state()
renderer.translate(self.wall.to_wall_vector[0], self.wall.to_wall_vector[1])
renderer.draw_arrow(0, 0, self.wall.along_wall_vector[0], self.wall.along_wall_vector[1])
renderer.pop_state()
# Draw heading (who knows, it might not be along_wall)
renderer.set_pen(0xFF00FF)
renderer.draw_arrow(
0, 0, arrow_length * cos(self.wall.heading_angle), arrow_length * sin(self.wall.heading_angle)
)
# Important sensors
renderer.set_pen(0)
for v in self.wall.vectors:
x, y, z = v
renderer.push_state()
renderer.translate(x, y)
renderer.rotate(atan2(y, x))
renderer.draw_line(0.01, 0.01, -0.01, -0.01)
renderer.draw_line(0.01, -0.01, -0.01, 0.01)
renderer.pop_state()
elif self.current == self.path:
goal_angle = self.path.get_heading_angle(self.parameters)
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle), arrow_length * sin(goal_angle))
def __init__(self, robot_pose, robot_info):
K3Supervisor.__init__(self, robot_pose, robot_info)
#Add at least one controller
self.gtg = self.add_controller('gotogoal.GoToGoal', self.parameters.gains)
#Set default controller
self.current = self.gtg
def __init__(self, robot_pose, robot_info):
K3Supervisor.__init__(self, robot_pose, robot_info)
#Add at least one controller
self.gtg = self.add_controller('gotogoal.GoToGoal',
self.parameters.gains)
#Set default controller
self.current = self.gtg
def process_state_info(self, state):
"""Sets parameters for supervisors and selects a supervisor
This code is run every time the supervisor executes"""
K3Supervisor.process_state_info(self, state)
#Add some data to variable self.parameters
#Below are some default parameters
#-------------------------------------
self.parameters.pose = self.pose_est
self.parameters.ir_readings = self.robot.ir_sensors.readings
def __init__(self, robot_pose, robot_info):
"""Creates an avoid-obstacle controller and go-to-goal controller"""
K3Supervisor.__init__(self, robot_pose, robot_info)
#Add controllers ( go to goal is default)
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.avoidobstacles = self.get_controller('avoidobstacles.AvoidObstacles', self.parameters)
self.gtg = self.get_controller('gotogoal.GoToGoal', self.parameters)
self.current = self.gtg
self.old_omega = 0
def __init__(self, robot_pose, robot_info):
"""Creates an avoid-obstacle controller and go-to-goal controller"""
K3Supervisor.__init__(self, robot_pose, robot_info)
#Add controllers ( go to goal is default)
self.ui_params.sensor_angles = [pose.theta for pose in robot_info.ir_sensors.poses]
self.avoidobstacles = self.add_controller('avoidobstacles.AvoidObstacles', self.ui_params)
self.gtg = self.add_controller('gotogoal.GoToGoal', self.ui_params.gains)
self.hold = self.add_controller('hold.Hold', None)
self.current = self.gtg
def process_state_info(self, state):
"""Sets parameters for supervisors and selects a supervisor
This code is run every time the supervisor executes"""
K3Supervisor.process_state_info(self, state)
#Add some data to variable self.parameters
#Below are some default parameters
#-------------------------------------
self.parameters.pose = self.pose_est
self.parameters.ir_readings = self.robot.ir_sensors.readings
def process_state_info(self, state):
"""Update state parameters for the controllers and self"""
K3Supervisor.process_state_info(self,state)
# The pose for controllers
self.parameters.pose = self.pose_est
# Distance to the goal
self.distance_from_goal = sqrt((self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2)
# Sensor readings in real units
self.parameters.sensor_distances = self.get_ir_distances()
def __init__(self, robot_pose, robot_info):
"""Creates an avoid-obstacle controller and go-to-goal controller"""
K3Supervisor.__init__(self, robot_pose, robot_info)
#Add controllers ( go to goal is default)
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.avoidobstacles = self.get_controller(
'avoidobstacles.AvoidObstacles', self.parameters)
self.gtg = self.get_controller('gotogoal.GoToGoal', self.parameters)
self.current = self.gtg
self.old_omega = 0
def process_state_info(self, state):
"""Update state parameters for the controllers and self"""
K3Supervisor.process_state_info(self, state)
# The pose for controllers
self.parameters.pose = self.pose_est
# Distance to the goal
self.distance_from_goal = sqrt(
(self.pose_est.x - self.parameters.goal.x)**2 +
(self.pose_est.y - self.parameters.goal.y)**2)
# Sensor readings in real units
self.parameters.sensor_distances = self.get_ir_distances()
def draw(self, renderer):
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
# Draw direction from obstacles
renderer.set_pen(0xFF0000)
arrow_length = self.robot_size * 5
renderer.draw_arrow(0, 0,
arrow_length * cos(self.avoidobstacles.away_angle),
arrow_length * sin(self.avoidobstacles.away_angle))
# Draw direction to goal
renderer.set_pen(0x444444)
goal_angle = atan2(self.parameters.goal.y - self.pose_est.y,
self.parameters.goal.x - self.pose_est.x) \
- self.pose_est.theta
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle),
arrow_length * sin(goal_angle))
def __init__(self, robot_pose, robot_info):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
# Fill in poses for the controller
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
# Add controllers
self.blending = self.create_controller('blending.Blending', self.parameters)
self.hold = self.create_controller('hold.Hold', None)
# Transitions if at goal
self.add_controller(self.hold,
(lambda: not self.at_goal(), self.blending))
self.add_controller(self.blending,
(self.at_goal,self.hold))
# Start in the 'blending' state
self.current = self.blending
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
goal_angle = self.gtg.get_heading_angle(self.parameters)
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle),
arrow_length * sin(goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0, 0, arrow_length * cos(away_angle),
arrow_length * sin(away_angle))
def __init__(self, robot_pose, robot_info):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
# Fill in poses for the controller
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
# Add controllers
self.blending = self.create_controller('blending.Blending',
self.parameters)
self.hold = self.create_controller('hold.Hold', None)
# Transitions if at goal
self.add_controller(self.hold,
(lambda: not self.at_goal(), self.blending))
self.add_controller(self.blending, (self.at_goal, self.hold))
# Start in the 'blending' state
self.current = self.blending
def draw(self, renderer):
K3Supervisor.draw(self,renderer)
renderer.set_pose(self.pose_est)
# Draw direction from obstacles
renderer.set_pen(0xFF0000)
arrow_length = self.robot_size*5
renderer.draw_arrow(0,0,
arrow_length*cos(self.avoidobstacles.away_angle),
arrow_length*sin(self.avoidobstacles.away_angle))
# Draw direction to goal
renderer.set_pen(0x444444)
goal_angle = atan2(self.parameters.goal.y - self.pose_est.y,
self.parameters.goal.x - self.pose_est.x) \
- self.pose_est.theta
renderer.draw_arrow(0,0,
arrow_length*cos(goal_angle),
arrow_length*sin(goal_angle))
def __init__(self, robot_pose, robot_info):
"""Creates an avoid-obstacle controller and go-to-goal controller"""
K3Supervisor.__init__(self, robot_pose, robot_info)
#Add controllers ( go to goal is default)
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.avoidobstacles = self.get_controller('avoidobstacles.AvoidObstacles', self.parameters)
self.gtg = self.get_controller('gotogoal.GoToGoal', self.parameters)
self.hold = self.get_controller('hold.Hold', None)
self.add_controller(self.hold)
self.add_controller(self.gtg,
(self.at_goal, self.hold),
(self.at_obstacle, self.avoidobstacles))
self.add_controller(self.avoidobstacles,
(self.at_goal, self.hold),
(self.free, self.gtg),
)
self.current = self.gtg
def __init__(self, robot_pose, robot_info):
"""Creates an avoid-obstacle controller and go-to-goal controller"""
K3Supervisor.__init__(self, robot_pose, robot_info)
#Add controllers ( go to goal is default)
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.avoidobstacles = self.get_controller(
'avoidobstacles.AvoidObstacles', self.parameters)
self.gtg = self.get_controller('gotogoal.GoToGoal', self.parameters)
self.hold = self.get_controller('hold.Hold', None)
self.add_controller(self.hold)
self.add_controller(self.gtg, (self.at_goal, self.hold),
(self.at_obstacle, self.avoidobstacles))
self.add_controller(
self.avoidobstacles,
(self.at_goal, self.hold),
(self.free, self.gtg),
)
self.current = self.gtg
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.goal_angle),
arrow_length * sin(self.blending.goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.away_angle),
arrow_length * sin(self.blending.away_angle))
# Draw heading
renderer.set_pen(0x0000FF)
renderer.draw_arrow(0, 0,
arrow_length * cos(self.blending.heading_angle),
arrow_length * sin(self.blending.heading_angle))
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self,renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size*5
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
goal_angle = self.gtg.get_heading_angle(self.parameters)
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(goal_angle),
arrow_length*sin(goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0,0,
arrow_length*cos(away_angle),
arrow_length*sin(away_angle))
def __init__(self, robot_pose, robot_info):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
# The maximal distance to an obstacle (inexact)
self.distmax = robot_info.ir_sensors.rmax + robot_info.wheels.base_length/2
# Fill in some parameters
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.parameters.ir_max = robot_info.ir_sensors.rmax
self.parameters.direction = 'left'
self.parameters.distance = self.distmax*0.85
self.process_state_info(robot_info)
#Add controllers
self.gtg = self.create_controller('GoToGoal', self.parameters)
self.avoidobstacles = self.create_controller('AvoidObstacles', self.parameters)
self.wall = self.create_controller('FollowWall', self.parameters)
self.hold = self.create_controller('Hold', None)
# Define transitions
self.add_controller(self.hold,
(lambda: not self.at_goal(), self.gtg))
self.add_controller(self.gtg,
(self.at_goal, self.hold),
(self.at_wall, self.wall))
self.add_controller(self.wall,
(self.at_goal,self.hold),
(self.unsafe, self.avoidobstacles),
(self.wall_cleared, self.gtg))
self.add_controller(self.avoidobstacles,
(self.at_goal, self.hold),
(self.safe, self.wall))
# Start in the 'go-to-goal' state
self.current = self.gtg
def __init__(self, robot_pose, robot_info):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
# Fill in poses for the controller
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
# Add controllers
self.avoidobstacles = self.create_controller('avoidobstacles.AvoidObstacles', self.parameters)
self.gtg = self.create_controller('gotogoal.GoToGoal', self.parameters)
self.hold = self.create_controller('hold.Hold', None)
# Transitions if at goal/obstacle
self.add_controller(self.hold)
self.add_controller(self.gtg,
(self.at_goal, self.hold),
(self.at_obstacle, self.avoidobstacles))
self.add_controller(self.avoidobstacles,
(self.at_goal, self.hold),
(self.free, self.gtg),
)
# Start in the 'go-to-goal' state
self.current = self.gtg
def __init__(self, robot_pose, robot_info, options=None):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
self.extgoal = False
if options is not None:
try:
self.parameters.goal.x = options.x
self.parameters.goal.y = options.y
self.extgoal = True
except Exception:
pass
# The maximal distance to an obstacle (inexact)
self.distmax = robot_info.ir_sensors.rmax + robot_info.wheels.base_length / 2
# Fill in some parameters
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.parameters.ir_max = robot_info.ir_sensors.rmax
self.parameters.direction = "left"
self.parameters.distance = self.distmax * 0.85
self.parameters.ga_path = ga.ga_execute((0, 0), (self.parameters.goal.x, self.parameters.goal.y))
self.parameters.ga_path.append((self.parameters.goal.x, self.parameters.goal.y))
global global_cnt
global_cnt = len(self.parameters.ga_path)
point_cnt = self.parameters.point_cnt
self.process_state_info(robot_info)
# Add controllers
self.gtg = self.create_controller("GoToGoal", self.parameters)
self.avoidobstacles = self.create_controller("K3_AvoidObstacles", self.parameters)
self.wall = self.create_controller("FollowWall", self.parameters)
self.hold = self.create_controller("Hold", None)
self.path = self.create_controller("FollowPath", self.parameters)
# Define transitions
# self.add_controller(self.hold,
# (lambda: not self.at_goal(), self.gtg))
# self.add_controller(self.gtg,
# (self.at_goal, self.hold),
# (self.at_wall, self.wall))
self.add_controller(
self.wall, (self.at_goal, self.hold), (self.unsafe, self.avoidobstacles), (self.wall_cleared, self.gtg)
)
# self.add_controller(self.avoidobstacles,
# (self.at_goal, self.hold),
# (self.safe, self.wall))
self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg))
self.add_controller(self.avoidobstacles, (self.at_goal, self.hold), (self.free, self.path))
self.add_controller(
self.path,
(lambda: self.next_point(), self.path),
(self.at_goal, self.hold),
# (lambda: self.parameters.point_cnt == len(self.parameters.ga_path) - 1 and not self.next_point(), self.gtg),
(self.at_obstacle, self.avoidobstacles),
)
self.current = self.path
def set_parameters(self,params):
"""Set parameters for itself and the controllers"""
K3Supervisor.set_parameters(self,params)
self.movingtopoint1.set_parameters(self.parameters)
self.movingtopoint2.set_parameters(self.parameters)
def __init__(self, robot_pose, robot_info, options=None):
"""Create controllers and the state transitions"""
K3Supervisor.__init__(self, robot_pose, robot_info)
self.extgoal = False
if options is not None:
try:
self.parameters.goal.x = options.x
self.parameters.goal.y = options.y
self.extgoal = True
except Exception:
pass
# The maximal distance to an obstacle (inexact)
self.distmax = robot_info.ir_sensors.rmax + robot_info.wheels.base_length / 2
# Fill in some parameters
self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
self.parameters.ir_max = robot_info.ir_sensors.rmax
self.parameters.direction = 'left'
self.parameters.distance = self.distmax * 0.85
self.parameters.ga_path = ga.ga_execute(
(0, 0), (self.parameters.goal.x, self.parameters.goal.y))
self.parameters.ga_path.append(
(self.parameters.goal.x, self.parameters.goal.y))
global global_cnt
global_cnt = len(self.parameters.ga_path)
point_cnt = self.parameters.point_cnt
self.process_state_info(robot_info)
#Add controllers
self.gtg = self.create_controller('GoToGoal', self.parameters)
self.avoidobstacles = self.create_controller('K3_AvoidObstacles',
self.parameters)
self.wall = self.create_controller('FollowWall', self.parameters)
self.hold = self.create_controller('Hold', None)
self.path = self.create_controller('FollowPath', self.parameters)
# Define transitions
# self.add_controller(self.hold,
# (lambda: not self.at_goal(), self.gtg))
# self.add_controller(self.gtg,
# (self.at_goal, self.hold),
# (self.at_wall, self.wall))
self.add_controller(self.wall, (self.at_goal, self.hold),
(self.unsafe, self.avoidobstacles),
(self.wall_cleared, self.gtg))
# self.add_controller(self.avoidobstacles,
# (self.at_goal, self.hold),
# (self.safe, self.wall))
self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg))
self.add_controller(self.avoidobstacles, (self.at_goal, self.hold),
(self.free, self.path))
self.add_controller(
self.path,
(lambda: self.next_point(), self.path),
(self.at_goal, self.hold),
#(lambda: self.parameters.point_cnt == len(self.parameters.ga_path) - 1 and not self.next_point(), self.gtg),
(self.at_obstacle, self.avoidobstacles))
self.current = self.path
def set_parameters(self, params):
K3Supervisor.set_parameters(self, params)
self.gtg.set_parameters(self.parameters)
self.avoidobstacles.set_parameters(self.parameters)
def set_parameters(self, params):
"""Set parameters for itself and the controllers"""
K3Supervisor.set_parameters(self, params)
self.movingtopoint1.set_parameters(self.parameters)
self.movingtopoint2.set_parameters(self.parameters)
def set_parameters(self,params):
K3Supervisor.set_parameters(self,params)
self.gtg.set_parameters(self.parameters)
self.avoidobstacles.set_parameters(self.parameters)
def set_parameters(self, params):
"""Set parameters for itself and the controllers"""
K3Supervisor.set_parameters(self, params)
self.blending.set_parameters(self.parameters)
def set_parameters(self,params):
"""Set parameters for itself and the controllers"""
K3Supervisor.set_parameters(self,params)
self.blending.set_parameters(self.parameters)
