added SIM_singlerigidbody

trick_sims/SIM_singlerigidbody/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)
+

trick_sims/SIM_singlerigidbody/Modified_data/state_data.dr

@@ -0,0 +1,54 @@
+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.10)
+drg[DR_GROUP_ID].set_single_prec_only(False)
+drg[DR_GROUP_ID].add_variable("dyn.body.force[0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.force[1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.force[2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.angle_force[0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.angle_force[1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.angle_force[2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.rotate[0][0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.rotate[0][1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.rotate[0][2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.rotate[1][0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.rotate[1][1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.rotate[1][2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.rotate[2][0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.rotate[2][1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.rotate[2][2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.pos[0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.pos[1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.pos[2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.angle[0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.angle[1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.angle[2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.vel[0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.vel[1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.vel[2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.acc[0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.acc[1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.acc[2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.omega[0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.omega[1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.omega[2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.omegaDot[0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.omegaDot[1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.omegaDot[2]")
+drg[DR_GROUP_ID].add_variable("dyn.body.FORCE_INIT[0]")
+drg[DR_GROUP_ID].add_variable("dyn.body.FORCE_INIT[1]")
+drg[DR_GROUP_ID].add_variable("dyn.body.FORCE_INIT[2]")
+
+
+
+trick.add_data_record_group(drg[DR_GROUP_ID], trick.DR_Buffer)
+drg[DR_GROUP_ID].enable()

trick_sims/SIM_singlerigidbody/README.md

@@ -0,0 +1,122 @@
+# Single Rigid Body
+``SIM_singlerigidbody`` is a simulation of the motion of a sphere that is a single rigid body with six degrees of freedom. 
+
+The motion of the sphere is found by using Euler's equations of motion, and can be controlled by changing several initial input values such as force, position, angle, angular velocity, velocity, and position of the force. The recorded data of the motion can then be viewed and analyzed. 
+
+## Dynamics model
+
+### Equations of Motion
+To determine the sphere's motion, we will use Euler's equations of motion for a single rigid body. Euler's equations describe the combined translational and rotational dynamics of a rigid body in three-dimensional space. 
+
+![Equations_of_motion](images\EquationsofMotion.png)
+
+Where, 
+
+![Force_and_Torque_eq](images\ForceTorque.png)
+* is a 6x1 column vector. 
+* F is the applied force.
+* τ is the torque.
+
+![Mass_Matrix_eq](images\MassMatrix.png)
+* is a 6x6 matrix. 
+* m is the mass.
+* I <sub> 3x3 </sub> is the identity matrix. 
+* I is the inertia. 
+* C <sub> m </sub> is the center of mass. 
+
+![Acceleration_Omega](images\AccOmega.png)
+* is a 6x1 column vector. 
+* a<sub> o </sub> is the acceleration.
+
+![Force_Torque_Remain](images\ForceTorqueRemain.png)
+* is a 6x1 column vector. 
+* &#0969; <sup> A </sup> is the angular velocity.
+
+
+#### Force
+The force acting on the sphere is calculated by the cross product of the rotation matrix and initial force. 
+
+![Force](images\Force.png)
+
+#### Torque
+Torque is found by the cross product of the rotation matrix and the cross product of the position of force and initial force. 
+
+![Torque](images\Torque.png)
+
+#### Mass
+The default value of the mass of the sphere is: 
+
+![Mass](images\Mass.png)
+
+#### Radius
+The default value of the radius is: 
+
+![Radius](images\Radius.png)
+
+#### Center of mass
+The center of mass of a sphere is equal to its radius. 
+
+![Center_of_mass](images\Centerofmass.png)
+
+#### Inertia
+Inertia of a sphere is calculated by: 
+
+![Inertia](images\Inertia.png)
+
+#### Acceleration and angular acceleration
+To determine the acceleration and angular acceleration, ```dLU_Choleski``` must be used. ```dLU_Choleski``` solves linear sets of equations in the form of [A]x = b and returns the vector x, which is the acceleration. It decomposes the symmetric, positive definite [A] matrix into a lower triangular form, where we can then substitute and our equation becomes: 
+
+![Vector_a](images\Vectora.png)
+
+The term 'a' is a 6x1 vector. Acceleration is equal to the first three values of vector a  and angular acceleration is equal to the last three values of vector a.
+
+### State Integration
+The state is defined by position, velocity, angle, and angular velocity. They are calculated by numerically integrating velocity, acceleration, angular velocity, and angular acceleration, respectively. 
+
+#### Position
+The position of the sphere is determined by integrating the velocity over time. 
+
+![Position](images\Position.png)
+
+#### Velocity
+The velocity is determined by integrating the acceleration of the sphere over time. 
+
+![Velocity](images\Velocity.png)
+
+#### Angle
+The angle is determined by integrating the angular velocity over time. 
+
+![Angle](images\Angle.png)
+
+#### Angular velocity
+The angular velocity is determined by integrating the angular acceleration over time. 
+
+![Angular_Velocity](images\Angularvelocity.png)
+
+
+
+## Inputs
+
+Variable                    |Type         |Units
+----------------------------|-------------|---------
+dyn.body.POS_INIT           |double[3]    |m
+dyn.body.ANGLE_INIT         |double[3]    |rad
+dyn.body.OMEGA_INIT         |double[3]    |rad*s^-1 
+dyn.body.FORCE_INIT         |double[3]    |N
+dyn.body.TORQUE_INIT        |double[3]    |N*m
+dyn.body.VEL_INIT           |double[3]    |m/s
+dyn.body.POS_FORCE          |double[3]    |m
+
+## Outputs
+
+Variable                    |Type         |Units
+----------------------------|-------------|--------
+dyn.body.pos                |double[3]    |m
+dyn.body.vel                |double[3]    |m/s
+dyn.body.acc                |double[3]    |m/s^2
+dyn.FORCE_INIT              |double[3]    |N
+dyn.force                   |double[3]    |N
+dyn.body.angle_force        |double[3]    |rad
+dyn.body.omegaDot           |double[3]    |rad/^s2
+dyn.body.omega              |double[3]    |rad*s^-1
+dyn.body.rotate             |double[3]    |

trick_sims/SIM_singlerigidbody/RUN_test/input.py

@@ -0,0 +1,81 @@
+################################################################################
+#     _     ___          _                                                     #
+#    / |___| _ ) ___  __| |_  _                                                #
+#    | |___| _ \/ _ \/ _` | || |                                               #
+#    |_|   |___/\___/\__,_|\_, |                                               #
+#     __     ___   ___  __|__/                                                 # 
+#    / / ___|   \ / _ \| __|                                                   #
+#   / _ \___| |) | (_) | _|                                                    #
+#   \___/   |___/ \___/|_|                                                     #
+#                                                                              #
+# This input creates a simple body with 6 DOF:                                 #
+#		-Creates 1 body                      			       #
+#		-Initializes 6 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! 
+# ============================================================================ -
+dyn.body.mass = 20.0
+dyn.body.radius = 2.0
+
+# ============================================================================ -
+# Initialize system 
+# ============================================================================ -
+M_PI = 3.141592653589793238460
+
+#dyn.body.POS_INIT[0] = 0.0
+#dyn.body.POS_INIT[1] = 0.0
+#dyn.body.POS_INIT[2] = 0.0
+
+#dyn.body.ANGLE_INIT[0] = 0.0
+#dyn.body.ANGLE_INIT[1] = 0.0
+#dyn.body.ANGLE_INIT[2] = 0.0
+
+#dyn.body.OMEGA_INIT[0] = 0.0
+#dyn.body.OMEGA_INIT[1] = 0.0
+#dyn.body.OMEGA_INIT[2] = 0.0
+
+#dyn.body.FORCE_INIT[0] = 0.0
+#dyn.body.FORCE_INIT[1] = 0.0
+#dyn.body.FORCE_INIT[2] = 10.0
+
+#dyn.body.TORQUE_INIT[0] = 0.0
+#dyn.body.TORQUE_INIT[1] = 0.0
+#dyn.body.TORQUE_INIT[2] = 0.0
+
+#dyn.body.VEL_INIT[0] = 0.0
+#dyn.body.VEL_INIT[1] = 0.0
+#dyn.body.VEL_INIT[2] = 0.0
+
+#dyn.body.POS_FORCE[0] = 2.0
+#dyn.body.POS_FORCE[1] = 0.0
+#dyn.body.POS_FORCE[2] = 0.0
+
+################################################################################
+# 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, 12)
+
+# ============================================================================ -
+# This tells the sim to stop after a few seconds
+# ============================================================================ -
+trick.stop(10.0)

trick_sims/SIM_singlerigidbody/S_define

@@ -0,0 +1,32 @@
+/************************TRICK HEADER*************************
+PURPOSE:
+    ( Single body rigid dynamics simulation )
+LIBRARY DEPENDENCIES:
+    ((src/body.cpp)
+     (src/body_shutdown.cpp)
+    )
+*************************************************************/
+#define TRICK_NO_MONTE_CARLO
+#define TRICK_NO_MASTERSLAVE
+#define TRICK_NO_INSTRUMENTATION
+#define TRICK_NO_REALTIMEINJECTOR
+#define TRICK_NO_ZEROCONF
+
+#include "sim_objects/default_trick_sys.sm"
+##include "include/body.hh"
+class BodySimObject : public Trick::SimObject {
+	public:
+	BODY body;
+
+	BodySimObject() {
+		("default_data")		body.default_data() ;
+		("initialization")			body.init() ;
+		("derivative")		       	body.derivative() ;
+		("integration") trick_ret = body.integ() ;
+		("shutdown")                    body.body_shutdown();
+	}
+};
+
+BodySimObject dyn;
+IntegLoop dyn_integloop(0.01) dyn;
+

trick_sims/SIM_singlerigidbody/S_overrides.mk

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

trick_sims/SIM_singlerigidbody/models/include/body.hh

@@ -0,0 +1,78 @@
+/*********************************************************************
+PURPOSE: (Simulate a single rigid body with six degrees of freedom)
+LIBRARY DEPENDENCIES: 
+	((SingleRigidBody/src/body.o))
+*********************************************************************/
+#ifndef BODY_HH
+#define BODY_HH
+
+class BODY {
+public: 
+	BODY();
+	~BODY();
+	//Variables
+
+	//Initializes state variables
+	double POS_INIT[3];
+	double VEL_INIT[3];
+	double ANGLE_INIT[3];
+	double OMEGA_INIT[3];
+
+	//State variables
+	double pos[3];
+	double vel[3];	
+	double angle[3];
+	double omega[3];
+
+	double acc[3];
+	double omegaDot[3];
+
+	double force[3];
+	double FORCE_INIT[3];
+	double force_remain[3];
+	double POS_FORCE[3];
+
+	double torque[3];
+	double TORQUE_INIT[3];
+	double torque_remain[3];
+
+	double inertia; 
+	double inertia_matrix[3][3];
+
+	double radius;
+	double mass; 
+	double mat_mass[6][6];
+	double massmatrix[3][3];
+
+	double CM[3]; 
+	double CM_skew[3][3];
+	double m_CM_skew[3][3];
+	double neg_m_CM_skew[3][3];
+
+	double vec_a[6];
+	double vec_b[6];
+
+	double rotate[3][3];
+	double angle_force[3];
+
+	double tmp_vec[6];
+	double **mat_mass_dyn;
+        double **mat_L;
+
+	// Methods
+	void default_data();
+	void init();
+	void derivative();
+	int integ();
+	int body_shutdown();
+
+	void rotation_matrix();
+	void calcforce();
+	void calctorque();
+	void calcforce_remain();
+	void calctorque_remain();
+	void eq_setup();
+	void eq_solver();
+};
+
+#endif