Source code for morse.actuators.stabilized_quadrotor

import logging; logger = logging.getLogger("morse." + __name__)
from morse.core.services import service
import morse.core.actuator
from morse.core import mathutils
from morse.helpers.filt2 import Filt2
from morse.helpers.components import add_data

[docs]class StabilizedQuadrotor(morse.core.actuator.Actuator): """ This actuator controls a stabilized quadrotor using a linear model. Basically, it reads a command (phi, theta, psi, h), and computes, using a second order filter the speed and the position of the robot. The quadrotor must not have Rigid Body physics. .. note:: Coordinates are given with respect to the origin of Blender's coordinate axis. .. note:: The actuator does not consider friction force. Setting theta_c or phi_c to 0 leads to a constant speed on axis x or y. """ _name = "Stabilized Flight for quadrotor" _short_desc = "Motion controller using linear model of a stabilized \ quadrotor" add_data('theta_c', 0.0, "float", 'Commands the pitch of the quadrotor. It is directly \ related to the quadrotor acceleration on the x axis.') add_data('phi_c', 0.0, "float", 'Commands the roll of the quadrotor. It is directly \ related to the quadrotor acceleration on the y axis.') add_data('psi_c', 0.0, "float", 'Commands the yaw of the quadrotor.') add_data('h_c', 0.0, "float", 'Commands the z of the quadrotor.') 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) # Env Variables self.v = [0.0, 0.0, 0.0] self.acc = [0.0, 0.0, 0.0] self.f_phi = Filt2(10.0, 0.7) self.f_theta = Filt2(10.0, 0.7) self.f_psi = Filt2(10.0, 0.7) self.f_h = Filt2(3.0, 0.5) logger.info('Component initialized') @service
[docs] def set_cons(self, phi, theta, psi, h): """ Specify a consign for the robot. It has the same effect that writing the corresponding constraint in the datastream. :param phi: commands the roll of the quadrotor :param theta: command the pitch of the quadrotor :param psi: command the yaw of the quadrotor :param h: the expected height for the quadrotor. """ self.local_data['phi_c'] = phi self.local_data['theta_c'] = theta self.local_data['psi_c'] = self.local_data['psi_c'] + psi self.local_data['h_c'] = h
@service
[docs] def stop(self): """ Stop the robot. It basically means that speed on the different axis is set to 0. Moreover, the different filters are reset. """ self.v = [0.0, 0.0, 0.0] self.f_phi.init() self.f_theta.init() self.f_psi.init()
[docs] def default_action(self): """ Apply ... to the parent robot. """ # Ticks dt = 1.0 / self.frequency # Compute height and Euler Angles self.f_phi.simulate(self.local_data['phi_c'], dt) self.f_theta.simulate(self.local_data['theta_c'], dt) self.f_psi.simulate(self.f_psi.x[0]-self.local_data['psi_c'], dt) self.f_h.simulate(self.local_data['h_c'], dt) rot = mathutils.Euler([self.f_phi.x[0], self.f_theta.x[0], self.f_psi.x[0]]) # Get the parent parent = self.robot_parent.bge_object # Compute acceleration # get previous height and vert. velocity main_to_origin = self.robot_parent.position_3d main_to_origin.update(parent) h = main_to_origin.z # assume velocity acc_b = mathutils.Vector([0.0, 0.0, 10.0 + 9.0 * (self.local_data['h_c'] - h) - 2.0 * 0.7 * 3.0 * self.v[2]]) acc_i = mathutils.Vector( rot.to_matrix()*acc_b ) self.v[0] += dt * acc_i[0] self.v[1] += dt * acc_i[1] self.v[2] += dt * (acc_i[2] - 10.0) #Change the parent position prev_pos = parent.localPosition parent.localPosition = mathutils.Vector(prev_pos + dt * mathutils.Vector([self.v[0],self.v[1],self.v[2]])) #Change the parent orientation parent.orientation = rot.to_matrix()