Source code for morse.actuators.keyboard

import logging; logger = logging.getLogger("morse." + __name__)
from morse.core import blenderapi
from morse.core.actuator import Actuator
from morse.helpers.components import add_data, add_property

[docs]class Keyboard(Actuator): """ This actuator does not require a connection with external data. It simply responds to the keyboard arrows to generate movement instructions for the robot attached. When parented to a robot, the user can press the arrow keys to modify the linear and angular velocities (V, W) of the robot. :kbd:`Up` forward :kbd:`Down` backwards :kbd:`Left` turn left :kbd:`Right` turn right """ _name = "Keyboard Actuator" _short_desc="A 'fake' actuator that allows to move a robot from the keyboard." add_property('_type', 'Position', 'ControlType', 'string', "Kind of control to move the parent robot, in ['Position', " "'Velocity', 'Differential']") add_property('_speed', 1.0, 'Speed', 'float', "Movement speed of the parent robot, in m/s") def __init__(self, obj, parent=None): logger.info('%s initialization' % obj.name) # Call the constructor of the parent class Actuator.__init__(self, obj, parent) # Correct the speed considering the Blender clock if self._type == 'Position': self._speed = self._speed / self.frequency elif self._type == 'Differential': self._stopped = True # get track width for calculating wheel speeds from yaw rate parent = self.robot_parent self._trackWidth = parent._trackWidth self._radius = parent._wheel_radius logger.info('Component initialized')
[docs] def default_action(self): """ Interpret keyboard presses and assign them to movement for the robot.""" keys_sensor = blenderapi.controller().sensors[0] #pressed_keys = keys_sensor.getPressedKeys() pressed_keys = keys_sensor.events # Reset movement variables vx, vy, vz = 0.0, 0.0, 0.0 rx, ry, rz = 0.0, 0.0, 0.0 for key, status in pressed_keys: logger.debug("GOT: {0}, STATUS {1}".format(key, status)) if key == blenderapi.UPARROWKEY: vx = self._speed if key == blenderapi.DOWNARROWKEY: vx = -self._speed if key == blenderapi.LEFTARROWKEY: rz = self._speed if key == blenderapi.RIGHTARROWKEY: rz = -self._speed # Give the movement instructions directly to the parent # The second parameter specifies a "local" movement if self._type == 'Position' or self._type == 'Velocity': self.apply_speed(self._type, [vx, vy, vz], [rx, ry, rz / 2.0]) elif self._type == 'Differential': self.apply_vw_wheels(vx, rz) # from v_omega_diff_drive
[docs] def apply_vw_wheels(self, vx, vw): """ Apply (v, w) to the parent robot. """ # calculate desired wheel speeds and set them if abs(vx) < 0.001 and abs(vw) < 0.001: # stop the wheel when velocity is below a given threshold for index in self.robot_parent._wheels.keys(): self.robot_parent._wheel_joints[index].setParam(9, 0, 100.0) self._stopped = True else: # this is need to "wake up" the physic objects if they have # gone to sleep apply a tiny impulse straight down on the # object if (self._stopped): self.robot_parent.bge_object.applyImpulse( self.robot_parent.bge_object.position, (0.0, 0.1, -0.000001)) # no longer stopped self._stopped = False # Another formula for computing left and right wheel speeds: # http://arri.uta.edu/acs/jmireles/Robotics/KinematicsMobileRobots.pdf v_ws_l = vx - (self._trackWidth / 2.0) * vw v_ws_r = vx + (self._trackWidth / 2.0) * vw # convert to angular speeds w_ws_l = v_ws_l / self._radius w_ws_r = v_ws_r / self._radius # set wheel speeds - front and rear wheels have the same speed # Left side wheels self.robot_parent._wheel_joints['FL'].setParam(9, w_ws_l, 100.0) if 'RL' in self.robot_parent._wheels: self.robot_parent._wheel_joints['RL'].setParam(9, w_ws_l, 100.0) # Right side wheels self.robot_parent._wheel_joints['FR'].setParam(9, w_ws_r, 100.0) if 'RR' in self.robot_parent._wheels: self.robot_parent._wheel_joints['RR'].setParam(9, w_ws_r, 100.0) logger.debug("New speeds set: left=%.4f, right=%.4f" % (w_ws_l, w_ws_r))