Source code for morse.actuators.rotorcraft_attitude

import logging; logger = logging.getLogger("morse." + __name__)

import morse.core.actuator
from math import degrees
from morse.core.mathutils import Vector
from morse.helpers.morse_math import normalise_angle
from morse.helpers.components import add_data, add_property
from morse.helpers.coordinates import CoordinateConverter

[docs]class RotorcraftAttitude(morse.core.actuator.Actuator): """ This actuator reads roll,pitch, yaw rate (or yaw, depending the control mode) and thrust commands as e.g. used to manually control a quadrotor via RC or by higher level control loops. This controller is meant to be used by quadrotors and similar flying robots with Rigid Body physics in blender. It is a simple PD-controller which applies torques to the robot to change and control the attitude. In YawRateControl, the yaw-rate input is integrated to yield an absolute yaw setpoint for the controller, otherwise yaw is considered directly as the order. Thrust is directly applied as force in z-direction of the robot. .. note:: Angle are given in aerospace North East Down convention (NED) """ _name = "Rotorcraft attitude motion controller" _short_desc = "Motion controller using force and torque to achieve desired attitude." add_data('roll', 0.0, 'float', "roll angle in radians") add_data('pitch', 0.0, 'float', "pitch angle in radians") add_data('yaw', 0.0, 'float', "If YawRateControl yaw rate in radians/sec," " otherwise yaw angle in radian") add_data('thrust', 0.0, 'float', "collective thrust: 0 .. 1 (= 0 .. 100%)") add_property('_rp_pgain', 100.0, 'RollPitchPgain', 'float', 'proportional gain for roll/pitch control') add_property('_rp_dgain', 20.0, 'RollPitchDgain', 'float', 'derivative gain for roll/pitch control') add_property('_yaw_pgain', 16.0, 'YawPgain', 'float' 'proportional gain for yaw control') add_property('_yaw_dgain', 4.0, 'YawDgain', 'float', 'derivative gain for yaw control') add_property('_thrust_factor', 40.0, 'ThrustFactor', 'float', 'multiplication factor for applied thrust force in N') add_property('_linear_thrust', True, 'LinearThrust', 'bool', 'If set to true, the force thrust is linear w.r.t. the collective ' 'thrust input (Force = ThrustFactor*thrust). ' 'If set to false, the force thrust is quadratic w.r.t. the ' 'collective thrust input (Force = ThrustFactor*thrust^2)') add_property('_yaw_rate_control', True, 'YawRateControl', 'bool', 'If set to true, the robot is controlled in YawRate, otherwise ' 'yaw is considered directly as the order') add_property('_use_angle_against_north', False, 'UseAngleAgainstNorth', 'bool', "If set to true, return the absolute yaw against North. The whole " "geodetic coordinates (longitude, latitude, altitude, angle_against_north)" " must be configured. Otherwise, return the yaw against the Blender " "coordinates") def __init__(self, obj, parent=None):'%s initialization' % # Call the constructor of the parent class morse.core.actuator.Actuator.__init__(self, obj, parent) #logger.setLevel(logging.DEBUG) # Make new reference to the robot velocities (mathutils.Vector) self.robot_w = self.robot_parent.bge_object.localAngularVelocity # get the robot inertia (list [ix, iy, iz]) robot_inertia = self.robot_parent.bge_object.localInertia self.inertia = Vector(tuple(robot_inertia)) logger.debug("robot inertia: (%.3f %.3f %.3f)" % tuple(self.inertia)) # yaw setpoint in radians is just integrated from yaw rate input self.yaw_setpoint = 0.0 self.prev_err = Vector((0.0, 0.0, 0.0)) if self._use_angle_against_north: self._coord_converter = CoordinateConverter.instance()"Component initialized, runs at %.2f Hz ", self.frequency)
[docs] def default_action(self): """ Run attitude controller and apply resulting force and torque to the parent robot. """ # Get the the parent robot robot = self.robot_parent if self.local_data['thrust'] > 0: if self._yaw_rate_control: # yaw_rate and yaw_setpoint in NED self.yaw_setpoint += self.local_data['yaw'] / self.frequency # wrap angle self.yaw_setpoint = normalise_angle(self.yaw_setpoint) logger.debug("yaw setpoint: %.3f", degrees(self.yaw_setpoint)) logger.debug("yaw current: %.3f setpoint: %.3f", -degrees(self.position_3d.yaw), degrees(self.yaw_setpoint)) else: self.yaw_setpoint = normalise_angle(self.local_data['yaw']) # Compute errors # # e = att_sp - att = attitude error # current angles to horizontal plane in NED roll = self.position_3d.roll pitch = -self.position_3d.pitch if self._use_angle_against_north: yaw = - normalise_angle(-self._coord_converter.angle_against_geographic_north(self.position_3d.euler)) else: yaw = -self.position_3d.yaw roll_err = self.local_data['roll'] - roll pitch_err = self.local_data['pitch'] - pitch # wrapped yaw error yaw_err = normalise_angle(self.yaw_setpoint - yaw) err = Vector((roll_err, pitch_err, yaw_err)) logger.debug("attitude error: (% .3f % .3f % .3f)", degrees(err[0]), degrees(err[1]), degrees(err[2])) # derivative we = (err - self.prev_err) * self.frequency logger.debug("yaw rate error: %.3f", we[2]) kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain)) kd = Vector((self._rp_dgain, self._rp_dgain, self._yaw_dgain)) # torque = self.inertia * (kp * err + kd * we) t = [] for i in range(3): t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i])) # convert to blender frame and scale with thrust torque = Vector((t[0], -t[1], -t[2])) * self.local_data['thrust'] logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0], torque[1], torque[2]) if self._linear_thrust: force = Vector((0.0, 0.0, self.local_data['thrust'] * self._thrust_factor)) else: force = Vector((0.0, 0.0, self.local_data['thrust'] * self.local_data['thrust'] \ * self._thrust_factor)) logger.debug("applied thrust force: %.3f", force[2]) self.prev_err = err.copy() else: force = Vector((0.0, 0.0, 0.0)) torque = Vector((0.0, 0.0, 0.0)) # directly apply local forces and torques to the blender object of the parent robot robot.bge_object.applyForce(force, True) robot.bge_object.applyTorque(torque, True)