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
[docs]class RotorcraftAttitude(morse.core.actuator.Actuator):
"""
This actuator reads roll,pitch, yaw rate 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. The yaw-rate input is integrated to yield
an absolute yaw setpoint for the controller. 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', "yaw angle in radians")
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')
def __init__(self, obj, parent=None):
logger.info('%s initialization' % obj.name)
# Call the constructor of the parent class
super(self.__class__, self).__init__(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))
logger.info("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:
# 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))
# 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
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_pgain, 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])
force = Vector((0.0, 0.0, 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)