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 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 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):
"""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 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()
