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

SIG_DynaMechsSimulationQueries.cpp

00001 #include "SIGEL_Simulation/SIG_DynaMechsSimulationQueries.h"
00002 
00003 #include <qdatetime.h>
00004 #include "matrix.h"
00005 #include "pointvector.h"
00006 #include <cmath>
00007 #include <cfloat>
00008 
00009 #include "SIGEL_Robot/SIG_JointSensor.h"
00010 #include "SIGEL_Tools/SIG_IO.h"
00011 #include "SIGEL_Simulation/SIG_SimulationCannotSolveException.h"
00012 #include "SIGEL_Tools/SIG_TypeConverter.h"
00013 
00014 #include <dm.h>
00015 
00016 using namespace SIGEL_Tools;
00017 
00018 SIGEL_Simulation::SIG_DynaMechsSimulationQueries::SIG_DynaMechsSimulationQueries(SIG_DynaMechsSimulationData& theSimulationData)
00019   : simulationData(theSimulationData)
00020 { };  
00021 
00022 void SIGEL_Simulation::SIG_DynaMechsSimulationQueries::sense(int sensorNo,
00023                                                              QVector<SIG_Register> & registers) const
00024 {
00025   SIGEL_Robot::SIG_Sensor *sensor = 0;
00026 
00027   if (simulationData.sensors.size() > 0)
00028     {
00029       long double minRegisterValue = std::pow( static_cast<long double>(2),
00030                                                static_cast<long double>(registers[0]->getSize() - 1) );
00031 
00032       int absoluteSensorNo = sensorNo + static_cast< int >(minRegisterValue);
00033 
00034       int sensorIndex = absoluteSensorNo % simulationData.sensors.size();
00035 
00036       sensor = simulationData.sensors[ sensorIndex ];
00037     };
00038 
00039   if (sensor)
00040     {
00041       SIGEL_Robot::SIG_JointSensor *jointSensor = static_cast< SIGEL_Robot::SIG_JointSensor* >(sensor);
00042 
00043       SIGEL_Robot::SIG_Joint const *joint = jointSensor->getJoint();
00044 
00045       int linkNumber = simulationData.jointIndices[ joint->getNumber() ];
00046 
00047       SIG_DynaMechsLink *dynaMechsLink = simulationData.dynaMechsLinks[ linkNumber ];
00048 
00049       dmLink *internalLink = dynaMechsLink->dynaMechsLink;
00050 
00051       double q;
00052       double qd;
00053 
00054       internalLink->getState( &q, &qd );
00055 
00056       double minPos = joint->getMechsMinPos();
00057       double maxPos = joint->getMechsMaxPos();
00058 
00059       double posRange;
00060 
00061       if ( (minPos == (- DBL_MAX)) && (maxPos == DBL_MAX) )
00062         posRange = 1;
00063       else
00064         {
00065           posRange = maxPos - minPos;
00066 
00067           if (q > maxPos)
00068             q = maxPos;
00069           else if (q < minPos)
00070             q = minPos;
00071         };
00072 
00073       double scaledState = (q - minPos) / posRange;
00074 
00075       long double minRegisterValue = - std::pow( static_cast<long double>(2),
00076                                                  static_cast<long double>(registers[0]->getSize() - 1) );
00077 
00078       long double registerValueRange = - minRegisterValue * 2;
00079 
00080       int registerValue = static_cast< int >( scaledState * registerValueRange + minRegisterValue );
00081 
00082 #ifdef SIG_DEBUG
00083       SIGEL_Tools::SIG_IO::cerr << "Reading sensor value from sensor "
00084                                 << sensor->getName()
00085                                 << " at joint "
00086                                 << joint->getName()
00087                                 << " corresponding to link "
00088                                 << linkNumber
00089                                 << ": "
00090                                 << dynaMechsLink->link->getName()
00091                                 << "\n"
00092                                 << "minPos: "
00093                                 << minPos
00094                                 << ", maxPos: "
00095                                 << maxPos
00096                                 << ", posRange: "
00097                                 << posRange
00098                                 << "\n"
00099                                 << "minRegisterValue: "
00100                                 << static_cast< double >(minRegisterValue)
00101                                 << "\n"
00102                                 << "registerValueRange: "
00103                                 << static_cast< double >(registerValueRange)
00104                                 << "\n"
00105                                 << "state: "
00106                                 << q
00107                                 << ", scaledState: "
00108                                 << scaledState
00109                                 << ", registerValue: "
00110                                 << registerValue
00111                                 << "\n";
00112 #endif
00113 
00114       registers[0]->loadValue( registerValue );
00115     };
00116 };
00117 
00118 QTime SIGEL_Simulation::SIG_DynaMechsSimulationQueries::getActualSimulationTime() const
00119 {
00120   int secs = static_cast< int >(simulationData.actualFrame * simulationData.simulationParameter.getStepSize());
00121 
00122   QTime time = QTime().addSecs( secs );
00123 
00124   return time;
00125 };
00126 
00127 DL_vector SIGEL_Simulation::SIG_DynaMechsSimulationQueries::getLinkPosition(int linkNo) const
00128 {
00129   SIG_DynaMechsLink *dynaMechsLink = simulationData.dynaMechsLinks[ linkNo ];
00130 
00131   if (dynaMechsLink)
00132     {
00133       NEWMAT::ColumnVector position =   SIG_TypeConverter::dynaMechsToSigel()
00134                                       * dynaMechsLink->transformation.SubMatrix( 1, 3, 4, 4 );
00135 
00136 #ifdef SIG_DEBUG
00137       SIGEL_Tools::SIG_IO::cerr << "Position of link "
00138                                 << dynaMechsLink->link->getName()
00139                                 << ":";
00140       for (int i=1; i<=3; i++)
00141         SIGEL_Tools::SIG_IO::cerr << " "
00142                                   << dynaMechsLink->transformation( i, 4 );
00143       SIGEL_Tools::SIG_IO::cerr << "\n";
00144 
00145       SIGEL_Tools::SIG_IO::cerr << "Position of link (DynaMechs forward kinematics) "
00146                                 << dynaMechsLink->link->getName()
00147                                 << ", internal number "
00148                                 << dynaMechsLink->dynaMechsLinkNumber
00149                                 << ":";
00150       dmABForKinStruct const *forKinStruct = simulationData.dynaMechsSystem.getForKinStruct( dynaMechsLink->dynaMechsLinkNumber );
00151       NEWMAT::ColumnVector dynaMechsPosition = SIG_TypeConverter::toColumnVector( forKinStruct->p_ICS );
00152       for (int i=1; i<=3; i++)
00153         SIGEL_Tools::SIG_IO::cerr << " "
00154                                   << dynaMechsPosition( i );
00155       SIGEL_Tools::SIG_IO::cerr << "\n";
00156 #endif
00157 
00158       return SIG_TypeConverter::toDL_vector( position );
00159     }
00160   else
00161     {
00162       DL_vector position(0, 0, 0);
00163 
00164       return position;
00165     };
00166 };
00167 
00168 DL_matrix SIGEL_Simulation::SIG_DynaMechsSimulationQueries::getLinkOrientation(int linkNo) const
00169 {
00170   SIG_DynaMechsLink *dynaMechsLink = simulationData.dynaMechsLinks[ linkNo ];
00171 
00172   if (dynaMechsLink)
00173     {
00174       NEWMAT::Matrix orientation =   SIG_TypeConverter::dynaMechsToSigel()
00175                                    * dynaMechsLink->transformation.SubMatrix( 1, 3, 1, 3 );
00176 
00177 #ifdef SIG_DEBUG
00178       SIGEL_Tools::SIG_IO::cerr << "Orientation of link "
00179                                 << dynaMechsLink->link->getName()
00180                                 << ":";
00181       for (int i=1; i<=3; i++)
00182         for (int j=1; j<=3; j++)
00183           SIGEL_Tools::SIG_IO::cerr << " "
00184                                     << dynaMechsLink->transformation( i, j );
00185       SIGEL_Tools::SIG_IO::cerr << "\n";
00186 
00187       SIGEL_Tools::SIG_IO::cerr << "Orientation of link (DynaMechs forward kinematics) "
00188                                 << dynaMechsLink->link->getName()
00189                                 << ", internal number "
00190                                 << dynaMechsLink->dynaMechsLinkNumber
00191                                 << ":";
00192       dmABForKinStruct const *forKinStruct = simulationData.dynaMechsSystem.getForKinStruct( dynaMechsLink->dynaMechsLinkNumber );
00193       NEWMAT::Matrix dynaMechsOrientation = SIG_TypeConverter::toMatrix( forKinStruct->R_ICS );
00194       for (int i=1; i<=3; i++)
00195         for (int j=1; j<=3; j++)
00196           SIGEL_Tools::SIG_IO::cerr << " "
00197                                     << dynaMechsOrientation( i, j );
00198       SIGEL_Tools::SIG_IO::cerr << "\n";
00199 #endif
00200 
00201       return SIG_TypeConverter::toDL_matrix( orientation );
00202     }
00203   else
00204     {
00205       DL_matrix orientation;
00206       orientation.makeone();
00207 
00208       return orientation;
00209     };
00210 };
00211 
00212 int SIGEL_Simulation::SIG_DynaMechsSimulationQueries::getRootNumber() const
00213 {
00214   return simulationData.robot.getRootLink()->getNumber();
00215 };
00216 
00217 double SIGEL_Simulation::SIG_DynaMechsSimulationQueries::getUsedForces() const
00218 {
00219   return 0; // has to be implemented properly!
00220   // klappt so in Dynamo nicht
00221 };
00222 
00223 int SIGEL_Simulation::SIG_DynaMechsSimulationQueries::getLinkCount() const
00224 {
00225   return simulationData.robot.getLinkIter().count();
00226 };
00227 
00228 void SIGEL_Simulation::SIG_DynaMechsSimulationQueries::checkDynas() const
00229 { };

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