LQRControler::LQRControler() {
trajectory=new ReferenceTrajectory();
Ke=Gain(4);
deltaxsiant.setZero();
xsiant.setZero();
ts=0.012;
}
void init_opencv_fcns()
{
cout << "Initializing OpenCV Module" << endl;
img_dbg.setTo(white);
pt_img_temp.setZero();
cout << "Completed Initializing OpenCV Module"<<endl<<endl;
}
bool GUSBAmpDriver::getSampleMatrixValue(MatrixXf& sampleMatrix)
{
sampleMatrix.setZero(); // Clear matrix - set all elements to zero
return true;
}
bool TMSIDriver::getSampleMatrixValue(MatrixXf& sampleMatrix)
{
//Check if the driver DLL was loaded
if(!m_bDllLoaded)
return false;
//Check if device was initialised and connected correctly
if(!m_bInitDeviceSuccess)
{
cout << "Plugin TMSI - ERROR - getSampleMatrixValue() - Cannot start to get samples from device because device was not initialised correctly" << endl;
return false;
}
sampleMatrix.setZero(); // Clear matrix - set all elements to zero
uint iSamplesWrittenToMatrix = 0;
int channelMax = 0;
int sampleMax = 0;
int sampleIterator = 0;
//get samples from device until the complete matrix is filled, i.e. the samples per block size is met
while(iSamplesWrittenToMatrix < m_uiSamplesPerBlock)
{
//Get sample block from device
LONG ulSizeSamples = m_oFpGetSamples(m_HandleMaster, (PULONG)m_lSignalBuffer, m_lSignalBufferSize);
LONG ulNumSamplesReceived = ulSizeSamples/(m_uiNumberOfAvailableChannels*4);
//Only do the next steps if there was at least one sample received, otherwise skip and wait until at least one sample was received
if(ulNumSamplesReceived > 0)
{
int actualSamplesWritten = 0; //Holds the number of samples which are actually written to the matrix in this while procedure
//Write the received samples to an extra buffer, so that they are not getting lost if too many samples were received. These are then written to the next matrix (block)
for(int i=0; i<ulNumSamplesReceived; i++)
{
for(uint j=i*m_uiNumberOfAvailableChannels; j<(i*m_uiNumberOfAvailableChannels)+m_uiNumberOfChannels; j++)
m_vSampleBlockBuffer.push_back((double)m_lSignalBuffer[j]);
}
//If the number of available channels is smaller than the number defined by the user -> set the channelMax to the smaller number
if(m_uiNumberOfAvailableChannels < m_uiNumberOfChannels)
channelMax = m_uiNumberOfAvailableChannels;
else
channelMax = m_uiNumberOfChannels;
//If the number of the samples which were already written to the matrix plus the last received number of samples is larger then the defined block size
//-> only fill until the matrix is completeley filled with samples. The other (unused) samples are still stored in the vector buffer m_vSampleBlockBuffer and will be used in the next matrix which is to be sent to the circular buffer
if(iSamplesWrittenToMatrix + ulNumSamplesReceived > m_uiSamplesPerBlock)
sampleMax = m_uiSamplesPerBlock - iSamplesWrittenToMatrix + sampleIterator;
else
sampleMax = ulNumSamplesReceived + sampleIterator;
//Read the needed number of samples from the vector buffer to store them in the matrix
for(; sampleIterator < sampleMax; sampleIterator++)
{
for(int channelIterator = 0; channelIterator < channelMax; channelIterator++)
{
sampleMatrix(channelIterator, sampleIterator) = ((m_vSampleBlockBuffer.first() * (m_bUseUnitGain ? m_vUnitGain[channelIterator] : 1)) + (m_bUseUnitOffset ? m_vUnitOffSet[channelIterator] : 0)) * (m_bUseChExponent ? pow(10., (double)m_vExponentChannel[channelIterator]) : 1);
m_vSampleBlockBuffer.pop_front();
}
actualSamplesWritten ++;
}
iSamplesWrittenToMatrix = iSamplesWrittenToMatrix + actualSamplesWritten;
}
if(m_outputFileStream.is_open() && m_bWriteDriverDebugToFile)
{
m_outputFileStream << "samples in buffer: " << m_vSampleBlockBuffer.size()/m_uiNumberOfChannels << endl;
m_outputFileStream << "ulSizeSamples: " << ulSizeSamples << endl;
m_outputFileStream << "ulNumSamplesReceived: " << ulNumSamplesReceived << endl;
m_outputFileStream << "sampleMax: " << sampleMax << endl;
m_outputFileStream << "sampleIterator: " << sampleIterator << endl;
m_outputFileStream << "iSamplesWrittenToMatrix: " << iSamplesWrittenToMatrix << endl << endl;
}
}
if(/*m_outputFileStream.is_open() &&*/ m_bWriteDriverDebugToFile)
{
//Get device buffer info
ULONG ulOverflow;
ULONG ulPercentFull;
m_oFpGetBufferInfo(m_HandleMaster, &ulOverflow, &ulPercentFull);
m_outputFileStream << "Unit offset: " << endl;
for(int w = 0; w<<m_vUnitOffSet.size(); w++)
cout << float(m_vUnitOffSet[w]) << " ";
m_outputFileStream << endl << endl;
m_outputFileStream << "Unit gain: " << endl;
for(int w = 0; w<<m_vUnitGain.size(); w++)
m_outputFileStream << float(m_vUnitGain[w]) << " ";
m_outputFileStream << endl << endl;
m_outputFileStream << "----------<See output file for sample matrix>----------" <<endl<<endl;
m_outputFileStream << "----------<Internal driver buffer is "<<ulPercentFull<<" full>----------"<<endl;
m_outputFileStream << "----------<Internal driver overflow is "<<ulOverflow<< ">----------"<<endl;
}
return true;
}
Eigen::MatrixXf LQRControler::Controler(Eigen::MatrixXf states,Eigen::MatrixXf ref,bool stop){
if(stop){
xs.setZero();
xr.setZero();
deltaxsi.setZero();
xsi.setZero();
xsiant.setZero();
deltaxsi.setZero();
deltaxsiant.setZero();
deltaxs.setZero();
xs_aumented.setZero();
deltaxs.setZero();
xsi.setZero();
deltaU.setZero();
xs_aumented.setZero();
ur.setZero();
auxu.setZero();
ur.setZero();
deltaU.setZero();
xsiant.setZero();
xsi.setZero();
deltaxsiant.setZero();
deltaxsi.setZero();
}else{
//Vectors of reference trajectory and control
xs<<0,0,states(2),states.block(3,0,5,1),0,0,states(10),states.block(11,0,5,1);
xr=trajectory->TrajetoryReference_LQR(ref);
//Vector integration of error(Trapezoidal method)
deltaxsi<<xs(2,0)-xr(2,0),xs(5,0)-xr(5,0);
xsi=xsiant+ts*(deltaxsi+deltaxsiant)/2;
// Error state vector
deltaxs=xs-xr;
// augmented error state vector
xs_aumented<<deltaxs,xsi;
//Control action variation
deltaU=Ke*xs_aumented;
//Control reference
ur<<9857.54,9837.48,0,0;
// Total control action
auxu=ur+deltaU;
//Variable update
xsiant=xsi;
deltaxsiant=deltaxsi;
}
if(auxu(0,0)>15000 ){
auxu(0,0)=15000;
}
if(auxu(1,0)>15000 ){
auxu(1,0)=15000;
}
/*The mass in the mathematical model was taken in grams,
for this reason the controller calculate the forces in g .m/s^2 and the torque in g .m^2/s^2.
But, the actuators are in the international units N and N. m for this reason the controls
actions are transforming from g to Kg*/
u(0,0)=auxu(0,0)/1000;
u(1,0)=auxu(1,0)/1000;
u(2,0)=auxu(2,0)/1000;
u(3,0)=auxu(3,0)/1000;
return u;
}
Eigen::MatrixXf LQRControler::Gain(int a){
MatrixXf Ke(4,18);
switch (a){
case 1:
Ke(0,0)=-0.042235;
Ke(0,1)=-1.634000;
Ke(0,2)=-214563.181606;
Ke(0,3)=22361.316355;
Ke(0,4)=-469.770928;
Ke(0,5)=-44.429909;
Ke(0,6)=-274.819725;
Ke(0,7)=179.740442;
Ke(0,8)=-4.138607;
Ke(0,9)=-151.715469;
Ke(0,10)=-27911.326194;
Ke(0,11)=2639.419097;
Ke(0,12)=-23.798532;
Ke(0,13)=61.610643;
Ke(0,14)=-2.455462;
Ke(0,15)=1.722355;
Ke(0,16)=-372525.858993;
Ke(0,17)=-1340.343031;
Ke(1,0)=0.043724;
Ke(1,1)=1.634345;
Ke(1,2)=-214913.758823;
Ke(1,3)=-22361.035830;
Ke(1,4)=491.422872;
Ke(1,5)=51.523958;
Ke(1,6)=273.003374;
Ke(1,7)=-184.352417;
Ke(1,8)=4.274720;
Ke(1,9)=151.747531;
Ke(1,10)=-27904.199336;
Ke(1,11)=-2630.232262;
Ke(1,12)=27.598384;
Ke(1,13)=-60.585039;
Ke(1,14)=2.419234;
Ke(1,15)=-1.787794;
Ke(1,16)=-373581.800483;
Ke(1,17)=1361.641991;
Ke(2,0)=-0.032541;
Ke(2,1)=0.001419;
Ke(2,2)=-0.994879;
Ke(2,3)=-23.208702;
Ke(2,4)=-473.620715;
Ke(2,5)=-839.879509;
Ke(2,6)=-711.719411;
Ke(2,7)=-20.867705;
Ke(2,8)=-2.984666;
Ke(2,9)=0.125815;
Ke(2,10)=-0.070789;
Ke(2,11)=-5.393054;
Ke(2,12)=-76.503440;
Ke(2,13)=-127.230151;
Ke(2,14)=-16.021031;
Ke(2,15)=-1.140229;
Ke(2,16)=-2.130183;
Ke(2,17)=-2398.132818;
Ke(3,0)=-0.021214;
Ke(3,1)=-0.000183;
Ke(3,2)=-0.378851;
Ke(3,3)=1.199515;
Ke(3,4)=-310.105790;
Ke(3,5)=574.890920;
Ke(3,6)=-0.508882;
Ke(3,7)=-513.578256;
Ke(3,8)=-1.946745;
Ke(3,9)=-0.021392;
Ke(3,10)=-0.026145;
Ke(3,11)=0.612819;
Ke(3,12)=-51.518510;
Ke(3,13)=89.089725;
Ke(3,14)=-0.597915;
Ke(3,15)=-13.133928;
Ke(3,16)=-0.818184;
Ke(3,17)=1624.536393;
break;
case 2:
// lambda=[ 0.000000 0.000000 0.000011 0.000011]
// rho=[ 0.000000 0.000000 50.000000 2.026424 2.026424 25.330296 2.026424 2.026424 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 750.000000 151.981775]
Ke(0,0)=-0.030988;
Ke(0,1)=-1.984245;
Ke(0,2)=-195799.395454;
Ke(0,3)=11891.295377;
Ke(0,4)=-180.876571;
Ke(0,5)=-1536.663662;
Ke(0,6)=-245.416213;
Ke(0,7)=165.890329;
Ke(0,8)=-1.743738;
Ke(0,9)=-110.745970;
Ke(0,10)=-29199.714836;
Ke(0,11)=1961.913398;
Ke(0,12)=-14.716127;
Ke(0,13)=-0.315128;
Ke(0,14)=-2.388003;
Ke(0,15)=1.261692;
Ke(0,16)=-376404.568417;
Ke(0,17)=-6400.497123;
Ke(1,0)=0.034637;
Ke(1,1)=1.976637;
Ke(1,2)=-196277.629306;
Ke(1,3)=-11846.000887;
Ke(1,4)=201.784531;
Ke(1,5)=1556.314812;
Ke(1,6)=244.191889;
Ke(1,7)=-170.779913;
Ke(1,8)=1.948502;
Ke(1,9)=110.337269;
Ke(1,10)=-29213.951294;
Ke(1,11)=-1948.095360;
Ke(1,12)=18.982996;
Ke(1,13)=2.228037;
Ke(1,14)=2.367666;
Ke(1,15)=-1.338513;
Ke(1,16)=-377983.474813;
Ke(1,17)=6454.109148;
Ke(2,0)=-0.047378;
Ke(2,1)=0.004004;
Ke(2,2)=-1.851807;
Ke(2,3)=-24.594792;
Ke(2,4)=-272.759244;
Ke(2,5)=-1726.758057;
Ke(2,6)=-604.991888;
Ke(2,7)=105.284974;
Ke(2,8)=-2.670061;
Ke(2,9)=0.224168;
Ke(2,10)=-0.150263;
Ke(2,11)=-5.076435;
Ke(2,12)=-47.269575;
Ke(2,13)=-180.121215;
Ke(2,14)=-14.019100;
Ke(2,15)=0.303278;
Ke(2,16)=-4.794292;
Ke(2,17)=-4676.061898;
Ke(3,0)=-0.047517;
Ke(3,1)=-0.002203;
Ke(3,2)=-1.245171;
Ke(3,3)=13.599331;
Ke(3,4)=-275.031481;
Ke(3,5)=1748.428079;
Ke(3,6)=108.705176;
Ke(3,7)=-612.052754;
Ke(3,8)=-2.678528;
Ke(3,9)=-0.123971;
Ke(3,10)=-0.111721;
Ke(3,11)=2.506000;
Ke(3,12)=-47.593811;
Ke(3,13)=182.225858;
Ke(3,14)=0.358147;
Ke(3,15)=-14.099505;
Ke(3,16)=-3.011545;
Ke(3,17)=4747.620655;
break;
case 3:
// lambda=[ 0.000000 0.000000 0.000011 0.000011]
// rho=[ 0.000000 0.000000 50.000000 2.026424 2.026424 25.330296 2.026424 2.026424 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.050000 0.050000 750.000000 151.981775]
Ke(0,0)=-0.028084;
Ke(0,1)=-1.112117;
Ke(0,2)=-210314.844482;
Ke(0,3)=11201.608878;
Ke(0,4)=-277.785251;
Ke(0,5)=-5359.965308;
Ke(0,6)=-1182.094742;
Ke(0,7)=946.735971;
Ke(0,8)=-2.066646;
Ke(0,9)=-81.795917;
Ke(0,10)=-28030.113924;
Ke(0,11)=1868.620069;
Ke(0,12)=-32.423089;
Ke(0,13)=-457.917860;
Ke(0,14)=-9.609483;
Ke(0,15)=7.214834;
Ke(0,16)=-606203.372276;
Ke(0,17)=-16615.863881;
Ke(1,0)=0.029079;
Ke(1,1)=1.108136;
Ke(1,2)=-210873.134168;
Ke(1,3)=-11160.249618;
Ke(1,4)=288.309901;
Ke(1,5)=5390.520867;
Ke(1,6)=1175.514586;
Ke(1,7)=-962.386221;
Ke(1,8)=2.139388;
Ke(1,9)=81.504539;
Ke(1,10)=-28055.217299;
Ke(1,11)=-1856.678122;
Ke(1,12)=34.673556;
Ke(1,13)=460.971816;
Ke(1,14)=9.571585;
Ke(1,15)=-7.419574;
Ke(1,16)=-608300.288983;
Ke(1,17)=16717.933874;
Ke(2,0)=-0.007605;
Ke(2,1)=0.000963;
Ke(2,2)=-0.489016;
Ke(2,3)=-10.222243;
Ke(2,4)=-79.412875;
Ke(2,5)=-561.700657;
Ke(2,6)=-397.251794;
Ke(2,7)=87.574033;
Ke(2,8)=-0.558177;
Ke(2,9)=0.070794;
Ke(2,10)=-0.042279;
Ke(2,11)=-2.391797;
Ke(2,12)=-19.169890;
Ke(2,13)=-79.063585;
Ke(2,14)=-14.033019;
Ke(2,15)=0.520162;
Ke(2,16)=-1.506872;
Ke(2,17)=-1492.420324;
Ke(3,0)=-0.006671;
Ke(3,1)=-0.000539;
Ke(3,2)=-0.111553;
Ke(3,3)=5.822244;
Ke(3,4)=-70.549998;
Ke(3,5)=490.165027;
Ke(3,6)=77.591946;
Ke(3,7)=-354.399256;
Ke(3,8)=-0.489745;
Ke(3,9)=-0.039581;
Ke(3,10)=-0.010310;
Ke(3,11)=1.145871;
Ke(3,12)=-17.037510;
Ke(3,13)=69.500655;
Ke(3,14)=0.441369;
Ke(3,15)=-12.937294;
Ke(3,16)=-0.338912;
Ke(3,17)=1298.506592;
break;
case 4:
// lambda=[ 0.000000 0.000000 0.000000 0.000000]
// rho=[ 0.000000 0.000000 20.000000 40.528473 81.056947 50.660592 4.052847 4.052847 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 400.000000 60.792710]
Ke(0,0)=-1.125831;
Ke(0,1)=-19.778281;
Ke(0,2)=-575993.163298;
Ke(0,3)=247883.643143;
Ke(0,4)=-21557.884892;
Ke(0,5)=-12854.879116;
Ke(0,6)=-2000.372074;
Ke(0,7)=-539.375599;
Ke(0,8)=-137.504190;
Ke(0,9)=-1794.787886;
Ke(0,10)=-41404.251389;
Ke(0,11)=8775.389726;
Ke(0,12)=-880.584294;
Ke(0,13)=-526.328810;
Ke(0,14)=-18.226381;
Ke(0,15)=-13.000127;
Ke(0,16)=-2772222.571130;
Ke(0,17)=-26362.179696;
Ke(1,0)=1.136584;
Ke(1,1)=19.881801;
Ke(1,2)=-575069.626305;
Ke(1,3)=-249177.147919;
Ke(1,4)=21767.189875;
Ke(1,5)=12956.111576;
Ke(1,6)=2010.509412;
Ke(1,7)=541.553128;
Ke(1,8)=138.805751;
Ke(1,9)=1804.217013;
Ke(1,10)=-41327.000766;
Ke(1,11)=-8793.228301;
Ke(1,12)=892.822556;
Ke(1,13)=531.631795;
Ke(1,14)=18.306742;
Ke(1,15)=13.058151;
Ke(1,16)=-2768057.387627;
Ke(1,17)=26569.044856;
Ke(2,0)=-0.137396;
Ke(2,1)=0.012876;
Ke(2,2)=-0.063661;
Ke(2,3)=-165.713137;
Ke(2,4)=-2732.126769;
Ke(2,5)=-2784.165149;
Ke(2,6)=-1276.475398;
Ke(2,7)=-160.102664;
Ke(2,8)=-16.443170;
Ke(2,9)=1.154133;
Ke(2,10)=-0.001965;
Ke(2,11)=-16.969634;
Ke(2,12)=-246.881269;
Ke(2,13)=-298.509866;
Ke(2,14)=-22.869671;
Ke(2,15)=-3.848760;
Ke(2,16)=-0.350008;
Ke(2,17)=-5020.319210;
Ke(3,0)=-0.138113;
Ke(3,1)=0.000406;
Ke(3,2)=-0.015478;
Ke(3,3)=-5.231209;
Ke(3,4)=-2743.690755;
Ke(3,5)=2778.132469;
Ke(3,6)=-158.824748;
Ke(3,7)=-1280.010522;
Ke(3,8)=-16.535203;
Ke(3,9)=0.035779;
Ke(3,10)=-0.000020;
Ke(3,11)=0.383136;
Ke(3,12)=-247.180538;
Ke(3,13)=297.468933;
Ke(3,14)=-3.836858;
Ke(3,15)=-22.898607;
Ke(3,16)=-0.101312;
Ke(3,17)=5001.332919;
break;
default:
Ke.setZero();
break;
}
return Ke;
}
bool GUSBAmpDriver::getSampleMatrixValue(MatrixXf& sampleMatrix)
{
sampleMatrix.setZero(); // Clear matrix - set all elements to zero
for(int queueIndex=0; queueIndex<m_QUEUE_SIZE; queueIndex++)
{
//receive data from each device
for (int deviceIndex = 0; deviceIndex < m_numDevices; deviceIndex++)
{
HANDLE hDevice = m_callSequenceHandles[deviceIndex];
//wait for notification from the system telling that new data is available
if (WaitForSingleObject(m_overlapped[deviceIndex][queueIndex].hEvent, 1000) == WAIT_TIMEOUT)
{
//throw string("Error on data transfer: timeout occurred.");
cout << "Error on data transfer: timeout occurred." << "\n";
return 0;
}
//get number of received bytes...
GetOverlappedResult(hDevice, &m_overlapped[deviceIndex][queueIndex], &m_numBytesReceived, false);
//...and check if we lost something (number of received bytes must be equal to the previously allocated buffer size)
if (m_numBytesReceived != m_bufferSizeBytes)
{
//throw string("Error on data transfer: samples lost.");
cout << "Error on data transfer: samples lost." << "\n";
return 0;
}
}
//store received data from each device in the correct order (that is scan-wise, where one scan includes all channels of all devices) ignoring the header
//Data is aligned as follows: element at position destBuffer(scanIndex * (m_chNumberOfChannels + m_bTrigger) + channelIndex) * sizeof(float) + HEADER_SIZE is sample of channel channelIndex (zero-based) of the scan with zero-based scanIndex.
//channelIndex ranges from 0..numDevices*numChannelsPerDevices where numDevices equals the number of recorded devices and numChannelsPerDevice the number of channels from each of those devices.
//It is assumed that all devices provide the same number of channels.
for (int scanIndex = 0; scanIndex < m_iNumberOfScans; scanIndex++)
{
for (int deviceIndex = 0; deviceIndex < m_numDevices; deviceIndex++)
{
for(int channelIndex = 0; channelIndex<m_chNumberOfChannels; channelIndex++)
{
BYTE ByteValue[sizeof(float)];
float FloatValue;
for(int i=0;i<sizeof(float);i++)
{
ByteValue[i] = m_buffers[deviceIndex][queueIndex][(scanIndex * (m_chNumberOfChannels + m_bTrigger) + channelIndex) * sizeof(float) + HEADER_SIZE + i];
}
memcpy(&FloatValue, &ByteValue, sizeof(float));
//store float-value to Matrix
sampleMatrix(channelIndex + deviceIndex*int(m_chNumberOfChannels), scanIndex + queueIndex * m_iNumberOfScans) = FloatValue;
}
}
}
//add new GetData call to the queue replacing the currently received one
for (int deviceIndex = 0; deviceIndex < m_numDevices; deviceIndex++)
if (!GT_GetData(m_callSequenceHandles[deviceIndex], m_buffers[deviceIndex][queueIndex], m_bufferSizeBytes, &m_overlapped[deviceIndex][queueIndex]))
{
cout << "\tError on GT_GetData.\n";
return 0;
}
}
return true;
}