00001 #include "SIGEL_Simulation/SIG_Simulation.h"
00002
00003 #include "SIGEL_Simulation/SIG_DynaMoSimulationData.h"
00004 #include "SIGEL_Simulation/SIG_DynaMechsSimulationData.h"
00005 #include "SIGEL_Simulation/SIG_DynaMoSimulationQueries.h"
00006 #include "SIGEL_Simulation/SIG_DynaMechsSimulationQueries.h"
00007 #include "SIGEL_Simulation/SIG_DynaMoCommandInterface.h"
00008 #include "SIGEL_Simulation/SIG_DynaMechsCommandInterface.h"
00009 #include "SIGEL_Tools/SIG_IO.h"
00010
00011 SIGEL_Simulation::SIG_Simulation::SIG_Simulation(SIGEL_Robot::SIG_Robot const & robot,
00012 SIGEL_Environment::SIG_Environment const & environment,
00013 SIGEL_Program::SIG_Program const & robotProgram,
00014 SIG_SimulationParameters const & simulationParameter,
00015 SIG_Recorder & theRecorder) :
00016 recorder(theRecorder),
00017 stopSimulation(false),
00018 QObject()
00019 {
00020 switch (simulationParameter.getSimulationLibrary())
00021 {
00022 case SIG_SimulationParameters::DynaMo:
00023 {
00024 SIG_DynaMoSimulationData *dynaMoSimulationData = new SIG_DynaMoSimulationData( robot,
00025 environment,
00026 simulationParameter );
00027
00028 simulationData = dynaMoSimulationData;
00029 simulationQueries = new SIG_DynaMoSimulationQueries( *dynaMoSimulationData );
00030 commandInterface = new SIG_DynaMoCommandInterface( *dynaMoSimulationData );
00031
00032 #ifdef SIG_DEBUG
00033
00034 #endif
00035 connect( &dynaMoSimulationData->dynaSystem,
00036 SIGNAL(signalDynamoMessage(QString)),
00037 SLOT(slotDynamoMessage(QString)) );
00038 };
00039 break;
00040 case SIG_SimulationParameters::DynaMechs:
00041 {
00042 SIG_DynaMechsSimulationData *dynaMechsSimulationData = new SIG_DynaMechsSimulationData( robot,
00043 environment,
00044 simulationParameter );
00045
00046 simulationData = dynaMechsSimulationData;
00047 simulationQueries = new SIG_DynaMechsSimulationQueries( *dynaMechsSimulationData );
00048 commandInterface = new SIG_DynaMechsCommandInterface( *dynaMechsSimulationData );
00049 };
00050 break;
00051 };
00052
00053 recorder.setSimulationQueries( *simulationQueries );
00054 recorder.init();
00055
00056 interpreter = new SIG_Interpreter( *robot.getLangParam(),
00057 robotProgram,
00058 *commandInterface,
00059 *simulationQueries );
00060
00061 };
00062
00063 void SIGEL_Simulation::SIG_Simulation::start()
00064 throw(SIGEL_Simulation::SIG_SimulationCannotSolveException)
00065 {
00066 QTime max=simulationData->simulationParameter.getTimeToSimulate();
00067 QTime act=simulationQueries->getActualSimulationTime();
00068
00069 do
00070 {
00071 makeTimeSteps(1);
00072 act=simulationQueries->getActualSimulationTime();
00073 }
00074 while (act<max);
00075
00076 recorder.finish();
00077 };
00078
00079 void SIGEL_Simulation::SIG_Simulation::makeTimeSteps(int numTimeSteps)
00080 throw(SIGEL_Simulation::SIG_SimulationCannotSolveException)
00081 {
00082 for(int i=0;i<numTimeSteps;i++)
00083 {
00084 interpreter->interprete( simulationData->simulationParameter.getStepSize() );
00085
00086 simulationData->setNewFrame( true );
00087 simulationData->simulationProgress();
00088
00089 #ifdef SIG_DEBUG
00090
00091 #endif
00092 simulationQueries->checkDynas();
00093
00094 if (stopSimulation)
00095 throw SIG_SimulationCannotSolveException( __FILE__,
00096 __LINE__,
00097 "Dynamo produced an Cannot Solve Constraints Error" );
00098
00099 simulationData->actualFrame++;
00100
00101 recorder.record();
00102 };
00103 };
00104
00105 void SIGEL_Simulation::SIG_Simulation::slotDynamoMessage(QString theMessage)
00106 {
00107
00108 #ifdef SIG_DEBUG
00109 SIGEL_Tools::SIG_IO::cout << theMessage;
00110 #endif
00111
00112 stopSimulation=true;
00113 };
00114
00115