Model Rocket Simulation. Ref #726

trick_sims/SIM_rocket/Modified_data/Rocket.dr

@@ -0,0 +1,35 @@
+global DR_GROUP_ID
+global drg
+try:
+    if DR_GROUP_ID >= 0:
+        DR_GROUP_ID += 1
+except NameError:
+    DR_GROUP_ID = 0
+    drg = []
+
+drg.append(trick.DRAscii("Rocket"))
+drg[DR_GROUP_ID].set_freq(trick.DR_Always)
+drg[DR_GROUP_ID].set_cycle(0.01)
+drg[DR_GROUP_ID].set_single_prec_only(False)
+drg[DR_GROUP_ID].add_variable("dyn.rocket.vel_unit[0]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.vel_unit[1]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.thrust_force[0]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.thrust_force[1]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.drag_force[0]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.drag_force[1]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.gravity_force[0]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.gravity_force[1]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.total_force[0]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.total_force[1]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.mission_time")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.total_mass")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.mass_rate")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.position[0]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.position[1]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.velocity[0]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.velocity[1]")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.coefficient_of_drag")
+drg[DR_GROUP_ID].add_variable("dyn.rocket.empty_mass")
+drg[DR_GROUP_ID].set_max_file_size(1 * 1073741824) # multiply converts GiB to B --Dr. Dre
+trick.add_data_record_group(drg[DR_GROUP_ID], trick.DR_Buffer)
+drg[DR_GROUP_ID].enable()

trick_sims/SIM_rocket/Modified_data/realtime.py

@@ -0,0 +1,11 @@
+
+trick.frame_log_on()
+trick.real_time_enable()
+trick.exec_set_software_frame(0.1)
+trick.itimer_enable()
+
+trick.exec_set_enable_freeze(True)
+trick.exec_set_freeze_command(True)
+
+simControlPanel = trick.SimControlPanel()
+trick.add_external_application(simControlPanel)

trick_sims/SIM_rocket/RUN_test/input.py

@@ -0,0 +1,4 @@
+execfile("Modified_data/Rocket.dr")
+#execfile("Modified_data/realtime.py")
+
+trick.exec_set_terminate_time(20.0)

trick_sims/SIM_rocket/Readme.md

@@ -0,0 +1,108 @@
+![Forces](images/Banner.png)
+
+The following describes a Trick-based simulation of a model rocket under the forces of thrust,
+gravity and drag, using the parameters and equations shown below. 
+
+###Thrust Force
+
+In our model rocket simulation, we use an Estes E9 motor by default.  It's thrust profile is shown below.
+
+![E9Engine](images/E9_engine.png)
+
+| Name                                | Units      | Value      |
+|-------------------------------------|------------|------------|
+| rocket.motor.thrust\_sample\_times  |     s      | 0.000, 0.010, 0.020, 0.030, 0.040, 0.050, 0.100, 0.110, 0.120, 0.130,0.140, 0.155, 0.165, 0.180, 0.190, 0.200, 0.215, 0.230, 0.250, 0.275, 0.300, 0.400, 0.500, 2.600, 2.700, 2.725, 2.750, 2.800, 100.0 |
+| rocket.motor.thrust\_sample\_values |     N      | 0.0,  0.1,  0.3,  0.5,  0.8,  1.5, 10.0, 12.6, 15.2, 17.8, 20.4, 23.0, 24.6, 25.0, 24.8, 24.4, 22.0, 17.0, 13.7, 12.6, 11.9, 10.7, 10.6, 10.5, 10.3, 10.0,  9.7, 0.0, 0.0 |
+| rocket.motor.thrust\_sample\_count  |    --      |   29         |
+    
+
+###Drag Force
+The drag on the model rocket will be calculated using the drag equation shown below:
+
+![ForceDrag](images/F_drag_eqn.png)
+
+| Symbol | Name                          | Units      | Value      |
+|--------|-------------------------------|------------|------------|
+| ρ| rocket.env.air\_density       | kg/m³ | 1.2        |
+| Cd     | rocket.coefficient\_of\_drag  | (unitless) | 0.75       |
+| A      | rocket.cross\_sectional\_area | m²    | 0.0015     |
+| v      | rocket.velocity               |   m/s      | Initially 0.0, Updated by integration of rocket-acceleration over time.|
+
+
+###Gravitation Force
+
+![ForceOfGravity](images/F_grav_eqn.png)
+
+| Symbol | Name                        | Units     | Value      |
+|--------|-----------------------------|-----------|------------|
+| g      | rocket.env.gravity          | m/s² | -9.81      |
+| m      | rocket.total\_mass          | kg        | Initially [Eq#2]. Updated by integration of rocket-mass-rate. |
+
+###Mass
+As the rocket motor burns, it expels mass, at high speed. Initially the rocket
+motor contains all of its propellant mass. When it burns out, it contains no propellant mass.
+
+| Name                          | Units | Value  |
+|-------------------------------|-------|--------|
+| rocket.empty\_mass            | kg    | 0.1162 |
+| rocket.motor.total\_mass      | kg    | 0.0567 |
+| rocket.motor.propellant\_mass | kg    | 0.0358 |
+
+
+##Initialization
+
+| Name                         | Units | Value |
+|------------------------------|-------|-------|
+| rocket.motor.exhaust\_speed  | m/s   | Calculated by [Eq#1] |
+| rocket.motor.total\_impulse  | N s   | Calculated by integrating motor thrust over time. |
+| rocket.total\_mass           | kg    | Calculated by [Eq#2] |
+| rocket.velocity              | m/s   | 0.0   |
+| rocket.position              | m     | 0.0   |
+| rocket.mission\_time         | s     | 0.0   |
+
+[Eq#1] **exhaust-speed** = rocket.motor.total\_impulse / rocket.motor.propellant\_mass
+
+[Eq#2] **rocket.total\_mass (@t=0)** = rocket.empty\_mass + rocket.motor.total\_mass (@t=0).
+
+
+##State Derivatives
+
+| Name                        | Units | Value  |
+|-----------------------------|-------|--------|
+| rocket.mission\_time\_rate  | s     | 1.0    |
+| rocket.thrust\_force        | N     | Interpolation of E9 Engine Thrust function |
+| rocket.mass\_rate           | kg/s  | Calculated by [Eq#3] |
+| rocket.drag\_force          | N     | Calculated by [Eq#4] |
+| rocket.gravity\_force       | N     | Calculated by [Eq#5] |
+| rocket.total\_force         | N     | Calculated by [Eq#6] |
+| rocket.acceleration         | N     | Calculated by [Eq#7] |
+
+[Eq#3] **rocket.mass\_rate** =  ‖ rocket.thrust\_force ‖ / rocket.motor.exhaust\_speed
+
+[Eq#4] **rocket.drag\_force** = - 0.5 × rocket.coefficient\_of\_drag × rocket.env.air\_density × rocket.cross\_sectional\_area × rocket.velocity × ‖ rocket.velocity ‖
+
+[Eq#5] **rocket.gravity\_force** = rocket.total\_mass × rocket.env.gravity
+
+[Eq#6] **rocket.total\_force** = rocket.thrust\_force + rocket.drag\_force + rocket.gravity\_force
+
+[Eq#7] **rocket.acceleration** = rocket.total\_force  / rocket.total\_mass
+
+
+##State Integration
+
+| Name                   | Units     | Value                                                      |
+|------------------------|-----------|------------------------------------------------------------|
+| rocket.mission\_time   | s         | Numerical integration of rocket.mission\_time\_rate. |
+| rocket.total\_mass     | kg        | Numerical integration of rocket.mass\_rate.  |
+| rocket.velocity        | m/s       | Numerical integration of rocket.acceleration.|
+| rocket.position        | m/s² | Numerical integration of rocket.velocity.    |
+
+##Results with Default Parameterization
+
+![Forces](images/Forces.png)
+
+![Forces](images/Position.png)
+
+![Forces](images/Velocity.png)
+
+![Forces](images/Mass.png)

trick_sims/SIM_rocket/S_define

@@ -0,0 +1,26 @@
+/************************************************************
+PURPOSE:
+    ( Simulate a model rocket. )
+LIBRARY DEPENDENCIES:
+    ((rocket/src/Rocket.cpp))
+*************************************************************/
+#include "sim_objects/default_trick_sys.sm"
+##include "rocket/include/Rocket.hh"
+class ModelRocketSimObject : public Trick::SimObject {
+    public:
+    Rocket rocket;
+    ModelRocketSimObject() {
+        ("default_data")     rocket.default_data() ;
+        ("initialization")   rocket.state_init() ;
+        ("derivative")       rocket.state_deriv() ;
+        ("integration") trick_ret = rocket.state_integ() ;
+    }
+};
+
+ModelRocketSimObject dyn;
+
+IntegLoop dyn_integloop (0.01) dyn;
+
+void create_connections() {
+    dyn_integloop.getIntegrator(Runge_Kutta_4, 6);
+}

trick_sims/SIM_rocket/S_overrides.mk

@@ -0,0 +1,2 @@
+TRICK_CFLAGS += -Imodels
+TRICK_CXXFLAGS += -Imodels

trick_sims/SIM_rocket/models/rocket/include/Environment.hh

@@ -0,0 +1,11 @@
+/*************************************************************************
+PURPOSE: (Environment for Model Rocket Sim)
+LIBRARY DEPENDENCIES:
+    ((rocket/src/Environment.o))
+**************************************************************************/
+class Environment {
+public:
+    double air_density; /* kg/m^3 */
+    double gravity[2];  /* m/s^2 */
+    int default_data();
+};

trick_sims/SIM_rocket/models/rocket/include/Motor.hh

@@ -0,0 +1,21 @@
+/*************************************************************************
+PURPOSE: (Simulate a model rocket motor.)
+LIBRARY DEPENDENCIES:
+    ((rocket/src/Motor.o)
+    (rocket/src/interpolate.o))
+**************************************************************************/
+class Motor {
+public:
+    const double * thrust_sample_times;
+    const double * thrust_sample_values;
+    unsigned int thrust_sample_count;
+
+    double total_impulse;   /* kg*m/s^2 */
+    double total_mass;      /* kg */
+    double propellant_mass; /* kg */
+    double exhaust_speed;   /* m/s */
+
+    int default_data ();
+    int state_init ();
+    double thrust_magnitude(double time);
+};

trick_sims/SIM_rocket/models/rocket/include/Rocket.hh

@@ -0,0 +1,40 @@
+/*************************************************************************
+PURPOSE: (Simulate a model rocket.)
+LIBRARY DEPENDENCIES:
+    ((rocket/src/Rocket.o))
+**************************************************************************/
+#ifndef _rocket_hh_
+#define _rocket_hh_
+
+#include "Environment.hh"
+#include "Motor.hh"
+
+class Rocket {
+public:
+    Environment env;
+    Motor motor;
+
+    double vel_unit[2];
+    double thrust_force[2];
+    double drag_force[2];
+    double gravity_force[2];
+    double total_force[2];
+
+    double mission_time ; /* s    time elapsed since launch */
+    double mission_time_rate;
+    double cross_sectional_area;
+    double coefficient_of_drag;
+    double empty_mass;
+    double total_mass ;   /* kg   xyz-position */
+    double mass_rate ;
+    double position[2] ;       /* m    xyz-position */
+    double velocity[2] ;       /* m/s  xyz-velocity */
+    double acc[2] ;       /* m/s2 xyz-acceleration  */
+
+    int default_data();
+    int state_init();
+    int state_deriv();
+    int state_integ();
+
+};
+#endif

trick_sims/SIM_rocket/models/rocket/include/interpolate.h

@@ -0,0 +1,10 @@
+#ifndef INTERPOLATE_H
+#define INTERPOLATE_H
+#ifdef __cplusplus
+extern "C" {
+#endif
+double interpolate( double x, const double xa[], const double fa[], int N_elems );
+#ifdef __cplusplus
+}
+#endif
+#endif

trick_sims/SIM_rocket/models/rocket/src/Environment.cpp

@@ -0,0 +1,8 @@
+#include "../include/Environment.hh"
+
+int Environment::default_data () {
+    air_density = 1.2;
+    gravity[0] = 0.0;
+    gravity[1] = -9.81;
+    return 0;
+}

trick_sims/SIM_rocket/models/rocket/src/Motor.cpp

@@ -0,0 +1,42 @@
+#include "../include/Motor.hh"
+#include "../include/interpolate.h"
+#include <iostream>
+
+#define NUM_ELEMENTS 29
+const double E9_sample_time_array[NUM_ELEMENTS] = {
+0.000, 0.010, 0.020, 0.030, 0.040, 0.050, 0.100, 0.110, 0.120, 0.130,
+0.140, 0.155, 0.165, 0.180, 0.190, 0.200, 0.215, 0.230, 0.250, 0.275,
+0.300, 0.400, 0.500, 2.600, 2.700, 2.725, 2.750, 2.800, 100.0 };
+
+const double E9_thrust_array[NUM_ELEMENTS] = {
+ 0.0,  0.1,  0.3,  0.5,  0.8,  1.5, 10.0, 12.6, 15.2, 17.8,
+20.4, 23.0, 24.6, 25.0, 24.8, 24.4, 22.0, 17.0, 13.7, 12.6,
+11.9, 10.7, 10.6, 10.5, 10.3, 10.0,  9.7, 0.0, 0.0 };
+
+int Motor::default_data () {
+
+    thrust_sample_times = E9_sample_time_array;
+    thrust_sample_values = E9_thrust_array;
+    thrust_sample_count = NUM_ELEMENTS;
+
+    total_mass = 0.0567;
+    propellant_mass = 0.0358;
+    state_init();
+    return 0;
+}
+
+int Motor::state_init () {
+
+    total_impulse = 0.0;
+    for (int ii=1 ; ii<NUM_ELEMENTS ; ii++) {
+        total_impulse +=  (thrust_sample_times[ii] - thrust_sample_times[ii-1]) *
+                          (thrust_sample_values[ii] + thrust_sample_values[ii-1]) / 2.0;
+    }
+    exhaust_speed = total_impulse / propellant_mass;
+    return 0;
+}
+
+double Motor::thrust_magnitude(double time) {
+    // Call interpolator
+    return interpolate( time, thrust_sample_times, thrust_sample_values, thrust_sample_count );
+}

trick_sims/SIM_rocket/models/rocket/src/Rocket.cpp

@@ -0,0 +1,86 @@
+/********************************* TRICK HEADER *******************************
+PURPOSE: ( Simulate a model rocket. )
+LIBRARY DEPENDENCY:
+    ((Rocket.o))
+*******************************************************************************/
+#include "../include/Rocket.hh"
+#include <math.h>
+#include <iostream>
+
+int Rocket::default_data() {
+
+    env.default_data();
+    motor.default_data();
+    mission_time = 0.0;
+    mission_time_rate = 1.0;
+    cross_sectional_area = 0.0015;
+    coefficient_of_drag = 0.75;
+    empty_mass = 0.1162; // 116.2 g
+    total_mass = empty_mass;
+    position[0] = 0.0;
+    position[1] = 0.0;
+    velocity[0] = 0.0;
+    velocity[1] = 0.001;
+    return (0);
+}
+
+int Rocket::state_init() {
+    motor.state_init();
+    total_mass = empty_mass + motor.total_mass;
+    return (0);
+}
+
+int Rocket::state_deriv() {
+
+    mission_time_rate = 1.0;
+
+    double thrust_mag = motor.thrust_magnitude( mission_time );
+
+    mass_rate = - thrust_mag / motor.exhaust_speed;
+
+    double vel_magnitude = sqrt( velocity[0] * velocity[0] + velocity[1] * velocity[1] );
+
+    if ( position[1] < 1.0 ) {
+        vel_unit[0] = 0.0;
+        vel_unit[1] = 1.0;
+    } else {
+        vel_unit[0] = velocity[0] / vel_magnitude;
+        vel_unit[1] = velocity[1] / vel_magnitude;
+    }
+
+    thrust_force[0] = vel_unit[0] * thrust_mag;
+    thrust_force[1] = vel_unit[1] * thrust_mag;
+
+    drag_force[0] = - 0.5 * coefficient_of_drag * env.air_density * cross_sectional_area * vel_magnitude * velocity[0];
+    drag_force[1] = - 0.5 * coefficient_of_drag * env.air_density * cross_sectional_area * vel_magnitude * velocity[1];
+
+    gravity_force[0] = env.gravity[0] * total_mass;
+    gravity_force[1] = env.gravity[1] * total_mass;
+
+    total_force[0] = thrust_force[0] + drag_force[0] + gravity_force[0];
+    total_force[1] = thrust_force[1] + drag_force[1] + gravity_force[1];
+
+    acc[0] = total_force[0] / total_mass;
+    acc[1] = total_force[1] / total_mass;
+
+   return(0);
+}
+
+#include "sim_services/Integrator/include/integrator_c_intf.h"
+
+int Rocket::state_integ() {
+
+   int integration_step;
+
+   load_state ( &mission_time, &total_mass, &position[0], &position[1], &velocity[0], &velocity[1], (double*)0);
+   load_deriv ( &mission_time_rate, &mass_rate, &velocity[0], &velocity[1], &acc[0], &acc[1], (double*)0);
+
+   integration_step = integrate();
+   unload_state ( &mission_time,
+                  &total_mass,
+                  &position[0], &position[1],
+                  &velocity[0], &velocity[1],
+                  (double*)0);
+
+   return(integration_step);
+}

trick_sims/SIM_rocket/models/rocket/src/interpolate.c

@@ -0,0 +1,22 @@
+#include "interpolate.h"
+#include <stdio.h>
+
+double interpolate( double x, const double xa[], const double fa[], int N_elems ) {
+    int ii;
+    for (ii=0 ; ((ii+2 < N_elems ) && (x > xa[ii+1])) ; ii++ ) ;
+    double x_lower = xa[ii];
+    double x_upper = xa[ii+1];
+    double f_lower = fa[ii];
+    double f_upper = fa[ii+1];
+    if (x < x_lower) {
+        fprintf(stderr, "Interpolation x is out of range low.\n");
+        return(f_lower);
+    }
+    if (x > x_upper) {
+        fprintf(stderr, "Interpolation x is out of range high.\n");
+        return(f_upper);
+    }
+    double f = (x_upper - x)/(x_upper - x_lower) * f_lower +
+               (x - x_lower)/(x_upper - x_lower) * f_upper ;
+    return(f);
+}

trick_sims/SIM_rocket/scripts/Forces.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+import matplotlib.pyplot as plt
+import numpy as np
+
+#
+data = np.genfromtxt('../RUN_test/log_Rocket.csv',
+                     delimiter=',',
+                     skip_header=1,
+                     skip_footer=1,
+                     names=['t',
+                            'vux', 'vuy',
+                            'Fthrustx', 'Fthrusty',
+                            'Fdragx', 'Fdragy',
+                            'Fgravx','Fgravy',
+                            'Ftotalx','Ftotaly',
+                            'Mt',
+                            'MassTotal', 'MassRate',
+                            'Posx','Posy',
+                            'velx','vely',
+                            'Cd', 'MassEmpty' ],
+                     dtype=(float, float)
+                    )
+
+curve1 = plt.plot(data['t'], data['Fthrusty'], 'C1-')
+curve2 = plt.plot(data['t'], data['Fdragy'], 'C2-')
+curve3 = plt.plot(data['t'], data['Fgravy'], 'C3-')
+curve4 = plt.plot(data['t'], data['Ftotaly'], 'C4-')
+plt.legend(('F_thrust', 'F_drag', 'F_grav', 'F_total'))
+plt.title('Rockets Forces Over Time')
+plt.xlabel('time (s)')
+plt.ylabel('force (N)')
+plt.grid(True)
+plt.show()

trick_sims/SIM_rocket/scripts/Mass.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+  
+import matplotlib.pyplot as plt
+import numpy as np
+
+#
+data = np.genfromtxt('../RUN_test/log_Rocket.csv',
+                     delimiter=',',
+                     skip_header=1,
+                     skip_footer=1,
+                     names=['t',
+                            'vux', 'vuy',
+                            'Fthrustx', 'Fthrusty',
+                            'Fdragx', 'Fdragy',
+                            'Fgravx','Fgravy',
+                            'Ftotalx','Ftotaly',
+                            'Mt',
+                            'MassTotal', 'MassRate',
+                            'Posx','Posy',
+                            'velx','vely',
+                            'Cd', 'MassEmpty' ],
+                     dtype=(float, float)
+                    )
+
+plt.plot(data['t'], data['MassTotal'], 'r-')
+plt.title('Rocket Mass')
+plt.xlabel('time (s)')
+plt.ylabel('Mass (kg)')
+plt.grid(True)
+plt.show()

trick_sims/SIM_rocket/scripts/Position.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+  
+import matplotlib.pyplot as plt
+import numpy as np
+
+#
+data = np.genfromtxt('../RUN_test/log_Rocket.csv',
+                     delimiter=',',
+                     skip_header=1,
+                     skip_footer=1,
+                     names=['t',
+                            'vux', 'vuy',
+                            'Fthrustx', 'Fthrusty',
+                            'Fdragx', 'Fdragy',
+                            'Fgravx','Fgravy',
+                            'Ftotalx','Ftotaly',
+                            'Mt',
+                            'MassTotal', 'MassRate',
+                            'Posx','Posy',
+                            'velx','vely',
+                            'Cd', 'MassEmpty' ],
+                     dtype=(float, float)
+                    )
+
+plt.plot(data['t'], data['Posy'])
+plt.title('Altitude')
+plt.xlabel('time (s)')
+plt.ylabel('Position-y (m)')
+plt.grid(True)
+plt.show()

trick_sims/SIM_rocket/scripts/Velocity.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+  
+import matplotlib.pyplot as plt
+import numpy as np
+
+#
+data = np.genfromtxt('../RUN_test/log_Rocket.csv',
+                     delimiter=',',
+                     skip_header=1,
+                     skip_footer=1,
+                     names=['t',
+                            'vux', 'vuy',
+                            'Fthrustx', 'Fthrusty',
+                            'Fdragx', 'Fdragy',
+                            'Fgravx','Fgravy',
+                            'Ftotalx','Ftotaly',
+                            'Mt',
+                            'MassTotal', 'MassRate',
+                            'Posx','Posy',
+                            'velx','vely',
+                            'Cd', 'MassEmpty' ],
+                     dtype=(float, float)
+                    )
+
+plt.plot(data['t'], data['vely'], 'g-')
+plt.title('Climb Rate')
+plt.xlabel('time (s)')
+plt.ylabel('Climb Rate (m/s)')
+plt.grid(True)
+plt.show()