Wednesday, August 30, 2017

Orbital Aero Model: Kinematics (Rotation)

In the first Kinematics post, I went into how bodies are moved in space. This post will go into how bodies are rotated in space.


The amount a body will rotate is not based on force and mass, but instead it is based on the moment applied to the body (the Torque, T) and the body's moment of inertia (I).

Torque


https://en.wikipedia.org/wiki/Torque


Torque (moment) is calculated from the equation:


T is the calculated torque applied.
r is the perpendicular distance from the axis of rotation to the force applied.
F is the force applied.

A simple way of thinking about moment is a wrench tightening a bolt.

If the wrench was 1 foot long with 10 pounds of pressure applied to the end of it, then the calculated moment is:
T = 1 * 10 = 10 foot-pounds of torque

If the wrench was twice as long but the same force applied, then the calculated moment is:
T = 2 * 10 = 20 foot-pounds of torque

Which demonstrates the entire purpose of having long handles on wrenches! They allow people to tighten bolts with less force.

Since the entire simulation (and most of the world) uses metric, I'll be using metric for the rest of this post and the code. So our moment units will be Newton-meters rather than foot-pounds.

For a metric example the wrench is 0.25 meters long with 50 newton of force applied, the calculated moment is:
T = 0.25 * 50 = 12.5 Newton-meters of torque

The torque equation I gave above was just for a single axis plane. The equation to cover a vector force in multiple dimensions uses the cross product:


This equation takes a vector force, F, and a vector position, r, to get the resulting torque, T, that is applied in all 3 axes.


In this diagram, a force vector, F, is applied at the corner of a block with the force location represented by vector r. The torque cross product equation gives the resulting torque in all 3 axes.

For more information on the cross product, see https://en.wikipedia.org/wiki/Cross_product The Vector class has a static function for calculating the cross product which the simulation code below will use.

Moment of Inertia


https://en.wikipedia.org/wiki/Moment_of_inertia

All physical objects have a mass, and all physical objects have a moment of inertia. Where mass is an object's resistance to movement, moment of inertia is an object's resistance to rotation.

And where mass is the same from any orientation, the moment of inertia can vary from orientation. There are simple formulas to calculate moment of inertia for a variety of shapes:

Uniform Solid Sphere (moment of inertia same from any orientation):




Block (moment of inertia varies by axis):





https://en.wikipedia.org/wiki/List_of_moments_of_inertia

What happens if you have a more complex shape? (Two spheres at the end of a long block.) Or you're rotating somewhere other than the axis that the moment of inertia equations were referenced from? (Rotating from the end of the block rather than the center.) In those situations, you use the Parallel-Axis Theorem to recalculate a new moment of inertia.

https://en.wikipedia.org/wiki/Parallel_axis_theorem

Currently in the Orbital Aero Model, all bodies are single uniform spheres. I have plans to make it so bodies can consist of one of a variety of basic shapes or a combination of many basic shapes to form complex shapes. So I'll hold off on covering the parallel axis theorem until I complete that code.

Angular Acceleration & Velocity


https://en.wikipedia.org/wiki/Kinematics

In our prior function calls, we calculated the total moment applied to the body. Since we know its moment of inertia, it's easy to calculate acceleration:


T is the torque, in newton-meters.
I is the moment of inertia, in kilogram meters squared.
alpha is the angular acceleration, in radians per second squared.

And with the angular acceleration, we can rotate the object:


theta is the newly calculated orientation, in radians.
theta0 is the initial orientation, in radians.
omega0 is the initial angular velocity, in radians per second.
alpha is the angular acceleration, in radians per second squared.
t is the time step of the simulation, in seconds.


omega is the newly calculated angular velocity, in radians per second.
omega0 is the initial angular velocity, in radians per second.
alpha is the angular acceleration, in radians per second squared.
t is the time step of the simulation, in seconds.

The smaller the time step the more accurate the simulation. However, that also means it's more computational intensive for simulating the same duration of time.



Orbital Aero Code


Now that all the background math has been covered, let's see how it's represented in code.

Body


I added a moment of inertia property to the Body class. Since everything in the orbital aero are considered uniform spheres for now, this returns a moment of inertia based on the body's mass and radius.


class Body
{
public:
    Vector momentOfInertiaTotal_kilogramMeters2() const;
.
.
.
}

Vector Body::momentOfInertiaTotal_kilogramMeters2() const
{
    // For solid spheres: I = 2/5 * M * R^2
    double momentOfInertia_kilogramMeters2 = (double)2.0 / (double)5.0 * this->massTotal_kilograms() * (this->radius_meters * this->radius_meters);
    return Vector(momentOfInertia_kilogramMeters2, momentOfInertia_kilogramMeters2, momentOfInertia_kilogramMeters2);
}

Control System


I'll demonstrate 2 ways of calculating and applying a moment to a body.

Reaction Wheel / Directly Calculate Moment from Joystick Deflection


This approach would be more for satellites with reaction wheels. Rather than expelling force via a rocket to rotate the vehicle, an electric motor attached to a flywheel spins causing the vehicle to counter-rotate. https://en.wikipedia.org/wiki/Reaction_wheel

Of course we could go in depth with the physics of the motor and flywheel, but for simplicity we can just scale the joystick input to a torque. And we can use the body's moment of inertia as a general rule for how much to scale the joystick input.


bool ControlSystemModel::update(OrbitalAeroModel &hostOrbitalAero, Body* hostBody, double timeStep_seconds)
{
  switch (controlInput)
  {
  case ControlInputEnum::idle:
  // No change to any angular velocities set.
  break;
  case ControlInputEnum::joystick:
  {
            // Scale the moment applied by the moment of inertia so our joystick inputs are effective for both
            // very small and very large bodies. This will result in 1 radians/second^2 for full joystick deflection.
            Vector momentOfIneretiaTotal_kilogramMeters2 = hostBody->momentOfInertiaTotal_kilogramMeters2();
            hostBody->momentTotal_newtonMeters.x = hostOrbitalAero.joystickInputs[this->joystickInputsIndex].roll * momentOfIneretiaTotal_kilogramMeters2.x;
            hostBody->momentTotal_newtonMeters.y = -hostOrbitalAero.joystickInputs[this->joystickInputsIndex].pitch * momentOfIneretiaTotal_kilogramMeters2.y;
            hostBody->momentTotal_newtonMeters.z = hostOrbitalAero.joystickInputs[this->joystickInputsIndex].yaw * momentOfIneretiaTotal_kilogramMeters2.z;

            // All thrust is along the x (forward) axis of the entity.
            double thrust_newtons = hostOrbitalAero.joystickInputs[this->joystickInputsIndex].thrust * 1000.0;

            // Convert thrust from local axis to world axis.
            Vector thrustBody_newtons(thrust_newtons, 0, 0);
            hostBody->forceThrust_newtons = thrustBody_newtons.rotatedBy(hostBody->orientation_quaternions.inverse());
      }
         break;
  default:
  {
            this->hostBody->momentTotal_newtonMeters = 0.0;
            this->hostBody->forceThrust_newtons = 0.0;
      }
  break;
  }

  return true;
}

Orientation Thrusters / Calculate Moment from Rockets at Edges of Body


This approach is more inline with the Space Shuttle's reaction control system. https://en.wikipedia.org/wiki/Reaction_control_system A combination of small thrusters on various parts of the spacecraft are fired for attitude adjustments. (They could also make translation adjustments if opposing thrusters are not fired to counteract the force applied.)

The code below models 6 thrusters for orientation control. There is a pair for each axis; one on either side of the body. For any thrust created on one side, a thrust in the opposite direction is created on the other side so that the body only rotates (no translation).

These thrusters pretend they can create force in either direction, which in reality you wouldn't do. (Rocket nozzles point in one direction, not two.) Really this is 12 thrusters with math simplified for 6. That's not many though, the Space Shuttle had 44 of these! (But those were also used for minor translation changes and as backups in case of malfunction.)


bool ControlSystemModel::update(OrbitalAeroModel &hostOrbitalAero, Body* hostBody, double timeStep_seconds)
{
  switch (controlInput)
  {
  case ControlInputEnum::idle:
  // No change to any angular velocities set.
  break;
  case ControlInputEnum::joystick:
  {
             // Orientation Thrusters

            // Roll 1 Thruster - Right Wingtip, pointed up/down.
            Vector roll1ThrustersForce_newtons(0.0, 0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].roll * 10.0);
            Vector roll1ThrustersLocation_meters(0.0, hostBody->radius_meters, 0.0);
            Vector roll1ThrustersMoment_newtonMeters = Vector::crossProduct(roll1ThrustersLocation_meters, roll1ThrustersForce_newtons);

            // Roll 2 Thruster - Left Wingtip, pointed up/down.
            Vector roll2ThrustersForce_newtons(0.0, 0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].roll * -10.0);
            Vector roll2ThrustersLocation_meters(0.0, -hostBody->radius_meters, 0.0);
            Vector roll2ThrustersMoment_newtonMeters = Vector::crossProduct(roll2ThrustersLocation_meters, roll2ThrustersForce_newtons);

            // Pitch 1 Thruster - Nose, pointed up/down.
            Vector pitch1ThrustersForce_newtons(0.0, 0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].pitch * 10.0);
            Vector pitch1ThrustersLocation_meters(-hostBody->radius_meters, 0.0, 0.0);
            Vector pitch1ThrustersMoment_newtonMeters = Vector::crossProduct(pitch1ThrustersLocation_meters, pitch1ThrustersForce_newtons);

            // Pitch 2 Thruster - Tail, pointed up/down.
            Vector pitch2ThrustersForce_newtons(0.0, 0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].pitch * -10.0);
            Vector pitch2ThrustersLocation_meters(hostBody->radius_meters, 0.0, 0.0);
            Vector pitch2ThrustersMoment_newtonMeters = Vector::crossProduct(pitch2ThrustersLocation_meters, pitch2ThrustersForce_newtons);

            // Yaw 1 Thruster, Nose, pointed left/right.
            Vector yaw1ThrustersForce_newtons(0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].yaw * 10.0, 0.0);
            Vector yaw1ThrustersLocation_meters(hostBody->radius_meters, 0.0, 0.0);
            Vector yaw1ThrustersMoment_newtonMeters = Vector::crossProduct(yaw1ThrustersLocation_meters, yaw1ThrustersForce_newtons);

            // Yaw 2 Thruster, Tail, pointed left/right.
            Vector yaw2ThrustersForce_newtons(0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].yaw * -10.0, 0.0);
            Vector yaw2ThrustersLocation_meters(-hostBody->radius_meters, 0.0, 0.0);
            Vector yaw2ThrustersMoment_newtonMeters = Vector::crossProduct(yaw2ThrustersLocation_meters, yaw2ThrustersForce_newtons);

            // Main thruster is at the tail along x (forward) axis of the entity.
            Vector mainThrustersForce_newtons(hostOrbitalAero.joystickInputs[this->joystickInputsIndex].thrust * 1000.0, 0.0, 0.0);
            Vector mainThrustersLocation_meters(-hostBody->radius_meters, 0.0, 0.0);
            Vector mainThrustersMoment_newtonMeters = Vector::crossProduct(mainThrustersLocation_meters, mainThrustersForce_newtons);

            // Get total thrust.
            Vector totalThrustBody_newtons = roll1ThrustersForce_newtons + roll2ThrustersForce_newtons + 
                                             pitch1ThrustersForce_newtons + pitch2ThrustersForce_newtons + 
                                             yaw1ThrustersForce_newtons + yaw2ThrustersForce_newtons + 
                                             mainThrustersForce_newtons;

            // Get total moment.
            hostBody->momentTotal_newtonMeters = roll1ThrustersMoment_newtonMeters + roll2ThrustersMoment_newtonMeters + 
                                                 pitch1ThrustersMoment_newtonMeters + pitch2ThrustersMoment_newtonMeters + 
                                                 yaw1ThrustersMoment_newtonMeters + yaw2ThrustersMoment_newtonMeters + 
                                                 mainThrustersMoment_newtonMeters;

            // Convert thrust from local axis to world axis.
            hostBody->forceThrust_newtons = totalThrustBody_newtons.rotatedBy(hostBody->orientation_quaternions.inverse());

         break;
  default:
  {
            this->hostBody->momentTotal_newtonMeters = 0.0;
            this->hostBody->forceThrust_newtons = 0.0;
      }
  break;
  }


  return true;
}

processKinematics()


Kinematics have been updated to calculate the angular acceleration from the applied moment and moment of inertia.


//  Translate and rotate bodies based on their mass, moments of inertia, and forces.
bool OrbitalAeroModel::processKinematics(double timeStep_seconds)
{
  // Loop through all bodies and reposition them based on the forces.
  map <uint64_t, Body*>::iterator iteratorPrimaryBody = tableBodies.begin();
  while (iteratorPrimaryBody != tableBodies.end())
  {
  Body* primaryBody = iteratorPrimaryBody->second;
  if (primaryBody->isActive && !primaryBody->isExternal)
  {
  if (primaryBody->isStationary)
  {
                // Body Stationary, No Movement
primaryBody->linearAcceleration_metersPerSecond2 = 0.0;
primaryBody->linearVelocity_metersPerSecond = 0.0;
                primaryBody->angularAcceleration_radiansPerSecond2.clear();
                primaryBody->angularVelocity_radiansPerSecond.clear();
  }
  else
  {
                // Translation
                primaryBody->linearAcceleration_metersPerSecond2 = primaryBody->forceTotal_newtons() / primaryBody->massTotal_kilograms();
                // Distance Travelled = (Initial Linear Velocity * Time) + (0.5 * Linear Acceleration * Time^2)
                primaryBody->location_meters += (primaryBody->linearVelocity_metersPerSecond * timeStep_seconds) + (primaryBody->linearAcceleration_metersPerSecond2 * (0.5 * timeStep_seconds * timeStep_seconds));
                // Final Linear Velocity = Initial Linear Velocity + (Linear Acceleration * Time)
primaryBody->linearVelocity_metersPerSecond += primaryBody->linearAcceleration_metersPerSecond2 * timeStep_seconds;
                
                // Rotation
                primaryBody->angularAcceleration_radiansPerSecond2 = primaryBody->momentTotal_newtonMeters / primaryBody->momentOfInertiaTotal_kilogramMeters2();
                // Rotation Amount = (Initial Angular Velocity * Time) + (0.5 * Angular Acceleration * Time^2)
                Euler rotationAmount_radians = (primaryBody->angularVelocity_radiansPerSecond * timeStep_seconds) + (primaryBody->angularAcceleration_radiansPerSecond2 * (0.5 * timeStep_seconds * timeStep_seconds));
                if (rotationAmount_radians.isNonZero())
                {
                    // Rotate around local axis.
                    primaryBody->orientation_quaternions.rotateAboutLocalAxis(rotationAmount_radians);
                }
                // Final Angular Velocity = Initial Angular Velocity + (Angular Acceleration * Time)
                primaryBody->angularVelocity_radiansPerSecond += primaryBody->angularAcceleration_radiansPerSecond2 * timeStep_seconds;
            }
  } // (primaryBody.isActive)
  iteratorPrimaryBody++;
  } // (iteratorPrimaryBody != tableBodies.end())

  return true;
}


Copyright (c) 2017 Clinton Kam
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

No comments:

Post a Comment