Autonomous Robots Lab
  • Home
  • News
  • Research
    • Autonomous Navigation and Exploration
    • Fixed-Wing UAVs
    • Agile and Physical Interaction Control
    • Localization and 3D Reconstruction
    • Subterranean Robotics
    • Collision-tolerant Aerial Robots
    • Marine Robotics
    • Intelligent Mobility >
      • Student Projects
      • Electric Bus Datasets
    • Robotics for Nuclear Sites
    • Degerminator
    • Autonomous Robots Arena
    • Code
    • Media
    • Research Presentations
    • Projects
  • Publications
  • Group
    • People
    • Research Collaborators
    • Positions
  • Education
    • Introduction to Aerial Robotics >
      • Online Textbook >
        • Modeling >
          • Frame Rotations and Representations
          • Multirotor Dynamics
        • State Estimation >
          • Inertial Sensors
          • Batch Discrete-Time Estimation
          • The Kalman Filter
        • Flight Control >
          • PID Control
          • LQR Control
          • Linear Model Predictive Control
        • Motion Planning >
          • Holonomic Vehicle BVS
          • Dubins Airplane
          • Collision-free Navigation
          • Structural Inspection Path Planning
        • Simulation Tools >
          • Simulations with SimPy
          • MATLAB & Simulink
          • RotorS Simulator >
            • RotorS Simulator Video Examples
      • Lecture Slides
      • Literature and Links
      • RotorS Simulator
      • Student Projects
      • Homework Assignments
      • Independent Study
      • Video Explanations
      • Syllabus
      • Grade Statistics
    • Autonomous Mobile Robot Design >
      • Lecture Slides
      • Semester Projects
      • Code Repository
      • Literature and Links
      • RotorS Simulator
      • Video Explanations
      • Resources for Semester Projects
      • Syllabus
    • Robotics for DDD Applications
    • CS302 - Data Structures
    • Student Projects >
      • Robot Competitions
      • Undergraduate Researchers Needed
      • ConstructionBots - Student Projects
    • EiT TTK4854 - Robotic Ocean Waste Removal
    • Aerial Robotic Autonomy >
      • Breadth Topics
      • Deep-dive Topics
      • Literature
    • Robotics Seminars
    • Robotics Days
    • Outreach >
      • Drones Demystified! >
        • Lecture Slides
        • Code Repository
        • Video Explanations
        • RotorS Simulator
        • Online Textbook
      • Autonomous Robots Camp >
        • RotorS Simulator
      • Outreach Student Projects
    • BadgerWorks >
      • General Study Links
      • Learn ROS
      • SubT-Edu
  • Resources
    • Autonomous Robots Arena
    • Robot Development Space
  • Contact

Vehicle Dynamics Simulation using SimPy

A Quick Introduction to SimPy for ODE Simulations

Below, the basic concepts of SimPy - and how they apply to the problem of simulating Ordinary Differential Equations -will be overviewed. 

1. Installation
Execute the following steps:
  • Download SimPy
  • Extract the archive, open a terminal there and type:
$ python setup.py install
You can now optionally the basic verification steps that SimPy's offers:
$ python -c "import simpy; simpy.test()"
Picture
2. Characterizing a system using Ordinary Differential Equations
A dynamic system, say a mass-spring system, can be characterized by the relation between its state variables and its inputs. This relation is expressed in the form of differential equations (and in that case "Ordinary Differential Equations"). To derive the ODE representation, we rely on laws of physics with the level of accuracy and fidelity we consider appropriate for the specific task at hand. 

The specifc system (shown in Figure 1) can be captured by the following secord order Ordinary Differential Equation (ODE):
Picture
Figure 1: Sprin-mass system. 
Picture
Simulating such a system with the help of SimPy is particularly simple as shown in the code section below. 
from scipy.integrate import odeint
from numpy import *
import matplotlib.pyplot as plt

def MassSpring(state,t):
  x = state[0] # position
  xd = state[1] # velocity

  # variables
  k = -5 # Nm
  m = 2 # kg
  g = 9.81 # m/s^2

  # mass acceleration
  xdd = ((k*x)/m) + g

  return [xd, xdd]

state0 = [0.0, 0.0]
t = arange(0.0, 20.0, 0.1)

state = odeint(MassSpring, state0, t)

plt.plot(t, state)
plt.xlabel('Time (s)')
plt.ylabel('States')
plt.title('Mass-Spring System')
plt.legend(('$x$ (m)', '$\dot{x}$ (m/sec)'))
plt.show()
raw_input()
Picture
The aforementioned system is particularly simple as it is expressed using linear ODEs. A slightly more difficult case arises, when one has to deal with nonlinear systems. Take for example the case of the Lorenz Attractor, a well known nonlinear system actively used to illustrate concepts of the chaos theory. Its state equations are: 
Picture
The parameters σ,ρ,β are dominating the response of the system and if ones sets them as:
Picture
Then the system exhibits chaotic behavior as shown in the simulation case study below. 
from scipy.integrate import odeint
from numpy import *
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def Lorenz(state,t):
  x = state[0]
  y = state[1]
  z = state[2]

  # set the constants
  sigma = 10.0
  rho = 28.0
  beta = 8.0/3.0

  xd = sigma * (y-x)
  yd = (rho-z)*x - y
  zd = x*y - beta*z

  return [xd, yd, zd]

state0 = [2.0, 3.0, 4.0]
t = arange(0.0, 30.0, 0.01)

state = odeint(Lorenz, state0, t)

fig = plt.figure()
ax = fig.gca(projection='3D')
ax.plot(state[:,0],state[:,1],state[:,2])
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
fig.show()
raw_input()
Picture
Figure 3: Lorenz Attractor expression chaotic behavior. 

Simulation of a Quadrotor Aerial Robot

The classes/methods below implement the simulation of the vehicle dynamics of a quadrotor aerial robot. The system is considered to be captured using rigid body modeling and the four thrust vectors attached to it, while its self-response is only characterized by its mass and the cross-inertia terms. Within that framework, scipy is employed for the forward integration purposes of the state update, while direct calculations and linear algebra are employed for the moment/forces calculations as well as the coordinate system transformations.
Picture
File: main.py
from quadrotor_sims_params import *

if __name__ == "__main__":
    fly_quadrotor()
File: quadrotor_sims_params.py
#       __QUADROTORSIMSPARAMS__
#       This file implements the sim parameters 
#       simple quadrotor simulation tool
#
#       Largely based on the work of https://github.com/nikhilkalige

from quadrotor_dynamics import QuadrotorDynamics
import numpy as np
import random
import time
import matplotlib.pyplot as plt

TURNS = 3

class SimulationParams(object):
    def __init__(self):
        self.mass = 1
        self.Ixx = 0.0053
        self.length = 0.2
        self.Bup = 21.58
        self.Bdown = 3.92
        self.Cpmax = np.pi * 1800/180
        self.Cn = TURNS
        self.gravity = 9.81

    def get_acceleration(self, p0, p3):
        ap = {
            'acc': (-self.mass * self.length * (self.Bup - p0) / (4 * self.Ixx)),
            'start': (self.mass * self.length * (self.Bup - self.Bdown) / (4 * self.Ixx)),
            'coast': 0,
            'stop': (-self.mass * self.length * (self.Bup - self.Bdown) / (4 * self.Ixx)),
            'recover': (self.mass * self.length * (self.Bup - p3) / (4 * self.Ixx)),
        }
        return ap

    def get_initial_parameters(self):
        p0 = p3 = 0.9 * self.Bup
        p1 = p4 = 0.1
        acc_start = self.get_acceleration(p0, p3)['start']
        p2 = (2 * np.pi * self.Cn / self.Cpmax) - (self.Cpmax / acc_start)
        return [p0, p1, p2, p3, p4]

    def get_sections(self, parameters):
        sections = np.zeros(5, dtype='object')
        [p0, p1, p2, p3, p4] = parameters

        ap = self.get_acceleration(p0, p3)

        T2 = (self.Cpmax - p1 * ap['acc']) / ap['start']
        T4 = -(self.Cpmax + p4 * ap['recover']) / ap['stop']

        aq = 0
        ar = 0

        sections[0] = (self.mass * p0, [ap['acc'], aq, ar], p1)

        temp = self.mass * self.Bup - 2 * abs(ap['start']) * self.Ixx / self.length
        sections[1] = (temp, [ap['start'], aq, ar], T2)

        sections[2] = (self.mass * self.Bdown, [ap['coast'], aq, ar], p2)

        temp = self.mass * self.Bup - 2 * abs(ap['stop']) * self.Ixx / self.length
        sections[3] = (temp, [ap['stop'], aq, ar], T4)

        sections[4] = (self.mass * p3, [ap['recover'], aq, ar], p4)
        return sections
    
def fly_quadrotor(params=None):
    gen = SimulationParams()
    quadrotor = QuadrotorDynamics()
    if not params:
        params = gen.get_initial_parameters()
    sections = gen.get_sections(params)
    state = quadrotor.update_state(sections)
    plt.plot(state)
    plt.show()
    raw_input()
File: quadrotor_dynamics.py
#       __QUADROTORDYNAMICS__
#       This file implements the dynamics of a
#       simple quadrotor micro aerial vehicle
#
#       Largely based on the work of https://github.com/nikhilkalige

import numpy as np
from scipy.integrate import odeint

class QuadrotorDynamics(object):

    def __init__(self, save_state=True, config={}):
        """
        Quadrotor Dynamics Parameters
        ----------
        save_state: Boolean
            Decides whether the state of the system should be saved
            and returned at the end
        """
        self.config = {
            # Define constants 
            'gravity': 9.81,  # Earth gravity [m s^-2]
            # Define Vehicle Parameters
            'mass': 1,  # Mass [kg]
            'length': 0.2,  # Arm length [m]
            # Cross-Intertia [Ixx, Iyy, Izz]
            'inertia': np.array([0.0053, 0.0053, 0.0086]),  # [kg m^2]
            'thrustToDrag': 0.018  # thrust to drag constant [m]
        }

        self.save_state = save_state
        self._dt = 0.005  # Simulation Step
        self.config.update(config)
        self.initialize_state()

    def initialize_state(self):
        self.state = {
            # Position [x, y, z] of the quadrotor in inertial frame
            'position': np.zeros(3),
            # Velocity [dx/dt, dy/dt, dz/dt] of the quadrotor in inertial frame
            'velocity': np.zeros(3),
            # Euler angles [phi, theta, psi]
            'orientation': np.zeros(3),
            # Angular velocity [p, q, r]
            'ang_velocity': np.zeros(3)
        }

    def motor_thrust(self, moments, total_thrust):
        """Compute Motor Thrusts

        Parameters
        ----------
        moments : numpy.array
            [Mp, Mq, Mr] moments
        total_thrust : float
            The total thrust generated by all motors
            
        Returns
        -------
        numpy.array
            The thrust generated by each motor [T1, T2, T3, T4]
        """
        [Mp, Mq, Mr] = moments
        thrust = np.zeros(4)
        tmp1add = total_thrust + Mr / self.config['thrustToDrag']
        tmp1sub = total_thrust - Mr / self.config['thrustToDrag']

        tmp2p = 2 * Mp / self.config['length']
        tmp2q = 2 * Mq / self.config['length']

        thrust[0] = tmp1add - tmp2q
        thrust[1] = tmp1sub + tmp2p
        thrust[2] = tmp1add + tmp2q
        thrust[3] = tmp1sub - tmp2p

        return thrust / 4.0

    def dt_eulerangles_to_angular_velocity(self, dtEuler, EulerAngles):
        """Euler angles derivatives TO angular velocities
        dtEuler = np.array([dphi/dt, dtheta/dt, dpsi/dt])
        """
        return np.dot(self.angular_rotation_matrix(EulerAngles), dtEuler)

    def acceleration(self, thrusts, EulerAngles):
        """Compute the acceleration in inertial reference frame
        thrust = np.array([Motor1, .... Motor4])
        """
        force_z_body = np.sum(thrusts) / self.config['mass']
        rotation_matrix = self.rotation_matrix(EulerAngles)
        # print rotation_matrix
        force_body = np.array([0, 0, force_z_body])
        return np.dot(rotation_matrix, force_body) - np.array([0, 0, self.config['gravity']])

    def angular_acceleration(self, omega, thrust):
        """Compute the angular acceleration in body frame
        omega = angular velocity :- np.array([p, q, r])
        """
        [t1, t2, t3, t4] = thrust
        thrust_matrix = np.array([self.config['length'] * (t2 - t4),
                                  self.config['length'] * (t3 - t1),
                                  self.config['thrustToDrag'] * (t1 - t2 + t3 - t4)])

        inverse_inertia = np.linalg.inv(self.inertia_matrix)
        p1 = np.dot(inverse_inertia, thrust_matrix)
        p2 = np.dot(inverse_inertia, omega)
        p3 = np.dot(self.inertia_matrix, omega)
        cross = np.cross(p2, p3)
        return p1 - cross

    def angular_velocity_to_dt_eulerangles(self, omega, EulerAngles):
        """Angular Velocity TO Euler Angles
        omega = angular velocity :- np.array([p, q, r])
        """
        rotation_matrix = np.linalg.inv(self.angular_rotation_matrix(EulerAngles))
        return np.dot(rotation_matrix, omega)

    def moments(self, ref_acc, angular_vel):
        """Compute the moments

        Parameters
        ----------
        ref_acc : numpy.array
            The desired angular acceleration that the system should achieve. This
            should be of form [dp/dt, dq/dt, dr/dt]
        angular_vel : numpy.array
            The current angular velocity of the system. This
            should be of form [p, q, r]

        Returns
        -------
        numpy.array
            The desired moments of the system
        """
        inverse_inertia = np.linalg.inv(self.inertia_matrix)
        p1 = np.dot(inverse_inertia, angular_vel)
        p2 = np.dot(self.inertia_matrix, angular_vel)
        cross = np.cross(p1, p2)
        value = ref_acc + cross
        return np.dot(self.inertia_matrix, value)

    def angular_rotation_matrix(self, EulerAngles):
        """Rotation matix for Angular Velocity <-> Euler Angles Conversion
        Use inverse of the matrix to convert from angular velocity to euler rates
        """
        [phi, theta, psi] = EulerAngles
        cphi = np.cos(phi)
        sphi = np.sin(phi)
        cthe = np.cos(theta)
        sthe = np.sin(theta)
        cpsi = np.cos(psi)
        spsi = np.sin(psi)
        RotMatAngV = np.array([ [1, 0,            -sthe],
                                [0, cphi,  cthe * sphi],
                                [0, -spsi, cthe * cphi]
                                ])
        return RotMatAngV

    def rotation_matrix(self, EulerAngles):
        [phi, theta, psi] = EulerAngles
        cphi = np.cos(phi)
        sphi = np.sin(phi)
        cthe = np.cos(theta)
        sthe = np.sin(theta)
        cpsi = np.cos(psi)
        spsi = np.sin(psi)

        RotMat = np.array([[cthe * cpsi, sphi * sthe * cpsi - cphi * spsi, cphi * sthe * cpsi + sphi * spsi],
                      [cthe * spsi, sphi * sthe * spsi + cphi * cpsi, cphi * sthe * spsi - sphi * cpsi],
                      [-sthe,       cthe * sphi,                      cthe * cphi]])
        return RotMat

    @property
    def inertia_matrix(self):
        return np.diag(self.config['inertia'])

    def update_state(self, piecewise_args):
        """Run the state update equations for self._dt seconds
        Update the current state of the system. It runs the model and updates
        its state to self._dt seconds.

        Parameters
        ----------
        piecewise_args : array
            It contains the parameters that are needed to run each section
            of the flight. It is an array of tuples.
            [(ct1, da1, t1), (ct2, da2, t2), ..., (ctn, dan, tn)]

            ct : float
                The collective thrust generated by all motors
            da : numpy.array
                The desired angular acceleration that the system should achieve.
                This should be of form [dp/dt, dq/dt, dr/dt]
            t: float
                Time for which this section should run and should be atleast twice
                self._dt
        """
        if self.save_state:
            overall_time = 0
            for section in piecewise_args:
                overall_time += section[2]

            overall_length = len(np.arange(0, overall_time, self._dt)) - (len(piecewise_args) - 1)
            # Allocate space for storing state of all sections
            final_state = np.zeros([overall_length + 100, 12])
        else:
            final_state = []

        # Create variable to maintain state between integration steps
        self._euler_dot = np.zeros(3)
        index = 0

        for section in piecewise_args:
            (total_thrust, desired_angular_acc, t) = section
            if t < (2 * self._dt):
                continue

            ts = np.arange(0, t, self._dt)
            state = np.concatenate((self.state['position'], self.state['velocity'],
                                    self.state['orientation'], self.state['ang_velocity']))
            output = odeint(self._integrator, state, ts, args=(total_thrust, desired_angular_acc))
            output_length = len(output)
            # State update
            [self.state['position'], self.state['velocity'],
             self.state['orientation'], self.state['ang_velocity']] = np.split(output[output_length - 1], 4)

            if self.save_state:
                # Final state update
                final_state[index:(index + output_length)] = output
                index = index + output_length - 1

        return final_state

    def _integrator(self, state, t, total_thrust, desired_angular_acc):
        """Callback function for scipy.integrate.odeint.
            At this point scipy is used to execute the forward integration

        Parameters
        ----------
        state : numpy.array
            System State: [x, y, z, xdot, ydot, zdot, phi, theta, psi, p, q, r]
        t : float
            Time

        Returns
        -------
        numpy.array
            Rates of the input state:
            [xdot, ydot, zdot, xddot, yddot, zddot, phidot, thetadot, psidot, pdot, qdot, rdot]
        """
        # Position inertial frame [x, y, z]
        pos = state[:3]
        # Velocity inertial frame [x, y, z]
        velocity = state[3:6]
        euler = state[6:9]
        # Angular velocity omega = [p, q, r]
        omega = state[9:12]

        # Derivative of euler angles [dphi/dt, dtheta/dt, dpsi/dt]
        euler_dot = self._euler_dot

        # omega = self.dt_eulerangles_to_angular_velocity(euler_dot, euler)
        moments = self.moments(desired_angular_acc, omega)
        thrusts = self.motor_thrust(moments, total_thrust)
        # Acceleration in inertial frame
        acc = self.acceleration(thrusts, euler)

        omega_dot = self.angular_acceleration(omega, thrusts)
        euler_dot = self.angular_velocity_to_dt_eulerangles(omega, euler)
        self._euler_dot = euler_dot
        # [velocity : acc : euler_dot : omega_dot]
        # print 'Vel', velocity
        # print 'Acc', acc
        # print 'Euler dot', euler_dot
        # print 'omdega dot', omega_dot
        return np.concatenate((velocity, acc, euler_dot, omega_dot))
Proudly powered by Weebly