diff --git a/trick_sims/SIM_DoubleCompoundPendulum/Modified_data/realtime.py b/trick_sims/SIM_DoubleCompoundPendulum/Modified_data/realtime.py new file mode 100644 index 000000000..6dd48d10c --- /dev/null +++ b/trick_sims/SIM_DoubleCompoundPendulum/Modified_data/realtime.py @@ -0,0 +1,12 @@ + +trick.frame_log_on() +trick.real_time_enable() +trick.exec_set_software_frame(0.033) +trick.itimer_enable() + +trick.exec_set_enable_freeze(True) +trick.exec_set_freeze_command(True) + +simControlPanel = trick.SimControlPanel() +trick.add_external_application(simControlPanel) + diff --git a/trick_sims/SIM_DoubleCompoundPendulum/Modified_data/state_data.dr b/trick_sims/SIM_DoubleCompoundPendulum/Modified_data/state_data.dr new file mode 100644 index 000000000..4158495dc --- /dev/null +++ b/trick_sims/SIM_DoubleCompoundPendulum/Modified_data/state_data.dr @@ -0,0 +1,47 @@ +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.DRBinary("StateData")) +drg[DR_GROUP_ID].set_freq(trick.DR_Always) +drg[DR_GROUP_ID].set_cycle(0.001) +drg[DR_GROUP_ID].set_single_prec_only(False) +drg[DR_GROUP_ID].add_variable("dyn.pendulum.pos1[0]") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.pos1[1]") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.pos2[0]") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.pos2[1]") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.vel1[0]") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.vel1[1]") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.vel2[0]") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.vel2[1]") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.gravity") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.omega1") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.omega2") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.omegaDot1") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.omegaDot2") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.force_remain") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.torque_remain") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.angle1") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.angle2") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.mass1") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.mass2") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.radius1") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.radius2") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.inertia1") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.inertia2") + +drg[DR_GROUP_ID].add_variable("dyn.pendulum.total_energy") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.potential_energy") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.kinetic_energy") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.angular_momentum1") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.angular_momentum2") +drg[DR_GROUP_ID].add_variable("dyn.pendulum.total_angular_momentum") + + +trick.add_data_record_group(drg[DR_GROUP_ID], trick.DR_Buffer) +drg[DR_GROUP_ID].enable() diff --git a/trick_sims/SIM_DoubleCompoundPendulum/README.md b/trick_sims/SIM_DoubleCompoundPendulum/README.md new file mode 100644 index 000000000..3d221750a --- /dev/null +++ b/trick_sims/SIM_DoubleCompoundPendulum/README.md @@ -0,0 +1,127 @@ +# Double Compound Pendulum + + +![](images/Picture1_DoubleCompoundPendulumSketch.png) + + +A double compound pendulum is a mechanical system consisting of two bodies, one which is attached to a fixed base on one end and the other which is attached to the loose end of the first. “Compound” refers to the fact that the mass of the system is not concentrated completely at one point. These pendulums are both free to rotate about their attachment point, provided they maintain the above constraints. + + +## Simplifications + +SIM_DoubleCompoundPendulum will neglect the effects of friction and air resistance. We present a classic example of a chaotic system, as slight variations in initial values can cause drastic differences and unpredictable behavior long term. + +The purpose of this sim is to model a double compound pendulum’s movement and at any given point calculate: + + +1. Angular momentum + +2. Potential energy + +3. Kinetic energy + +4. Total energy + +In this simulation, you can set the initial conditions to what you’d like, and the equations within the sim will simulate how the simulation will behave given the starting conditions. + +## Changing Inputs to the Simulation + +Go to the input file from the SIM_DoubleCompoundPendulum directory + + + vi RUN_DoubleCompoundPendulum/input.py/ + + +In this file is where you can change the starting angle of the system, the mass values, the lengths of the masses, and the simulation run time. + + + +## Building and Running the Simulation + +In order to build a Trick simulation, the Trick-specific command **`trick-CP`** must be run to begin compilation process. **`trick-CP`** is located in the Trick `bin` directory. Run the **`trick-CP`** command in the SIM_DoubleCompoundPendulum directory. Upon successful compulation, the following message will be displayed in the terminal: + + + === Simulation make complete === +To run, type in the terminal while in the SIM_DoubleCompoundPendulum directory: + + %S_main_*.exe RUN_test/input.py + + +## Kinematics of the System + +To the motion of this pendulum, we start by getting the kinematic equations associated with it. The following equations present the position in the x and y directions of the center of mass of each mass on the pendulum. + + +![](images/Picture2_FirstSetofPositionEquations.png) + +![](images/Picture3_SecondSetofPositionEquations.png) + + +Once you have these values, you can differentiate them in order to derive the velocity of each pendulum: + + +![](images/Picture4_FirstsetofVelocityEquations.png) + +![](images/Picture5_SecondsetofVelocityEquations.png) + + +Finally, you can use these values to derive the potential and kinetic energy of the system. + +### Energy in the System + +#### Potential Energy + +The general equation for potential energy: + + +![](images/Picture6_GeneralPotentialEnergyEquation.png) + + +Can be applied to our simulation by just plugging in the values we have for the y components of position. + + +![](images/Picture7_PotentialEnergywithvariablespluggedin.png) + + +For the purpose of simplicity in coding, we included 2 variables to make the potential energy equation less hairy. + + +Hence: + +![](images/Picture8_Mu1andMu2.png) + +![](images/Picture9_FinalpotentialenergyEquation) + + +#### Kinetic Energy + +When deriving kinetic energy there is both translational and rotational energy, and the equation must account for both. Hence the general equation for kinetic energy is as follows. + + +![](images/Picture10_GeneralKineticEnergyEquation.png) + + +If we plug in the position, velocity, and inertia for a slim rod where they belong in this equation, we arrive at an equation for kinetic energy as follows. + + +![](images/Picture11_KineticEnergywithvariablespluggedin.png) + + +Using the following trigonometric identities as well as variables to simplify the equation + + +![](images/Picture12_TrigonometricIdentities.png) + +![](images/Picture13_J1andJ2andJ3.png) + +We arrive at the equation used in the simulation. + +![](images/Picture14_FinalkineticEnergyEquation.png) + +## Angular Momentum +Angular Momentum is equal to inertia times the angular velocity of an object. As such, our equation for inertia comes out to: +![](images/Picture15_AngularMomentumFormula.png) + +## Results with Default Parameterization +![](images/Picture16_Graphs.png) + diff --git a/trick_sims/SIM_DoubleCompoundPendulum/RUN_test/input.py b/trick_sims/SIM_DoubleCompoundPendulum/RUN_test/input.py new file mode 100644 index 000000000..0ff23fa80 --- /dev/null +++ b/trick_sims/SIM_DoubleCompoundPendulum/RUN_test/input.py @@ -0,0 +1,91 @@ +################################################################################ +# ___ ___ _ _ ___ _ ___ # +# | \ / _ \| | | | - )| | | _| # +# | |) | (_) | |_| |--| | |_| -- # +# |__ / \___/|_____|_-_)|___|___| # +# # +# __ ___ _ ____ _ _ _ _ _ _ _ # +# | _\| _| \ | | \| | | | | | | | | \ / | # +# | _/| --| \| ||) | |_| | |_| |_| | \/ | # +# |_| |___|_|\__|__/|_____|___|_____|_|\/|_| # +# # +# __ ___ ___ ____ # +# __| ___| \ / _ \| __| # +# | ___| |) | (_) | _| # +# |__ |___/ \___/|_| # +# # +# This input creates a double pendulum with DOF: # +# -Creates 2 bodies # +# -Initializes 2 DOF # +################################################################################ + +# ============================================================================ - +# This file is used to store several quality of life python functions. +# ============================================================================ - +exec(open("Modified_data/state_data.dr").read()) +#exec(open("Modified_data/realtime.py").read()) + +################################################################################ +# dyn DATA # +# (NOTE: dyn is our instance of BodySimObject) # +# This builds our objects, and defines their physical properties. # +################################################################################ + +# ============================================================================ - +# Establish the physical properties of the body...CONSTANTS! +# ============================================================================ - +gravity = 9.81 +mass1 = 10 +mass2 = 10 + +length1 = 1.0 +length2 = 1.0 + +radius1 = length1 * 0.5 +radius2 = length2 * 0.5 + +dyn.pendulum.mass1 = mass1 +dyn.pendulum.mass2 = mass2 + +dyn.pendulum.radius1 = radius1 +dyn.pendulum.length1 = length1 + +dyn.pendulum.radius2 = radius2 +dyn.pendulum.length2 = length2 + +dyn.pendulum.gravity = gravity + + +dyn.pendulum.inertia1 = (1.0/3.0) * mass1 * length1 * length1 +dyn.pendulum.inertia2 = (1.0/3.0) * mass2 * length2 * length2 +dyn.pendulum.Ja = (0.25 * mass1 * length1 * length1) + (mass2 * length1 * length1) + ((1.0/12.0) * mass1 * length1 * length1) +dyn.pendulum.Jb = ((1.0/12) * mass2 * length2 * length2) + (0.25 * mass2 * length2 * length2) +dyn.pendulum.Jx = 0.5 * mass2 * length1 * length2 +dyn.pendulum.mu1 = ((0.5 * mass1) + mass2) * gravity * length2 +dyn.pendulum.mu2 = 0.5 * mass2 * gravity * length2 + + + +# ============================================================================ - +# Initialize system +# ============================================================================ - + +dyn.pendulum.ANGLE1_INIT = 0.7853981633974483 +dyn.pendulum.ANGLE2_INIT = 0.7853981633974483 + +################################################################################ +# SIMULATION SET UP # +# This is where we set up the remaining inputs to create a working sim. # +################################################################################ + +# ============================================================================ - +# getintegrator(integration method, number of variables to be integrated) +# This sets up the integration scheme we use to analyze this system. +# ============================================================================ - +dyn_integloop.getIntegrator(trick.Runge_Kutta_4, 20) + +# ============================================================================ - +# This tells the sim to stop after a few seconds +# ============================================================================ - + +trick.stop(10.0) diff --git a/trick_sims/SIM_DoubleCompoundPendulum/S_define b/trick_sims/SIM_DoubleCompoundPendulum/S_define new file mode 100644 index 000000000..019d8437e --- /dev/null +++ b/trick_sims/SIM_DoubleCompoundPendulum/S_define @@ -0,0 +1,25 @@ +/************************TRICK HEADER************************* +PURPOSE: + ( double compound pendulum simulation ) +LIBRARY DEPENDENCIES: + ((models/src/pendulum.cpp) + ) +*************************************************************/ + +#include "sim_objects/default_trick_sys.sm" +##include "models/include/pendulum.hh" +class PendulumSimObject : public Trick::SimObject { + public: + PENDULUM pendulum; + + PendulumSimObject() { + ("default_data") pendulum.default_data() ; + ("initialization") pendulum.init() ; + ("derivative") pendulum.derivative() ; + ("integration") trick_ret = pendulum.integ() ; + } +}; + +PendulumSimObject dyn; +IntegLoop dyn_integloop(0.0001) dyn; + diff --git a/trick_sims/SIM_DoubleCompoundPendulum/S_overrides.mk b/trick_sims/SIM_DoubleCompoundPendulum/S_overrides.mk new file mode 100644 index 000000000..fe03f5da8 --- /dev/null +++ b/trick_sims/SIM_DoubleCompoundPendulum/S_overrides.mk @@ -0,0 +1,24 @@ +#============================================================================= +# Allow user to specify their own package locations. +# - File is skipped if not present +#============================================================================= + +#============================================================================= +# Set up the include paths +#============================================================================= + +TRICK_CFLAGS := -I${PWD} -I.. +TRICK_CXXFLAGS := -I${PWD} -I.. + + +#============================================================================= +# Add target to clean the documents directories of TeX related files. +#============================================================================= +clean_docs: + @rm -f *_log *execution *_hs *_summary + @cd docs; rm -f *.aux *.bbl *.blg *.lof *.log *.lot *.toc + @cd docs/tex; rm -f *.aux *.log + @echo Cleaned documents directory. + +spotless: clean_docs + diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture10_GeneralKineticEnergyEquation.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture10_GeneralKineticEnergyEquation.png new file mode 100644 index 000000000..eaf0a0a5f Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture10_GeneralKineticEnergyEquation.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture11_KineticEnergywithvariablespluggedin.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture11_KineticEnergywithvariablespluggedin.png new file mode 100644 index 000000000..deca2e46e Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture11_KineticEnergywithvariablespluggedin.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture12_TrigonometricIdentities.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture12_TrigonometricIdentities.png new file mode 100644 index 000000000..bbad4650f Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture12_TrigonometricIdentities.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture13_J1andJ2andJ3.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture13_J1andJ2andJ3.png new file mode 100644 index 000000000..64b90f943 Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture13_J1andJ2andJ3.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture14_FinalkineticEnergyEquation.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture14_FinalkineticEnergyEquation.png new file mode 100644 index 000000000..9d3a1d4c5 Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture14_FinalkineticEnergyEquation.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture15_AngularMomentumFormula.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture15_AngularMomentumFormula.png new file mode 100644 index 000000000..4623f73c1 Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture15_AngularMomentumFormula.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture16_Graphs.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture16_Graphs.png new file mode 100644 index 000000000..a97006847 Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture16_Graphs.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture1_DoubleCompoundPendulumSketch.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture1_DoubleCompoundPendulumSketch.png new file mode 100644 index 000000000..98683f3cd Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture1_DoubleCompoundPendulumSketch.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture2_FirstSetofPositionEquations.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture2_FirstSetofPositionEquations.png new file mode 100644 index 000000000..c72661f79 Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture2_FirstSetofPositionEquations.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture3_SecondSetofPositionEquations.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture3_SecondSetofPositionEquations.png new file mode 100644 index 000000000..436fff40b Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture3_SecondSetofPositionEquations.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture4_FirstsetofVelocityEquations.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture4_FirstsetofVelocityEquations.png new file mode 100644 index 000000000..b9ab0970f Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture4_FirstsetofVelocityEquations.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture5_SecondsetofVelocityEquations.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture5_SecondsetofVelocityEquations.png new file mode 100644 index 000000000..bdad22d23 Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture5_SecondsetofVelocityEquations.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture6_GeneralPotentialEnergyEquation.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture6_GeneralPotentialEnergyEquation.png new file mode 100644 index 000000000..9471a52f4 Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture6_GeneralPotentialEnergyEquation.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture7_PotentialEnergywithvariablespluggedin.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture7_PotentialEnergywithvariablespluggedin.png new file mode 100644 index 000000000..6aeb45c3d Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture7_PotentialEnergywithvariablespluggedin.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture8_Mu1andMu2.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture8_Mu1andMu2.png new file mode 100644 index 000000000..ce5db821c Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture8_Mu1andMu2.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/images/Picture9_FinalpotentialenergyEquation.png b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture9_FinalpotentialenergyEquation.png new file mode 100644 index 000000000..d0c0123fe Binary files /dev/null and b/trick_sims/SIM_DoubleCompoundPendulum/images/Picture9_FinalpotentialenergyEquation.png differ diff --git a/trick_sims/SIM_DoubleCompoundPendulum/models/include/pendulum.hh b/trick_sims/SIM_DoubleCompoundPendulum/models/include/pendulum.hh new file mode 100644 index 000000000..bddf5a7d5 --- /dev/null +++ b/trick_sims/SIM_DoubleCompoundPendulum/models/include/pendulum.hh @@ -0,0 +1,102 @@ +/********************************************************************* +PURPOSE: (Simulate a double compound pendulum) +LIBRARY DEPENDENCIES: + ((models/src/pendulum.o)) +*********************************************************************/ +#ifndef PENDULUM_HH +#define PENDULUM_HH + +class PENDULUM { +public: + PENDULUM(); + ~PENDULUM(); + //Variables + + //Initializes state variables + double POS1_INIT[2]; + double POS2_INIT[2]; + double VEL1_INIT[2]; + double VEL2_INIT[2]; + double ANGLE1_INIT; + double ANGLE2_INIT; + double OMEGA1_INIT; + double OMEGA2_INIT; + double OMEGADOT1_INIT; + double OMEGADOT2_INIT; + + //State variables + double pos1[2]; /* m position of center of mass wrt inertial frame */ + double pos2[2]; /* m position of center of mass wrt inertial frame */ + double vel1[2]; /* m/s velocity wrt inertial fram */ + double vel2[2]; /* m/s velocity wrt inertial fram */ + double angle1; /* rad angle wrt inertial frame */ + double angle2; /* rad angle wrt inertial frame */ + double omega1; /* rad/s angular velocity wrt inertial frame */ + double omega2; /* rad/s angular velocity wrt inertial frame */ + + double Ja; /* -- constant in kinetic energy equation*/ + double Jb; /* -- constant in kinetic energy equation*/ + double Jx; /* -- constant in kinetic energy equation*/ + + double mu1; /* -- constant in potential energy equation*/ + double mu2; /* -- constant in potential energy equation*/ + + double angular_momentum1; /* kg*m^2/s angular momentum of pendulum 1*/ + double angular_momentum2; /* kg*m^2/s angular momentum of pendulum 2*/ + double total_angular_momentum; /* kg*m^2/s system's total angular momentum*/ + double kinetic_energy; /* N*m kinetic energy */ + double potential_energy; /* N*m potential energy */ + double total_energy; /* N*m total energy */ + double omegaDot1; /* rad/s2 angular acceleration wrt inertial frame */ + double omegaDot2; /* rad/s2 angular acceleration wrt inertial frame */ + + double inertia_tensor1[3][3]; /* kg*m^2 inertia tensor */ + double inertia_tensor2[3][3]; /* kg*m^2 inertia tensor */ + + double radius1; /* m radius length */ + double length1; /* m pendulum 1 length */ + double length2; /* m pendulum 2 length */ + double radius2; /* m radius length */ + double mass1; /* kg mass of object */ + double mass2; /* kg mass of object */ + double inertia1; /* kg*m^3 */ + double inertia2; /* kg*m^3 */ + double mat_mass[6][6]; /* -- mass matrix */ + double quad1_mat[3][3]; /* -- mass matrix */ + double quad2_mat[3][3]; /* -- mass matrix */ + double quad3_mat[3][3]; /* -- mass matrix */ + double quad4_mat[3][3]; /* -- mass matrix */ + double matrix_a[2][2]; /* -- mass matrix */ + double I_matrix[2][2]; /* -- inertia matrix */ + + double CM[3]; + double CM1[3]; + double CM2[3]; + + double vec_a[6]; + double vec_b[6]; + + double rotate1[3][3]; + double rotate2[3][3]; + + double gravity; /*m/s^2 acceleration due to gravity*/ + double force_remain; + double torque_remain; + + // Methods + void default_data(); + void init(); + void derivative(); + int integ(); + + void mass_matrix(); + void calcposition(); + void calcforce_remain(); + void calctotal_energy(); + void calctotal_angular_momentum(); + void calctorque_remain(); + void eq_setup(); + void eq_solver(); +}; + +#endif diff --git a/trick_sims/SIM_DoubleCompoundPendulum/models/src/pendulum.cpp b/trick_sims/SIM_DoubleCompoundPendulum/models/src/pendulum.cpp new file mode 100644 index 000000000..3b942625c --- /dev/null +++ b/trick_sims/SIM_DoubleCompoundPendulum/models/src/pendulum.cpp @@ -0,0 +1,236 @@ +/************************************************************************* +PURPOSE: (Double Compound Pendulum Simulation) +LIBRARY DEPENDENCIES: + ((models/src/pendulum.o)) +**************************************************************************/ + +#include +#include +#include + +#include "trick/integrator_c_intf.h" +#include "trick/trick_math_proto.h" +#include "trick/trick_math_error.h" +#include "trick/reference_frame.h" +#include "trick/vector_macros.h" +#include "trick/matrix_macros.h" + +#include "../include/pendulum.hh" + +PENDULUM::PENDULUM(){}; +PENDULUM::~PENDULUM(){}; + +/************************************************************************* +FUNCTION: PENDULUM::default_data() +PURPOSE:` (Creates an object) +*************************************************************************/ + +void PENDULUM::default_data() { + + //Initializing vectors and matricies to zero. + for(int i = 0; i<6; i++) + vec_b[i] = 0.0; + + pos1[0] = 0.0; + pos1[1] = 0.0; + pos2[0] = 0.0; + pos2[1] = 0.0; + vel1[0] = 0.0; + vel1[1] = 0.0; + vel2[0] = 0.0; + vel2[1] = 0.0; + angle1 = 0.0; + length1 = 0.0; + length2 = 0.0; + angle2 = 0.0; + omega1 = 0.0; + omega2 = 0.0; + omegaDot1 = 0.0; + omegaDot2 = 0.0; + inertia1 = 0.0; + inertia2 = 0.0; + Ja = 0.0; + Jb = 0.0; + Jx = 0.0; + mu1 = 0.0; + mu2 = 0.0; + +}; + +/************************************************************************ +FUNCTION: PENDULUM::init() +PURPOSE: (Initializing state variables, creating inertia and CoM skew + matrices) +************************************************************************/ +void PENDULUM::init() { + + pos1[0] = POS1_INIT[0]; + pos1[1] = POS1_INIT[1]; + pos2[0] = POS2_INIT[0]; + pos2[1] = POS2_INIT[1]; + vel1[0] = VEL1_INIT[0]; + vel1[1] = VEL1_INIT[1]; + vel2[0] = VEL2_INIT[0]; + vel2[1] = VEL2_INIT[1]; + + angle1 = ANGLE1_INIT; + angle2 = ANGLE2_INIT; + omega1 = OMEGA1_INIT; + omega2 = OMEGA2_INIT; + omegaDot1 = OMEGADOT1_INIT; + omegaDot2 = OMEGADOT2_INIT; + +}; + +/****************************************************************************** +FUNCTION: PENDULUM::mass_matrix() +PURPOSE: (Calculates 6x6 mass matrix M for eq M*a + r = b) +******************************************************************************/ +void PENDULUM::mass_matrix(){ + matrix_a[0][0] = Ja; + matrix_a[0][1] = Jx * cos(angle1 - angle2); + matrix_a[1][0] = Jx * cos(angle1 - angle2); + matrix_a[1][1] = Jb; +}; + +void PENDULUM::calcposition() { + pos1[0] = length1 * sin(angle1); + pos1[1] = -length1 * cos(angle1); + + pos2[0] = (length1 * sin(angle1)) + (length2 * sin(angle2)); + pos2[1] = (-length1 * cos(angle1)) + (-length2 * cos(angle2)); +}; + +/**************************************************************************** +FUNCTION: PENDULUM::calcforce_remain() +PURPOSE: (Calculate remaining force wrt inertial frame) +****************************************************************************/ +void PENDULUM::calcforce_remain() { + force_remain = Jx * sin(angle1 - angle2) * omega2 * omega2 + (mu1 * sin(angle1)); +}; + +/*************************************************************************** +FUNCTION: PENDULUM::calctorque_remain() +PURPOSE: (Calculate remaining torque wrt inertial frame) +***************************************************************************/ +void PENDULUM::calctorque_remain() { + torque_remain = mu2 * sin(angle2) - (Jx * sin(angle1 - angle2) * omega1 * omega1); +}; + +/*************************************************************************** +FUNCTION: PENDULUM::eq_setup() +PURPOSE: (Establishes b column vector for final eq M*a=b) +***************************************************************************/ +void PENDULUM::eq_setup() { + vec_b[0] = -force_remain; + vec_b[1] = -torque_remain; +}; + +/************************************************************************** +FUNCTION: PENDULUM::eq_solver() +PURPOSE: (Performs choleski decomposition and computes linear and angular + accelerations) +**************************************************************************/ +void PENDULUM::eq_solver() { + // Extract elements + double a = matrix_a[0][0]; + double b = matrix_a[0][1]; + double c = matrix_a[1][0]; + double d = matrix_a[1][1]; + + // Calculate determinant + double det = (a * d) - (b * c); + + // Check if determinant is zero + if (det == 0) { + throw std::invalid_argument("Matrix is singular and cannot be inverted."); + } + + // Calculate the inverse + double I_matrix[2][2]; + I_matrix[0][0] = d / det; + I_matrix[0][1] = -1.0 * b / det; + I_matrix[1][0] = -1.0 * c / det; + I_matrix[1][1] = a / det; + + // Calculate angular acceleration + omegaDot1 = (I_matrix[0][0] * vec_b[0]) + (I_matrix[0][1] * vec_b[1]); + omegaDot2 = (I_matrix[1][0] * vec_b[0]) + (I_matrix[1][1] * vec_b[1]); +}; + + + +/*************************************************************************** +FUNCTION: PENDULUM::calctotalenergy() +PURPOSE: (Calculates kinetic, potential, and total energy in the system) +***************************************************************************/ +void PENDULUM::calctotal_energy(){ + kinetic_energy = (0.5*omega1*omega1*(Ja))+(0.5*(Jb)*omega2*omega2)+(Jx*omega1*omega2*cos(angle1 - angle2)); + potential_energy = (-mu1*cos(angle1))-(mu2*cos(angle2)); + total_energy = kinetic_energy + potential_energy; +}; + +/*************************************************************************** +FUNCTION: PENDULUM::calctotalangularmomentum() +PURPOSE: (Calculates individual and total angular momentum in the system) +***************************************************************************/ +void PENDULUM::calctotal_angular_momentum(){ + angular_momentum1 = inertia1*omega1; + angular_momentum2 = inertia2*omega2; + total_angular_momentum = angular_momentum1+angular_momentum2; + +}; + +/*************************************************************************** +FUNCTION: PENDULUM::derivative() +PURPOSE: (Calls all functions in desired order for calculations) +***************************************************************************/ +void PENDULUM::derivative() { + mass_matrix(); + calcforce_remain(); + calctorque_remain(); + eq_setup(); + eq_solver(); + calcposition(); + calctotal_angular_momentum(); + calctotal_energy(); +}; + +/*************************************************************************** +FUNCTION: PENDULUM::integrate() +PURPOSE: (Sets up trick integration) +***************************************************************************/ + +#ifndef IN_MAKE +int PENDULUM::integ() { + + int integration_step; + + load_state( + &angle1, + &omega1, + &angle2, + &omega2, + NULL); + + load_deriv( + &omega1, + &omegaDot1, + &omega2, + &omegaDot2, + NULL); + + integration_step = integrate(); + + unload_state( + &angle1, + &omega1, + &angle2, + &omega2, + NULL); + + + return(integration_step); +}; + +#endif