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