//----------------------------------------------------------------------------- // File: SpinningBook.cpp // Class: None // Parent: None // Children: None // Purpose: Simulates a spinning book about principal axes //----------------------------------------------------------------------------- // The following are standard C/C++ header files. // If a filename is enclosed inside < > it means the header file is in the Include directory. // If a filename is enclosed inside " " it means the header file is in the current directory. #include // Character Types #include // Mathematical Constants #include // Variable Argument Lists #include // Standard Input/Output Functions #include // Utility Functions #include // String Operations #include // Signals (Contol-C + Unix System Calls) #include // Nonlocal Goto (For Control-C) #include // Time and Date information #include // Verify Program Assertion #include // Error Codes (Used in Unix system()) #include // Floating Point Constants #include // Implementation Constants #include // Standard Definitions #include // Exception handling (e.g., try, catch throw) #include "SimTKsimbody.h" using namespace SimTK; using namespace std; //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // Prototypes for local functions (functions not called by code in other files) //----------------------------------------------------------------------------- bool SimulateSpinningBook( void ); bool WriteStringToFile( const char outputString[], FILE *fptr ) { return fputs( outputString, fptr ) != 0; } bool WriteStringToScreen( const char outputString[] ) { return WriteStringToFile( outputString, stdout ); } bool WriteDoubleToFile( double x, int precision, FILE *fptr ); FILE* FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute ); //----------------------------------------------------------------------------- // The executable program starts here //----------------------------------------------------------------------------- int main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] ) { // Default value is simulation failed bool simulationSucceeded = false; try { // Simulate the multibody system simulationSucceeded = SimulateSpinningBook(); } // This catch statement handles certain types of expections catch( const exception &e ) { WriteStringToScreen("\n\nError: Programming error encountered. \n The exception thrown is: " ); WriteStringToScreen( e.what() ); } // The exception-declaration statement (...) handles any types of excpetion, // including C exceptions an system/application generated exception. // This includes exceptions such as memory protection and floating-point violations. // An ellipsis catch handler must be the last handler for its try block. catch( ... ) { WriteStringToScreen( "\n\n Errorr: Programming error encountered. \n An unhandled expection was thrown." ); } // Give the user a chance to see a final program message before exiting the program WriteStringToScreen(simulationSucceeded ? "\n\n Simulatin succeeded. Press Enter to terminate the program: " : "\n\nSimulation failed. Press Enter to terminate the program: " ); // Keep the screen displayed until the user presses the Enter (or Return) key. getchar(); // The value returned by the main function is the exit status of the program. // A normal program exit returns 0 (other return values usually signal an error). return simulationSucceeded == true ? 0 : 1; } //----------------------------------------------------------------------------- bool SimulateSpinningBook( void ) { // Declare a multibody system (contains one or more force and matter sub-systems) MultibodySystem mbs; // Gravity is a force that is acting on the book if the book is spinning near the surface of the earth, // but due to the assumption that the book is of an uniform material, I can assume gravity // is acting at the center of mass. When calculating the Moment of all the forces acting on the book B, I am choosing to due so about // the center of mass. Therefore because the moment arm ( i.e. position vector r_cm/cm) equals zero, there is no moment due to gravity. // I.e. Left-hand side of M=Hdot equals zero. Therefore I am not initializing it in my code. // 0. The ground's right-handed, orthogonal x,y,z unit vectors are directed with x horizontally right and y vertically upward. // 1. Create a gravity vector that is straight down (in the ground's frame) // 2. Create a uniform gravity sub-system // 3. Add the gravity sub-system to the multibody system // Vec3 gravityVector( 0, -9.8, 0 ); // UniformGravitySubsystem gravity( gravityVector ); // mbs.addForceSubsystem( gravity ); // Create a matter sub-system (the book) SimbodyMatterSubsystem book; // Create the mass, center of mass, and inertia properties for the book const Real massOfBook = 0.4; // The location of the book's center of mass is a vector from the book's // origin expressed in the "x, y, z"' unit vectors fixed in the book's frame. // Example: The vector(0,0,0) locates the book's center of mass at the book's origin. // Example: The vector(1,0,0) locates the book's center of mass 1 unit in the "x" direction from the book's origin. const Vec3 bookCenterOfMassLocation( 0, 0, 0 ); // Create the book's inertia matrix about its origin for the "x, y, z" unit vectors fixed in the book's frame. // Note: The 3x3 inertia matrix is symmetric - so only 6 elements need to be defined. // Ixx, Iyy, Izz are moments of inertia ( diagonal terms in the matrix) // Ixy, Ixz, Iyz are products of inertia (off-diagonal terms in the matrix) const Real Lx = 20.0/100; const Real Ly = 30.0/100; const Real Lz = 5.0/100; const Real Ixx = massOfBook/12 * ( Ly*Ly + Lz*Lz ); const Real Iyy = massOfBook/12 * ( Lx*Lx + Lz*Lz ); const Real Izz = massOfBook/12 * ( Lx*Lx + Ly*Ly ); const Real Ixy = 0, Ixz = 0, Iyz = 0; const Inertia bookInertiaMatrix( Ixx, Iyy, Izz, Ixy, Ixz, Iyz ); // The MassProperties class holds the mass, center of mass, and inertia properties of a rigid body. // Although the next line creates an instance of the MassProperties class for the book, // it does not get associated with the book until the addRigidBody method. MassProperties bookMassProperties( massOfBook, bookCenterOfMassLocation, bookInertiaMatrix ); // The motion of the book is related to the motion of the ground via "mobilizers" // The "mobilizers" specify the allowable motion of the book to the ground. // More specifically, the motion of an "outboard body" (e.g., the book) // to its inboard body (e.g., the ground) is specified by first constructing: // 1. An "outboard frame" on the ground (which hooks to the "outboard body" - the book ) // 2. An "inboard frame" on the book (which hooks to the "inboard body" - the ground) // The orientation and position of the outboard frame from the ground's frame is specified below. // The outboard frame's axes are aligned with the ground's axes and its origin is coincident with the ground's origin. // In other words, for this simple problem the outboard frame and the ground frame are identical. const Transform outboardFrameTransformFromGround; // The default constructor is the identity transform // The orientation and position of the inboard frame from the book's frame is specified below. // The inboard frame's axes are aligned with the book's axes and its origin is coincident with the book's origin // In other words, for this simple problem the inboard frame and the book frame are identical. // Although the inboard frame can be constructed in a simple manner analogous to the outboardFrameTransformFromGround (above) // it is worthwhile to look at the details of the rotation matrix and position vector in the transform: // a. The rotation matrix relating the InboardFrame's x,y,z axes to the BookFrame's x,y,z axes is specified InboardFrame_BookFrame. // b. The position of the InboardFrame's origin from the BookFrame origin, expressed in terms of the BookFrame's "x, y, z" unit vectors. const Rotation inboardFrameOrientationInBook; // ( 1,0,0, 0,1,0, 0,0,1 ); const Vec3 inboardFrameOriginLocationFromBookOrigin( 0, 0, 0 ); const Transform inboardFrameTransformFromBook( inboardFrameOrientationInBook, inboardFrameOriginLocationFromBookOrigin ); // There are many ways that the book can move relative to the ground. // The following allows the book to move in "x", "y", and "z" directions. // Another option producing the same result is Mobilizer::Free Mobilizer bookToGroundMobilizer = Mobilizer::Free; const BodyId bookBodyId = book.addRigidBody( bookMassProperties, inboardFrameTransformFromBook, GroundId, outboardFrameTransformFromGround, bookToGroundMobilizer ); // Add the matter (book) sub-system to the system. mbs.setMatterSubsystem( book ); // Create a state for this system. // Define appropriate states for this multi-body system. // Set the initial time to 0.0 State s; mbs.realize( s ); s.setTime( 0.0 ); // Set the initial values for the configuration variables (q0,q1,q2,q3,x,y,z) //For the spinning book problem the configuration variables are all = 0 expect q0=1 book.setMobilizerQ( s, bookBodyId, 0, 1.0 ); book.setMobilizerQ( s, bookBodyId, 1, 0.0 ); book.setMobilizerQ( s, bookBodyId, 2, 0.0 ); book.setMobilizerQ( s, bookBodyId, 3, 0.0 ); book.setMobilizerQ( s, bookBodyId, 4, 0.0 ); book.setMobilizerQ( s, bookBodyId, 5, 0.0 ); book.setMobilizerQ( s, bookBodyId, 6, 0.0 ); // Set the initial values for the motion variables (wx, wy,wz, x0dot, y0cot, z0dot) //The translational motion variables are equal to zero and the rotational motion variables are listed below: //Real wx=0.2, wy=7.0, wz=0.2; Real wx=7.0, wy=0.2, wz=0.2; //Real wx=0.2, wy=0.2, wz=7.0; book.setMobilizerU( s, bookBodyId, 0, wx ); book.setMobilizerU( s, bookBodyId, 1, wy ); book.setMobilizerU( s, bookBodyId, 2, wz ); book.setMobilizerU( s, bookBodyId, 3, 0 ); book.setMobilizerU( s, bookBodyId, 4, 0 ); book.setMobilizerU( s, bookBodyId, 5, 0 ); // Create a study using the Runge Kutta Merson integrator (alternately use the CPodesIntegrator) RungeKuttaMerson myStudy( mbs, s ); // Set the numerical accuracy for the integrator myStudy.setAccuracy( 1.0E-7 ); // The next statement does lots of accounting myStudy.initialize(); // Open a file to record the simulation results (they are also displayed on screen) FILE *outputFile = FileOpenWithMessageIfCannotOpen( "SpinningBookForPlottingResults.txt", "w" ); WriteStringToFile( " time wx wy wz x y z Hx Hy Hz kineticEnergy\n", outputFile ); WriteStringToScreen( " time wx wy wz x y z Hx Hy Hz kineticEnergy\n" ); // Visualize results with VTKReporter VTKReporter animationResults( mbs ); // For visualization purposes only, create a red brickk with a Lx = 20/100, Ly = 30/100, Lz = 5/100 measured in meters // Construct vector of half lengths for next function; required as input. const Vec3 dimOfBookHalfLengths = 0.5*Vec3(Lx,Ly,Lz); DecorativeBrick redBrick = DecorativeBrick(dimOfBookHalfLengths); redBrick.setColor(Red); // Can also specify a Vec3 with rgb which scale from 0 to 1 redBrick.setOpacity(0.0); // 0.0 is solid and 1.0 is transparent // Decorate the book with the red brick at the book's origin const Rotation bookToRedSphereOrientation;// (1,0,0, 0,1,0, 0,0,1 ); const Vec3 bookOriginToRedSphereOriginLocation( 0, 0, 0 ); const Transform bookToRedSphereTransform( bookToRedSphereOrientation, bookOriginToRedSphereOriginLocation ); animationResults.addDecoration( bookBodyId, bookToRedSphereTransform, redBrick ); // Set the numerical integration step and the time for the simulation to run const Real dt = 0.01; const Real finalTime = 4.0; // Run the simulation and print the results while( 1 ) { // Query for results to be printed Real time = s.getTime(); Real kineticEnergy = mbs.getKineticEnergy(s); // Locate the book origin's (center of mass) from the ground's origin, expressed in terms of the ground's "x,y,z" unit vectors. // Extract the book's x,y,z-location from this vector. const Vec3 bookLocation = book.calcBodyOriginLocationInBody( s, bookBodyId, GroundId ); Real yLocation = bookLocation[1]; Real xLocation = bookLocation[0]; Real zLocation = bookLocation[2]; // Get the book's angular velocity in ground, expressed in terms of the ground's "x,y,z" unit vectors. // Return the angular velocity w_G_B of body B's (book)frame in body G's frame, expressed in ground. const Vec3 bookAngVel_G_B = book.calcBodyAngularVelocityInBody( s, bookBodyId, GroundId ); // a. Return R_GB, the rotation matrix to book B's x,y,z axes from ground's G's x,y,z axes. // b. Return R_BG, the roation matrix to ground G's x,y,z axes, to B's x,y,z axes. const Rotation groundToBookRotationMatrix = book.getBodyRotation( s, bookBodyId ); const Rotation InverseRotationBookToGroundRotationMatrix = groundToBookRotationMatrix.invert(); //The angular velocities (wx,wy,wz) expressed in the book's unit vectors is needed //to calcualte the angular mementum of the book in the B's x,y,z unit vectors. //a. expressing wx,wy,wz that are currently epressed in the ground's "x,y,z" unit vectors (bookAngVel_G_B) //b. Extract the book's wx, wy, wz from this vector const Vec3 bookAngVel_B_G = InverseRotationBookToGroundRotationMatrix*bookAngVel_G_B; Real bookWx_B_G = bookAngVel_B_G[0]; Real bookWy_B_G = bookAngVel_B_G[1]; Real bookWz_B_G = bookAngVel_B_G[2]; //Calculating Angular Momentum for book wrt ground expressed in ground Real bookHx=Ixx*bookWx_B_G; Real bookHy=Iyy*bookWy_B_G; Real bookHz=Izz*bookWz_B_G; // Exact analytical results and difference of numerical integration results //double yExact = 44.7*sin(angleInRadians)*time - 4.9*time*time; //double yDifferenceFromExact = yExact - yLocation; // double xExact = 44.7*cos(angleInRadians)*time; // double xDifferenceFromExact = xExact -xLocation; // Print results to screen WriteDoubleToFile( time, 2, stdout ); WriteDoubleToFile( bookWx_B_G, 2, stdout ); WriteDoubleToFile( bookWy_B_G , 4, stdout ); WriteDoubleToFile( bookWz_B_G , 4, stdout ); WriteDoubleToFile( xLocation, 4, stdout ); WriteDoubleToFile( yLocation, 4, stdout ); WriteDoubleToFile( zLocation, 4, stdout ); WriteDoubleToFile( bookHx, 4, stdout ); WriteDoubleToFile( bookHy, 4, stdout ); WriteDoubleToFile( bookHz, 4, stdout ); WriteDoubleToFile( kineticEnergy, 7, stdout ); WriteStringToScreen( "\n" ); // Print results to file WriteDoubleToFile( time, 2, outputFile ); WriteDoubleToFile( bookWx_B_G, 2, outputFile ); WriteDoubleToFile( bookWy_B_G , 4, outputFile ); WriteDoubleToFile( bookWz_B_G , 4, outputFile ); WriteDoubleToFile( xLocation, 4, outputFile ); WriteDoubleToFile( yLocation, 4, outputFile ); WriteDoubleToFile( zLocation, 4, outputFile ); WriteDoubleToFile( bookHx, 4, outputFile ); WriteDoubleToFile( bookHy, 4, outputFile ); WriteDoubleToFile( bookHz, 4, outputFile ); WriteDoubleToFile( kineticEnergy, 7, outputFile ); WriteStringToFile( "\n", outputFile ); // Animate the results for this step animationResults.report(s); // Check if integration has completed if( time >= finalTime ) break; // Increment time step myStudy.step( time + dt); } // Simulation completed properly return true; } //----------------------------------------------------------------------------- FILE* FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute ) { // Try to open the file FILE *Fptr1 = fopen( filename, attribute ); // If unable to open the file, issue a message if( !Fptr1 ) { WriteStringToScreen( "\n\n Unable to open the file: " ); WriteStringToScreen( filename ); WriteStringToScreen( "\n\n" ); } return Fptr1; } //----------------------------------------------------------------------------- bool WriteDoubleToFile( double x, int precision, FILE *fptr ) { // Ensure the precision (number of digits in the mantissa after the decimal point) makes sense. // Next, calculate the field width so it includes one extra space to the right of the number. if( precision < 0 || precision > 17 ) precision = 5; int fieldWidth = precision + 8; // Create the format specifier and print the number char format[20]; sprintf( format, " %%- %d.%dE", fieldWidth, precision ); return fprintf( fptr, format, x ) >= 0; }