Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_DynaMoSimulationQueries.cpp

00001 #include "SIGEL_Simulation/SIG_DynaMoSimulationQueries.h"
00002 #include <qdatetime.h>
00003 #include "matrix.h"
00004 #include "pointvector.h"
00005 #include "SIGEL_Simulation/SIG_DynaLink.h"
00006 #include "SIGEL_Simulation/SIG_Dyna.h"
00007 #include "SIGEL_Simulation/SIG_DynaSensor.h"
00008 #include <cmath>
00009 #include "NaN.h"
00010 #include "SIGEL_Tools/SIG_IO.h"
00011 #include "SIGEL_Simulation/SIG_SimulationCannotSolveException.h"
00012 
00013 SIGEL_Simulation::SIG_DynaMoSimulationQueries::SIG_DynaMoSimulationQueries(SIG_DynaMoSimulationData& theSimulationData)
00014  : simulationData(theSimulationData)
00015 { };  
00016 
00017 void SIGEL_Simulation::SIG_DynaMoSimulationQueries::sense(int sensorNo,QVector<SIG_Register> & registers) const
00018 {
00019  if (simulationData.dynaSystem.dynaSensors.size()==0) return;
00020  int modSensorNo=sensorNo % simulationData.dynaSystem.dynaSensors.size();
00021  SIG_DynaSensor sensor=simulationData.dynaSystem.getSensor(modSensorNo);
00022  registers[0]->loadValue(sensor.senseJoint1());
00023  if (sensor.joint->joint->getJointType()==SIGEL_Robot::SIG_Joint::tCylindricalJoint)
00024   registers[1]->loadValue(sensor.senseJoint2());
00025 };
00026 
00027 
00028 QTime SIGEL_Simulation::SIG_DynaMoSimulationQueries::getActualSimulationTime() const
00029 {
00030   double d=simulationData.simulationParameter.getStepSize()*simulationData.actualFrame*1000;
00031   QTime time(0,0,0,0);
00032   return time.addMSecs(static_cast<int>(floor(d)));
00033 };
00034 
00035 DL_vector SIGEL_Simulation::SIG_DynaMoSimulationQueries::getLinkPosition(int linkNo) const
00036 {
00037   SIG_DynaLink &theLink=simulationData.dynaSystem.getLink(linkNo);
00038   DL_vector pos;
00039   theLink.position.tovector(&pos);
00040   simulationData.dynaSystem.foundNaN=false;
00041   DL_vector pos2=simulationData.dynaSystem.unNaN(pos);
00042   if (!simulationData.dynaSystem.foundNaN)
00043    return pos2;
00044   else
00045    return simulationData.environment.getStartPosition();
00046 };
00047 
00048 DL_matrix SIGEL_Simulation::SIG_DynaMoSimulationQueries::getLinkOrientation(int linkNo) const
00049 {
00050   SIG_DynaLink &theLink=simulationData.dynaSystem.getLink(linkNo);
00051   DL_matrix orientation=theLink.orientation;
00052   return simulationData.dynaSystem.unNaN(orientation);
00053 };
00054 
00055 int SIGEL_Simulation::SIG_DynaMoSimulationQueries::getRootNumber() const
00056 {
00057   return simulationData.dynaSystem.rootLinkNo;
00058 };
00059 
00060 double SIGEL_Simulation::SIG_DynaMoSimulationQueries::getUsedForces() const
00061 {
00062   return 0; // has to be implemented properly!
00063   // klappt so in Dynamo nicht
00064 };
00065 
00066 int SIGEL_Simulation::SIG_DynaMoSimulationQueries::getLinkCount() const
00067 {
00068   return simulationData.dynaSystem.dynaLinks.size();  
00069 };
00070 
00071 void SIGEL_Simulation::SIG_DynaMoSimulationQueries::checkDynas() const
00072 {
00073   for(int i=0;i<getLinkCount();i++)
00074   {
00075    SIG_DynaLink &theLink=simulationData.dynaSystem.getLink(i);
00076    DL_vector pos;
00077    theLink.position.tovector(&pos);
00078    simulationData.dynaSystem.foundNaN=false;
00079    DL_vector pos2=simulationData.dynaSystem.unNaN(pos);
00080    if (simulationData.dynaSystem.foundNaN)
00081    { 
00082     QString msg("NaN Position found! ");
00083     msg+=theLink.link->getName();
00084     // throw SIG_SimulationCannotSolveException(__FILE__,__LINE__,msg);
00085     
00086    }; 
00087    DL_matrix orientation=theLink.orientation;
00088    simulationData.dynaSystem.foundNaN=false;
00089    DL_matrix orientation2=simulationData.dynaSystem.unNaN(orientation);
00090    if (simulationData.dynaSystem.foundNaN)
00091    { 
00092     QString msg("NaN Orientation found! ");
00093     msg+=theLink.link->getName();
00094     // throw SIG_SimulationCannotSolveException(__FILE__,__LINE__,msg);
00095 
00096    }; 
00097   };  
00098 };
00099 
00100 #ifdef SIG_DEBUG
00101 void SIGEL_Simulation::SIG_DynaMoSimulationQueries::printDynaDatas() const
00102 {
00103  int sz=getLinkCount();
00104  for (int i=0; i<sz; i++)
00105  {
00106   SIG_DynaLink &theLink=simulationData.dynaSystem.getLink(i);
00107   SIGEL_Tools::SIG_IO::cerr << "   --- Link " << i << " ------------------------\n";
00108   SIGEL_Tools::SIG_IO::cerr << theLink.link->getName() << "\n";
00109   SIGEL_Tools::SIG_IO::cerr << "Dyna: " << theLink.dyna << "\n";
00110   SIGEL_Tools::SIG_IO::cerr << "Masse: " << theLink.dyna->get_mass() << "\n";
00111   SIGEL_Tools::SIG_IO::cerr << "Inertia X:" << static_cast<SIG_Dyna*>(theLink.dyna)->get_inertiatensor()->x
00112                                << " Y:" << static_cast<SIG_Dyna*>(theLink.dyna)->get_inertiatensor()->y
00113                                << " Z:" << static_cast<SIG_Dyna*>(theLink.dyna)->get_inertiatensor()->z << "\n";
00114   SIGEL_Tools::SIG_IO::cerr << "Vel X:" << theLink.dyna->get_velocity()->x
00115                                << " Y:" << theLink.dyna->get_velocity()->y
00116                                << " Z:" << theLink.dyna->get_velocity()->z << "\n";
00117   SIGEL_Tools::SIG_IO::cerr << "AVel X:" << theLink.dyna->get_angvelocity()->x
00118                                 << " Y:" << theLink.dyna->get_angvelocity()->y
00119                                 << " Z:" << theLink.dyna->get_angvelocity()->z << "\n";
00120   SIGEL_Tools::SIG_IO::cerr << "Pos X:" << theLink.dyna->get_position()->x
00121                                << " Y:" << theLink.dyna->get_position()->y
00122                                << " Z:" << theLink.dyna->get_position()->z << "\n";
00123   SIGEL_Tools::SIG_IO::cerr << "Orient " << theLink.dyna->get_orientation()->c0.x
00124                                << " " << theLink.dyna->get_orientation()->c1.x
00125                                << " " << theLink.dyna->get_orientation()->c2.x << "\n";
00126   SIGEL_Tools::SIG_IO::cerr << " " << theLink.dyna->get_orientation()->c0.y
00127                                << " " << theLink.dyna->get_orientation()->c1.y
00128                                << " " << theLink.dyna->get_orientation()->c2.y << "\n";
00129   SIGEL_Tools::SIG_IO::cerr << " " << theLink.dyna->get_orientation()->c0.z
00130                                << " " << theLink.dyna->get_orientation()->c1.z
00131                                << " " << theLink.dyna->get_orientation()->c2.z << "\n";
00132   SIGEL_Tools::SIG_IO::cerr << "Next Vel X:" << theLink.dyna->get_next_velocity()->x
00133                                << " Y:" << theLink.dyna->get_next_velocity()->y
00134                                << " Z:" << theLink.dyna->get_next_velocity()->z << "\n";
00135   SIGEL_Tools::SIG_IO::cerr << "Next AVel X:" << theLink.dyna->get_next_angvelocity()->x
00136                                 << " Y:" << theLink.dyna->get_next_angvelocity()->y
00137                                 << " Z:" << theLink.dyna->get_next_angvelocity()->z << "\n";
00138   SIGEL_Tools::SIG_IO::cerr << "Next Pos X:" << theLink.dyna->get_next_position()->x
00139                                << " Y:" << theLink.dyna->get_next_position()->y
00140                                << " Z:" << theLink.dyna->get_next_position()->z << "\n";
00141  };
00142 };
00143 
00144 int SIGEL_Simulation::SIG_DynaMoSimulationQueries::getGPTestValue() const
00145 {
00146  return simulationData.gptestvalue;
00147 };
00148 #endif

Generated at Mon Sep 3 01:32:20 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000