Source code for morse.actuators.rotorcraft_velocity

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

from morse.core import blenderapi
from morse.core.mathutils import Vector, Matrix
from math import radians, degrees, sin, cos, fabs, copysign, sqrt
from morse.helpers.morse_math import normalise_angle
from morse.helpers.components import add_data, add_property

import morse.core.actuator
from morse.core.services import service
from morse.core import status


[docs]class RotorcraftVelocity(morse.core.actuator.Actuator): """ This controller will receive a velocity and yaw rate command and make the robot move by changing attitude. This controller is meant for rotorcrafts like quadrotors. """ _name = "Rotorcraft Velocity motion controller" _short_desc = "Motion controller using force and torque to move a rotorcraft with a specified velocity." add_data('vx', 0.0, 'float', "desired x velocity in m/s") add_data('vy', 0.0, 'float', "desired y velocity in m/s") add_data('vz', 0.0, 'float', "desired z velocity in m/s") add_data('vyaw', 0.0, 'float', "desired yaw rate radians/s") add_data('tolerance', 0.2, 'float', "velocity tolerance in m/s") add_property('_h_pgain', 0.2, 'HorizontalPgain', 'float', 'proportional gain for the outer horizontal velocity [xy] loop') add_property('_h_dgain', 0.8, 'HorizontalDgain', 'float', 'derivative gain for the outer horizontal velocity[xy] loop') add_property('_v_pgain', 8, 'VerticalPgain', 'float', 'proportional gain for the vertical velocity loop') add_property('_v_dgain', 2, 'VerticalDgain', 'float', 'derivative gain for the vertical velocity loop') add_property('_yaw_pgain', 12.0, 'YawPgain', 'float', 'proportional gain for yaw control of the inner loop') add_property('_yaw_dgain', 8.0, 'YawDgain', 'float', 'derivative gain for yaw control of the inner loop') add_property('_rp_pgain', 9.7, 'RollPitchPgain', 'float', 'proportional gain for roll/pitch control of the inner loop') add_property('_rp_dgain', 2, 'RollPitchDgain', 'float', 'derivative gain for roll/pitch control of the inner loop') add_property('_max_bank_angle', radians(30), 'MaxBankAngle', 'float', 'limit the maximum roll/pitch angle of the robot. This \ effectively limits the horizontal acceleration of the robot') add_property('_max_velocity', 2, 'MaxVelocity', 'float', 'limit the maximum velocity of the robot.') 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.INFO) robot_obj = self.robot_parent.bge_object # set desired velocity to zero self.local_data['vx'] = 0.0 self.local_data['vy'] = 0.0 self.local_data['vz'] = 0.0 self.local_data['vyaw'] = 0.0 # get the robot inertia (list [ix, iy, iz]) robot_inertia = robot_obj.localInertia self.inertia = Vector(tuple(robot_inertia)) logger.info("robot inertia: (%.3f %.3f %.3f)" % tuple(self.inertia)) self.nominal_thrust = robot_obj.mass * abs(blenderapi.gravity()[2]) logger.info("nominal thrust: %.3f", self.nominal_thrust) self._attitude_compensation_limit = cos(self._max_bank_angle) ** 2 # current attitude setpoints in radians self.roll_setpoint = 0.0 self.pitch_setpoint = 0.0 self.yaw_setpoint = 0.0 self.thrust = 0.0 self._prev_vel_blender = Vector((0.0, 0.0, 0.0)) self._prev_yaw = 0.0 # previous attitude error self._prev_err = Vector((0.0, 0.0, 0.0)) logger.info("Component initialized, runs at %.2f Hz ", self.frequency) @service
[docs] def setvel(self, vx, vy, vz, vyaw, tolerance=0.1): """ Set a new desired velocity and return. The robot will try to go achieve the desired velocity, but the service caller has no mean to know when the desired velocity is reached or if it failed. :param vx: x coordinate of the velocity (meter/sec) :param vy: y coordinate of the velocity (meter/sec) :param vz: z coordinate of the velocity (meter/sec) :param vyaw: yaw rate (radian/sec) :param tolerance: speed difference considered to decide whether the desired velocity has been reached (meter/sec) :return: True, except if the desired velocity is already reached. In this case, returns False. """ vel = sqrt(sum(v * v for v in self._lin_vel_sp)) if vel - tolerance <= 0: logger.info("Robot already at target velocity ({})." " I do not set a new velocity.".format(vel)) return False self.local_data['vx'] = vx self.local_data['vy'] = vy self.local_data['vz'] = vz self.local_data['vyaw'] = vyaw self.local_data['tolerance'] = tolerance return True
[docs] def default_action(self): """ Move the robot. """ robot_obj = self.robot_parent.bge_object vel_body_sp = Vector((self.local_data['vx'], self.local_data['vy'], self.local_data['vz'])) # TODO: limit max_velocity setpoint # current angles to horizontal plane in NED # (not quite, but approx good enough) roll = self.position_3d.roll pitch = self.position_3d.pitch yaw = self.position_3d.yaw # convert the commands in body frame to blender frame # in which frame do we want to command?? #body2blender = Matrix.Rotation(math.pi, 3, 'X') * Matrix.Rotation(yaw, 3, 'Z') body2blender = Matrix.Rotation(yaw, 3, 'Z') vel_blender_sp = body2blender * vel_body_sp # current velocity of robot in Blender world frame vel_blender = robot_obj.worldLinearVelocity # errors in blender frame vel_error = vel_blender_sp - vel_blender # zero desired acceleration for now... no reference... accel_error = -(vel_blender - self._prev_vel_blender) * self.frequency logger.debug("velocity in blender frame: (% .3f % .3f % .3f)", vel_blender[0], vel_blender[1], vel_blender[2]) logger.debug("velocity setpoint:: (% .3f % .3f % .3f)", vel_blender_sp[0], vel_blender_sp[1], vel_blender_sp[2]) logger.debug("velocity error: (% .3f % .3f % .3f)", vel_error[0], vel_error[1], vel_error[2]) logger.debug("acceleration error: (% .3f % .3f % .3f)", accel_error[0], accel_error[1], accel_error[2]) cmd_blender_x = self._h_pgain * vel_error[0] + self._h_dgain * accel_error[0] cmd_blender_y = self._h_pgain * vel_error[1] + self._h_dgain * accel_error[1] self.roll_setpoint = sin(yaw) * cmd_blender_x - cos(yaw) * cmd_blender_y self.pitch_setpoint = cos(yaw) * cmd_blender_x + sin(yaw) * cmd_blender_y self.yaw_setpoint = self._prev_yaw - (self.local_data['vyaw'] / self.frequency) # saturate max roll/pitch angles if fabs(self.roll_setpoint) > self._max_bank_angle: self.roll_setpoint = copysign(self._max_bank_angle, self.roll_setpoint) if fabs(self.pitch_setpoint) > self._max_bank_angle: self.pitch_setpoint = copysign(self._max_bank_angle, self.pitch_setpoint) # wrap yaw setpoint self.yaw_setpoint = normalise_angle(self.yaw_setpoint) logger.debug("roll current: % 2.3f setpoint: % 2.3f", degrees(roll), degrees(self.roll_setpoint)) logger.debug("pitch current: % 2.3f setpoint: % 2.3f", degrees(pitch), degrees(self.pitch_setpoint)) logger.debug("yaw current: % 2.3f setpoint: % 2.3f", degrees(yaw), degrees(self.yaw_setpoint)) # compute thrust # nominal command to keep altitude (feed forward) thrust_attitude_compensation = max(self._attitude_compensation_limit, cos(roll) * cos(pitch)) thrust_ff = self.nominal_thrust / thrust_attitude_compensation # feedback to correct vertical speed thrust_fb = self._v_pgain * vel_error[2]# + self._v_dgain * accel_error[2] self.thrust = max(0, thrust_ff + thrust_fb) # Compute attitude errors # # e = att_sp - att = attitude error roll_err = normalise_angle(self.roll_setpoint - roll) pitch_err = normalise_angle(self.pitch_setpoint - pitch) yaw_err = normalise_angle(self.yaw_setpoint - yaw) err = Vector((roll_err, pitch_err, yaw_err)) # derivative we = (err - self._prev_err) * self.frequency # we = Vector((0.0, 0.0, 0.0)) # 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.thrust / self.nominal_thrust logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0], torque[1], torque[2]) force = Vector((0.0, 0.0, self.thrust)) logger.debug("applied thrust force: %.3f", force[2]) self._prev_err = err.copy() self._prev_vel_blender = vel_blender.copy() self._prev_yaw = yaw # directly apply local forces and torques to the blender object of the parent robot robot_obj.applyForce(force, True) robot_obj.applyTorque(torque, True)