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;
00063
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
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
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