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;
00220
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 { };