//----------------------------------------------------------------------------- // File: NewtonsAppleForVisualization.cpp // Class: None // Parent: None // Children: None // Purpose: Simulates Newton's falling apple // Author: Paul Mitiguy - April 8, 2007 //----------------------------------------------------------------------------- #include "StandardCppHeadersAndNamespace.h" #include "UserForceControlHorizontal.h" #include "SimTKsimbody.h" using namespace SimTK; //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // Prototypes for local functions (functions not called by code in other files) //----------------------------------------------------------------------------- bool DoRequiredTasks( void ); bool WriteStringToFile( const char outputString[], FILE *fptr ) { return fputs( outputString, fptr ) != EOF; } 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 program failed bool programSucceeded = false; // It is a good programming practice to do little in the main function of a program. // The try-catch code in this main routine catches exceptions thrown by functions in the // try block, e.g., catching an exception that occurs when a NULL pointer is de-referenced. try { // Do the required programming tasks programSucceeded = DoRequiredTasks(); } // This catch statement handles certain types of exceptions catch( const exception &e ) { WriteStringToScreen( "\n\n Error: Programming error encountered.\n The exception thrown is: " ); WriteStringToScreen( e.what() ); } // The exception-declaration statement (...) handles any type of exception, // including C exceptions and system/application generated exceptions. // 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 Error: Programming error encountered.\n An unhandled exception was thrown." ); } // Give the user a chance to see a final message before exiting the program. WriteStringToScreen( programSucceeded ? "\n\n Program succeeded. Press Enter to finish: " : "\n\n Program failed. Press Enter to finish: " ); getchar(); // Keeps the screen displayed until the user presses the Enter (or Return) key. // 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 programSucceeded == true ? 0 : 1; } //----------------------------------------------------------------------------- bool DoRequiredTasks( void ) { // Declare a multibody system // Each multibody system contains at most one SimbodyMatterSubsytem (which inherits from MatterSubsystem - the ma in F=ma) // Each multibody system can contain 0, 1, 2, ... force subsystems (the F in F=ma) MultibodySystem mbs; SimbodyMatterSubsystem matterSubsystem; // Create a single matter sub-system (for the apple) mbs.setMatterSubsystem( matterSubsystem ); // Add the matter sub-system to the system. // 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 the mass, center of mass, and inertia properties for the apple const Real massOfApple = 0.142; // The location of the apple's center of mass is a vector from the apple's // origin expressed in the "x, y, z"' unit vectors fixed in the apple's frame. // Example: The vector(0,0,0) locates the apple's center of mass at the apple's origin. // Example: The vector(1,0,0) locates the apple's center of mass 1 unit in the "x" direction from the apple's origin. const Vec3 appleCenterOfMassLocation( 0, 0, 0 ); // Create the apple's inertia matrix about its origin for the "x, y, z" unit vectors fixed in the apple'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) // The following assumes an apple of radius 1.44 inches (3.6576 cm) with a radius of gyration of 0.91 inches (2.3114 cm) // which approximates a perfect sphere of radius 1.44 inches (3.6576 cm) const Real I = massOfApple * pow( 0.023114, 2 ); // Inertia = mass * radiusOfGyration^2 const Real Ixx = I, Iyy = I, Izz = I; const Real Ixy = 0, Ixz = 0, Iyz = 0; const Inertia appleInertiaMatrix( 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 apple, // it does not get associated with the apple until the addRigidBody method. MassProperties appleMassProperties( massOfApple, appleCenterOfMassLocation, appleInertiaMatrix ); // The Apple's motion relative to ground is specified by a "mobilizer" (sometimes called a "joint"). // The Mobilizer has an "Inboard Frame" fixed on the "Inboard Body" (Ground) // and an "Outboard Frame" fixed on the "Outboard Body" (Apple) // The Inboard Frame's axes are aligned with the Ground's axes. // The Inboard origin is coincident with the Ground's origin. const Rotation inboardFrameOrientationInGround; // Default constructor is identity matrix const Vec3 inboardFrameOriginLocationFromGroundOrigin( 0, 0, 0 ); const Transform inboardFrameTransformFromGround( inboardFrameOrientationInGround, inboardFrameOriginLocationFromGroundOrigin ); // The Outboard Frame's axes are aligned with the Apple's axes. // The Outboard origin is offset from the Apple's origin in the Apple's y-direction. const Rotation outboardFrameOrientationInApple; // Default constructor is identity matrix const Vec3 outboardFrameOriginLocationFromAppleOrigin( 0, 0, 0 ); const Transform outboardFrameTransformFromApple( outboardFrameOrientationInApple, outboardFrameOriginLocationFromAppleOrigin ); // There are many ways that the OutboardFrame can move relative to the InboardFrame. // The following allows translation along the Mobilizer's InboardFrame's "x,y,z" axes. Mobilizer AppleToGroundMobilizer = Mobilizer::Cartesian; const BodyId appleBodyId = matterSubsystem.addRigidBody( appleMassProperties, outboardFrameTransformFromApple, GroundId, inboardFrameTransformFromGround, AppleToGroundMobilizer ); // Add an air-resistance force sub-system to this multi-body system. GeneralForceElements userForceElements; mbs.addForceSubsystem( userForceElements ); // Although "new" was used to allocate this UserForce, do not "delete" it. // This bug will be fixed in the next version of Simbody so it can take an object from the stack or heap. // For now, addUserForce takes ownership of the allocated item and takes care of deleting it at the end. // UserForceCostHorizontal *sinusoidalForceOnApple = new UserForceCostHorizontal( appleBodyId ); // userForceElements.addUserForce( sinusoidalForceOnApple ); UserForceControlHorizontal *computedTorque = new UserForceControlHorizontal( appleBodyId, massOfApple, 1.0, 1.0 ); userForceElements.addUserForce( computedTorque ); // 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 (x,y,z) matterSubsystem.setMobilizerQ( s, appleBodyId, 0, 0.0 ); matterSubsystem.setMobilizerQ( s, appleBodyId, 1, 10.0 ); matterSubsystem.setMobilizerQ( s, appleBodyId, 2, 0.0 ); // Set the initial values for the motion variables matterSubsystem.setMobilizerU( s, appleBodyId, 0, 0.0 ); matterSubsystem.setMobilizerU( s, appleBodyId, 1, 0.0 ); matterSubsystem.setMobilizerU( s, appleBodyId, 2, 0.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( "ControlResults1p0.txt", "w" ); WriteStringToFile( "time xLocation xVelocity mechanicalEnergy xDifferenceFromExact\n", outputFile ); WriteStringToScreen( "time xLocation xVelocity mechanicalEnergy xDifferenceFromExact\n" ); // Visualize results with VTKReporter VTKReporter animationResults( mbs ); // For visualization purposes only, create a red sphere const Real radiusOfApple = 3.6576/100; // 1.44 inches (3.6576 cm) DecorativeSphere redSphere = DecorativeSphere( radiusOfApple ); redSphere.setColor(Red); // Can also specify a Vec3 with rgb which scale from 0 to 1 redSphere.setOpacity(0.0); // 0.0 is solid and 1.0 is transparent // Decorate the apple with the red sphere at the apple's origin const Rotation appleToRedSphereOrientation; //( 1,0,0, 0,1,0, 0,0,1 ); const Vec3 appleOriginToRedSphereOriginLocation( 0, 0, 0 ); const Transform appleToRedSphereTransform( appleToRedSphereOrientation, appleOriginToRedSphereOriginLocation ); animationResults.addDecoration( appleBodyId, appleToRedSphereTransform, redSphere ); // Set the numerical integration step and the time for the simulation to run const Real integrationStepDt = 0.01; const Real finalTime = 20.0; const Real finalTimeCompare = finalTime - 0.01*integrationStepDt; // Run the simulation and print the results while( 1 ) { // Query for results to be printed Real time = s.getTime(); Real kineticEnergy = mbs.getKineticEnergy(s); Real uniformGravitationalPotentialEnergy = mbs.getPotentialEnergy(s); Real mechanicalEnergy = kineticEnergy + uniformGravitationalPotentialEnergy; // Locate the apple origin's from the ground's origin, expressed in terms of the ground's "x,y,z" unit vectors. // Extract the apple's y-location from this vector. const Vec3 appleLocation = matterSubsystem.calcBodyOriginLocationInBody( s, appleBodyId, GroundId ); Real xLocation = appleLocation[0]; // Get the apple origin's velocity in ground, expressed in terms of the ground's "x,y,z" unit vectors. // Extract the apple's y-velocity from this vector. const Vec3 appleVelocity = matterSubsystem.calcBodyOriginVelocityInBody( s, appleBodyId, GroundId ); Real xVelocity = appleVelocity[0]; // Get the apple origin's acceleration in ground, expressed in terms of the ground's "x,y,z" unit vectors. // Extract the apple's y-acceleration from this vector. // const Vec3 appleAcceleration = matterSubsystem.calcBodyOriginAccelerationInBody( s, appleBodyId, GroundId ); // Real yAcceleration = appleAcceleration[1]; // Exact analytical results and difference of numerical integration results Real xExact = -cos(time); Real xDifferenceFromExact = xExact - xLocation; // Print results to screen WriteDoubleToFile( time, 2, stdout ); WriteDoubleToFile( xLocation, 4, stdout ); WriteDoubleToFile( xVelocity, 4, stdout ); WriteDoubleToFile( mechanicalEnergy, 7, stdout ); WriteDoubleToFile( xDifferenceFromExact, 7, stdout ); WriteStringToScreen( "\n" ); // Print results to file WriteDoubleToFile( time, 2, outputFile ); WriteDoubleToFile( xLocation, 4, outputFile ); WriteDoubleToFile( xVelocity, 4, outputFile ); WriteDoubleToFile( mechanicalEnergy, 7, outputFile ); WriteDoubleToFile( xDifferenceFromExact, 7, outputFile ); WriteStringToFile( "\n", outputFile ); // Animate the results for this step animationResults.report(s); // Check if integration has completed if( time >= finalTimeCompare ) break; // Increment time step myStudy.step( time + integrationStepDt ); } // 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; }