Planning.Bezier.flatReference

Parameters, inputs, outputs, defaults and equations from the source file.

function flatReference
    "Recover multirotor attitude, rates, moment, and thrust from flat outputs"
    input Planning.Bezier.MultirotorTrajectory trajectory;
    input Real mass(unit = "kg") "Vehicle mass";
    input Real gravity(unit = "m/s2") "Positive gravitational acceleration";
    input Real inertia[3, 3](each unit = "kg.m2") "Body inertia matrix";
    input Real tolerance(min = 0.0) = 1.0e-6 "Singularity tolerance";
    output Planning.Bezier.FlatReference reference;
  protected
    constant Real worldX[3] = {1.0, 0.0, 0.0};
    constant Real worldY[3] = {0.0, 1.0, 0.0};
    constant Real worldZ[3] = {0.0, 0.0, 1.0};
    Real thrustAxis[3](each unit = "m/s2");
    Real thrustAxisNorm(unit = "m/s2");
    Real thrustAxisNormRate(unit = "m/s3");
    Real thrustAxisNormAcceleration(unit = "m/s4");
    Real bodyZ[3];
    Real bodyZRate[3](each unit = "1/s");
    Real bodyZAcceleration[3](each unit = "1/s2");
    Real headingX[3];
    Real headingXRate[3](each unit = "1/s");
    Real headingXAcceleration[3](each unit = "1/s2");
    Real headingBasis[3];
    Real headingBasisRate[3](each unit = "1/s");
    Real headingBasisAcceleration[3](each unit = "1/s2");
    Real bodyYRaw[3];
    Real bodyYRawRate[3](each unit = "1/s");
    Real bodyYRawAcceleration[3](each unit = "1/s2");
    Real bodyYRawNorm;
    Real bodyYRawNormRate(unit = "1/s");
    Real bodyYRawNormAcceleration(unit = "1/s2");
    Real bodyY[3];
    Real bodyYRate[3](each unit = "1/s");
    Real bodyYAcceleration[3](each unit = "1/s2");
    Real bodyX[3];
    Real bodyXRate[3](each unit = "1/s");
    Real bodyXAcceleration[3](each unit = "1/s2");
    Real rotationRate[3, 3](each unit = "1/s");
    Real rotationAcceleration[3, 3](each unit = "1/s2");
    Real angularVelocitySkew[3, 3](each unit = "1/s");
    Real angularAccelerationSkew[3, 3](each unit = "1/s2");
  algorithm
    assert(mass > 0.0, "Multirotor mass must be positive");
    assert(gravity > 0.0, "Gravity must be positive");
    thrustAxis := gravity * worldZ + trajectory.acceleration;
    thrustAxisNorm := sqrt(thrustAxis * thrustAxis);
    assert(thrustAxisNorm > tolerance,
      "Multirotor flat reference has a zero thrust direction");
    bodyZ := thrustAxis / thrustAxisNorm;
    thrustAxisNormRate := bodyZ * trajectory.jerk;
    bodyZRate := (trajectory.jerk - bodyZ * thrustAxisNormRate)
      / thrustAxisNorm;
    thrustAxisNormAcceleration := bodyZRate * trajectory.jerk
      + bodyZ * trajectory.snap;
    bodyZAcceleration := (trajectory.snap
      - bodyZ * thrustAxisNormAcceleration
      - 2.0 * bodyZRate * thrustAxisNormRate) / thrustAxisNorm;

    headingX := {cos(trajectory.yaw), sin(trajectory.yaw), 0.0};
    headingXRate := trajectory.yawRate
      * {-sin(trajectory.yaw), cos(trajectory.yaw), 0.0};
    headingXAcceleration := trajectory.yawAcceleration
        * {-sin(trajectory.yaw), cos(trajectory.yaw), 0.0}
      - trajectory.yawRate^2
        * {cos(trajectory.yaw), sin(trajectory.yaw), 0.0};
    bodyYRaw := cross(bodyZ, headingX);
    bodyYRawNorm := sqrt(bodyYRaw * bodyYRaw);
    if bodyYRawNorm > tolerance then
      headingBasis := headingX;
      headingBasisRate := headingXRate;
      headingBasisAcceleration := headingXAcceleration;
    else
      headingBasis := if abs(bodyZ[1]) <= abs(bodyZ[2])
          and abs(bodyZ[1]) <= abs(bodyZ[3]) then
          worldX
        else if abs(bodyZ[2]) <= abs(bodyZ[3]) then
          worldY
        else
          worldZ;
      headingBasisRate := zeros(3);
      headingBasisAcceleration := zeros(3);
    end if;
    bodyYRaw := cross(bodyZ, headingBasis);
    bodyYRawRate := cross(bodyZRate, headingBasis)
      + cross(bodyZ, headingBasisRate);
    bodyYRawAcceleration := cross(bodyZAcceleration, headingBasis)
      + 2.0 * cross(bodyZRate, headingBasisRate)
      + cross(bodyZ, headingBasisAcceleration);
    bodyYRawNorm := sqrt(bodyYRaw * bodyYRaw);
    assert(bodyYRawNorm > tolerance,
      "Unable to construct a body frame from thrust and heading");
    bodyY := bodyYRaw / bodyYRawNorm;
    bodyYRawNormRate := bodyY * bodyYRawRate;
    bodyYRate := (bodyYRawRate - bodyY * bodyYRawNormRate)
      / bodyYRawNorm;
    bodyYRawNormAcceleration := bodyYRate * bodyYRawRate
      + bodyY * bodyYRawAcceleration;
    bodyYAcceleration := (bodyYRawAcceleration
      - bodyY * bodyYRawNormAcceleration
      - 2.0 * bodyYRate * bodyYRawNormRate) / bodyYRawNorm;

    bodyX := cross(bodyY, bodyZ);
    bodyXRate := cross(bodyYRate, bodyZ) + cross(bodyY, bodyZRate);
    bodyXAcceleration := cross(bodyYAcceleration, bodyZ)
      + 2.0 * cross(bodyYRate, bodyZRate)
      + cross(bodyY, bodyZAcceleration);
    reference.bodyToWorld[:, 1] := bodyX;
    reference.bodyToWorld[:, 2] := bodyY;
    reference.bodyToWorld[:, 3] := bodyZ;
    rotationRate[:, 1] := bodyXRate;
    rotationRate[:, 2] := bodyYRate;
    rotationRate[:, 3] := bodyZRate;
    rotationAcceleration[:, 1] := bodyXAcceleration;
    rotationAcceleration[:, 2] := bodyYAcceleration;
    rotationAcceleration[:, 3] := bodyZAcceleration;

    angularVelocitySkew := transpose(reference.bodyToWorld) * rotationRate;
    reference.angularVelocityBody := {
      angularVelocitySkew[3, 2],
      angularVelocitySkew[1, 3],
      angularVelocitySkew[2, 1]};
    angularAccelerationSkew := transpose(reference.bodyToWorld)
      * (rotationAcceleration - rotationRate * angularVelocitySkew);
    reference.angularAccelerationBody := {
      angularAccelerationSkew[3, 2],
      angularAccelerationSkew[1, 3],
      angularAccelerationSkew[2, 1]};
    reference.velocityBody := transpose(reference.bodyToWorld)
      * trajectory.velocity;
    reference.quaternion := LieGroups.SO3.Quat.from_DCM(
      reference.bodyToWorld);
    reference.momentBody := inertia * reference.angularAccelerationBody
      + cross(reference.angularVelocityBody,
          inertia * reference.angularVelocityBody);
    reference.thrust := mass * thrustAxisNorm;
    annotation(Documentation(info="<html>
      <p>Uses differential flatness to reconstruct a body frame whose
      z-axis is aligned with required thrust and whose x-axis follows the yaw
      heading. Analytic derivatives provide body rate and angular acceleration;
      rigid-body dynamics then provide moment and collective thrust.</p>
      <p>The thrust direction is singular when gravity plus commanded
      acceleration is zero. Heading is singular when it is parallel to the
      thrust direction; the latter case uses the least-aligned Cartesian axis
      and differentiates the resulting orthonormal frame consistently.</p>
    </html>"));
  end flatReference;