Source code for morse.actuators.waypoint

import logging; logger = logging.getLogger("morse." + __name__)
import math
from morse.core import status, blenderapi, mathutils
import morse.core.actuator
from morse.core.services import service, async_service, interruptible
from morse.helpers.components import add_data, add_property

[docs]class Waypoint(morse.core.actuator.Actuator): """ This actuator reads the coordinates of a destination point, and moves the robot towards the given point, with the robot restricted to moving only forward, turning around its Z axis, and possibly going up and down. This controller is meant to be used mainly by non-holonomic robots. While a robot is moving towards a given waypoint, a property of the **Robot** will be changed in indicate the status of the robot. The ``movement_status`` property will take one of these values: **Stop**, **Transit** or **Arrived**. The movement speed of the robot is internally adjusted to the Blender time measure, following the formula: ``blender_speed = given_speed * tics``, where **tics** is the number of times the code gets executed per second. The default value in Blender is ``tics = 60``. This actuator also implements a simple obstacle avoidance mechanism. The blend file contains the **Motion_Controller** empty, as well as two additional Empty objects: **Radar.L** and **Radar.R**. These detect nearby objects to the left or right of the robot, and will instruct the robot to turn in the opposite direction of the obstacle. If the radar objects are not present, the controller will not have any obstacle avoidance, and the robot can get blocked by any object between it and the target. .. note:: For objects to be detectable by the radars, they must have the following settings in the **Physics Properties** panel: - **Actor** must be checked - **Collision Bounds** must be checked This will work even for Static objects """ _name = "Waypoint" add_property('_obstacle_avoidance', True, 'ObstacleAvoidance', 'bool', "if " "true (default), will activate obstacle avoidance if the radars are" " present") add_property('_free_z', False, 'FreeZ', 'bool', "if false " "(default), the robot is only controlled on 'X' and heading; if " "true, 'Z' is also controlled (for aerial or submarine robots)") add_property('_angle_tolerance', math.radians(10), 'AngleTolerance', 'float', "Tolerance in radian regarding the final heading of the robot") add_property('_speed', 1.0, 'Speed', 'float', "movement speed for the robot, given in m/s") add_property('_target', "", 'Target', 'string', 'name of a blender object in the scene. When specified, this \ object will be placed at the coordinates given to the \ actuator, to indicate the expected destination of the \ robot') add_property('_ignore', [], 'Ignore', 'string', "List of property names. If the object detected by the \ radars has any of these properties defined, it will be \ ignored by the obstacle avoidance, and will not make the \ robot change its trajectory. Useful when trying to move \ close to an object of a certain kind") add_property('_type', 'Velocity', 'ControlType', 'string', "Kind of control, can be one of ['Velocity', 'Position']") add_property('_remain_at_destination', False, 'RemainAtDestination', 'bool', "If true (default: false), the robot actively attempts to \ remain at the waypoint once reached. This is especially \ useful for flying robots that would otherwise typically fall.") add_data('x', 0.0, "float", "X coordinate of the destination, in world frame, in meter") add_data('y', 0.0, "float", "Y coordinate of the destination, in world frame, in meter") add_data('z', 0.0, "float", "Z coordinate of the destination, in world frame, in meter") add_data('tolerance', 0.5, "float", "Tolerance, in meter, to consider the destination as reached.") add_data('speed', 1.0, "float", "If the property 'Speed' is set, use this speed as initial value.") def __init__(self, obj, parent=None): logger.info('%s initialization' % obj.name) # Call the constructor of the parent class morse.core.actuator.Actuator.__init__(self, obj, parent) # Direction of the global vectors self.world_x_vector = mathutils.Vector([1, 0, 0]) self.world_y_vector = mathutils.Vector([0, 1, 0]) self.world_z_vector = mathutils.Vector([0, 0, 1]) self._destination = self.bge_object.position self._projection = self.bge_object.position self._wp_object = None self._collisions = False # Variable to store current speed. Used for the stop/resume services self._previous_speed = 0 self.local_data['x'] = self._destination[0] self.local_data['y'] = self._destination[1] self.local_data['z'] = self._destination[2] # Waypoint tolerance (in meters) self.local_data['tolerance'] = 0.5 self.local_data['speed'] = self._speed # Initially (ie, before receiving a waypoint), # the robot is in 'Arrived' state self.robot_parent.move_status = "Arrived" # Identify an object as the target of the motion try: wp_name = self.bge_object['Target'] if wp_name != '': scene = blenderapi.scene() self._wp_object = scene.objects[wp_name] logger.info("Using object '%s' to indicate motion target" % wp_name) except KeyError: self._wp_object = None # Identify the collision detectors for the sides if self._obstacle_avoidance: for child in self.bge_object.children: if "Radar.R" in child.name: self._radar_r = child if "Radar.L" in child.name: self._radar_l = child try: logger.info("Radar Right is '%s'" % self._radar_r.name) logger.info("Radar Left is '%s'" % self._radar_l.name) self._collisions = True except AttributeError: logger.warning("No radars found attached to the waypoint \ actuator.There will be no obstacle avoidance") # Convert a string listing the properties to avoid into a list # Objects with any of these properties will be ignored # when doing obstacle avoidance try: props = self.bge_object['Ignore'].split(',') self.bge_object['Ignore'] = props except KeyError: self.bge_object['Ignore'] = [] logger.info('Component initialized') @service
[docs] def setdest(self, x, y, z, tolerance=0.5, speed=1.0): """ Set a new waypoint and returns. The robot will try to go to this position, but the service caller has no mean to know when the destination is reached or if it failed. In most cases, the asynchronous service 'goto' should be prefered. :param x: x coordinate of the destination, in world frame, in meter :param y: y coordinate of the destination, in world frame, in meter :param z: z coordinate of the destination, in world frame, in meter :param tolerance: tolerance, in meter, to consider the destination as reached. Optional (default: 0.5 m). :param speed: speed to join the goal. Optional (default 1m/s) :return: Returns always True (if the robot is already moving, the previous target is replaced by the new one) except if the destination is already reached. In this case, returns ``False``. """ distance, _, _ = self.bge_object.getVectTo([x, y, z]) if distance - tolerance <= 0: logger.info("Robot already at destination (distance = {})." " I do not set a new destination.".format(distance)) return False self.local_data['x'] = x self.local_data['y'] = y self.local_data['z'] = z self.local_data['tolerance'] = tolerance self.local_data['speed'] = speed self.robot_parent.move_status = "Transit" return True
@interruptible @async_service
[docs] def goto(self, x, y, z, tolerance=0.5, speed=1.0): """ This method can be used to give a one time instruction to the actuator. When the robot reaches the destination, it will send a reply, indicating that the new status of the robot is "Stop". :param x: x coordinate of the destination, in world frame, in meter :param y: y coordinate of the destination, in world frame, in meter :param z: z coordinate of the destination, in world frame, in meter :param tolerance: tolerance, in meter, to consider the destination as reached. Optional (default: 0.5 m). :param speed: speed to join the goal. Optional (default 1m/s) """ self.local_data['x'] = x self.local_data['y'] = y self.local_data['z'] = z self.local_data['tolerance'] = tolerance self.local_data['speed'] = speed self.robot_parent.move_status = "Transit"
[docs] def interrupt(self): self.local_data['x'] = self.position_3d.x self.local_data['y'] = self.position_3d.y self.local_data['z'] = self.position_3d.z self.local_data['speed'] = 0 morse.core.actuator.Actuator.interrupt(self)
@service #@async_service
[docs] def stop(self): """ This method will instruct the robot to set its speed to 0.0, and reply immediately. If a **goto** request is ongoing, it will remain "pending", as the current destination is not changed. """ #self.local_data['x'] = self.bge_object.worldPosition[0] #self.local_data['y'] = self.bge_object.worldPosition[1] #self.local_data['z'] = self.bge_object.worldPosition[2] #self.local_data['tolerance'] = 0.5 self._previous_speed = self.local_data['speed'] self.local_data['speed'] = 0 # Set the status of the robot self.robot_parent.move_status = "Stop" return self.robot_parent.move_status
@service
[docs] def resume(self): """ Restores the speed to the same value as before the last call to the **stop** service. The robot will continue to the last waypoint specified. """ self.local_data['speed'] = self._previous_speed self._previous_speed = 0 # Set the status of the robot self.robot_parent.move_status = "Transit" return self.robot_parent.move_status
@service
[docs] def get_status(self): """ Ask the actuator to send a message indicating the current movement status of the parent robot. There are three possible states: **Transit**, **Arrived** and **Stop**. """ return self.robot_parent.move_status
[docs] def default_action(self): """ Move the object towards the destination. """ parent = self.robot_parent speed = self.local_data['speed'] v = 0 rz = 0 self._previous_destination = self._destination self._destination = [ self.local_data['x'], self.local_data['y'], self.local_data['z'] ] self._projection = [ self.local_data['x'], self.local_data['y'], self.bge_object.worldPosition[2] ] # Do nothing at all if: # - we were not ask to actively remain at the last wp # - we already are at destination # - no new destination has been received. if not self._remain_at_destination and \ parent.move_status == "Arrived" and \ self._destination == self._previous_destination: return logger.debug("Robot {0} move status: '{1}'".format( parent.bge_object.name, parent.move_status)) # Place the target marker where the robot should go if self._wp_object: self._wp_object.position = self._destination # Vectors returned are already normalized projection_distance, projection_vector, local_vector = \ self.bge_object.getVectTo(self._projection) true_distance, global_vector, local_vector = \ self.bge_object.getVectTo(self._destination) # Convert to the Blender Vector object global_vector = mathutils.Vector(global_vector) projection_vector = mathutils.Vector(projection_vector) # if Z is not free, distance is the projection distance if self._free_z: distance = true_distance else: distance = projection_distance logger.debug("GOT DISTANCE: xy: %.4f ; xyz: %.4f" % (projection_distance, true_distance)) logger.debug("Global vector: %.4f, %.4f, %.4f" % (global_vector[0], global_vector[1], global_vector[2])) logger.debug("Local vector: %.4f, %.4f, %.4f" % (local_vector[0], local_vector[1], local_vector[2])) logger.debug("Projection vector: %.4f, %.4f, %.4f" % (projection_vector[0], projection_vector[1], projection_vector[2])) # If the target has been reached, change the status if distance - self.local_data['tolerance'] <= 0: self.robot_parent.apply_speed(self._type, [0, 0, 0], [0, 0, 0]) parent.move_status = "Arrived" #Do we have a running request? if yes, notify the completion self.completed(status.SUCCESS, parent.move_status) logger.debug("TARGET REACHED") logger.debug("Robot {0} move status: '{1}'".format( parent.bge_object.name, parent.move_status)) else: # Do nothing if the speed is zero if speed == 0: return parent.move_status = "Transit" angle_diff = 0 rotation_direction = 0 # If the projected distance is not null: else computing the # target angle is not possible! if projection_distance - self.local_data['tolerance'] / 2 > 0: ### Get the angle of the robot ### robot_angle = parent.position_3d.yaw ### Get the angle to the target ### target_angle = projection_vector.angle(self.world_x_vector) # Correct the direction of the turn according to the angles dot = projection_vector.dot(self.world_y_vector) logger.debug("Vector dot product = %.2f" % dot) if dot < 0: target_angle *= -1 ### Get the angle that the robot must turn ### if target_angle < robot_angle: angle_diff = robot_angle - target_angle rotation_direction = -1 else: angle_diff = target_angle - robot_angle rotation_direction = 1 # Make a correction when the angles change signs if angle_diff > math.pi: angle_diff = (2 * math.pi) - angle_diff rotation_direction *= -1 logger.debug("Angles: R=%.4f, T=%.4f Diff=%.4f Direction = %d" % (robot_angle, target_angle, angle_diff, rotation_direction)) try: dt = 1 / self.frequency if distance < speed * dt: v = distance / dt else: v = speed if abs(angle_diff) < speed * dt: rotation_speed = angle_diff / dt / 2.0 else: rotation_speed = speed / 2.0 # Compute the speeds if self._type == 'Position': v /= self.frequency rotation_speed /= self.frequency # For the moment ignoring the division by zero # It happens apparently when the simulation starts except ZeroDivisionError: pass # Allow the robot to rotate in place if the waypoing is # to the side or behind the robot if angle_diff >= math.pi/3.0: logger.debug("Turning on the spot!!!") v = 0 # Collision avoidance using the Blender radar sensor if self._collisions and v != 0 and self._radar_r['Rcollision']: # No obstacle avoidance when the waypoint is near if distance + self.local_data['tolerance'] > \ self._radar_r.sensors["Radar"].distance: # Ignore obstacles with the properties specified ignore = False for prop in self.bge_object['Ignore']: if prop in self._radar_r.sensors["Radar"].hitObject: ignore = True logger.debug("Ignoring object '%s' " "with property '%s'" % (self._radar_r.sensors["Radar"].hitObject, prop)) break if not ignore: rz = rotation_speed logger.debug("Obstacle detected to the RIGHT, " "turning LEFT") elif self._collisions and v != 0 and self._radar_l['Lcollision']: # No obstacle avoidance when the waypoint is near if distance + self.local_data['tolerance'] > \ self._radar_l.sensors["Radar"].distance: # Ignore obstacles with the properties specified ignore = False for prop in self.bge_object['Ignore']: if prop in self._radar_l.sensors["Radar"].hitObject: ignore = True logger.debug("Ignoring object '%s' " "with property '%s'" % \ (self._radar_l.sensors["Radar"].hitObject, prop)) break if not ignore: rz = - rotation_speed logger.debug("Obstacle detected to the LEFT, " "turning RIGHT") # Test if the orientation of the robot is within tolerance elif -self._angle_tolerance < angle_diff < self._angle_tolerance: rz = 0 # If not, rotate the robot in the corresponding direction else: rz = rotation_speed * rotation_direction if self._free_z: vx = math.fabs(v * local_vector.dot(self.world_x_vector)) vz = v * local_vector.dot(self.world_z_vector) else: vx = v vz = 0 logger.debug("Applying vx = %.4f, vz = %.4f, rz = %.4f (v = %.4f)" % (vx, vz, rz, v)) self.robot_parent.apply_speed(self._type, [vx, 0, vz], [0, 0, rz])