rendered paste body#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <iostream>
#include <sstream>
#include <string>
#include <stdlib.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <fstream>
#include <math.h>
#include <limits>
#include <libplayerc++/playerc++.h>
#include <Eigen/Dense>
#include "Timer.h"
#define pi 3.1415926535897932384626433
using namespace std;
using namespace PlayerCc;
using namespace Eigen;
typedef Matrix<long double, Dynamic, Dynamic> MatrixXld;
long double randn(long double mu=0.0, long double sigma=1.0) {
// source: http://www.dreamincode.net/code/snippet1446.htm
static bool deviateAvailable=false; // flag
static float storedDeviate; // deviate from previous calculation
long double polar, rsquared, var1, var2;
// If no deviate has been stored, the polar Box-Muller transformation is
// performed, producing two independent normally-distributed random
// deviates. One is stored for the next round, and one is returned.
if (!deviateAvailable) {
// choose pairs of uniformly distributed deviates, discarding those
// that don't fall within the unit circle
do {
var1=2.0*( (long double)(rand())/(long double)(RAND_MAX) ) - 1.0;
var2=2.0*( (long double)(rand())/(long double)(RAND_MAX) ) - 1.0;
rsquared=var1*var1+var2*var2;
} while ( rsquared>=1.0 || rsquared == 0.0);
// calculate polar tranformation for each deviate
polar=sqrt(-2.0*log(rsquared)/rsquared);
// store first deviate and set flag
storedDeviate=var1*polar;
deviateAvailable=true;
// return second deviate
return var2*polar*sigma + mu;
}
// If a deviate is available from a previous call to this function, it is
// returned, and the flag is set to false.
else {
deviateAvailable=false;
return storedDeviate*sigma + mu;
}
}
double radiansToDegrees(double rad)
{
return rad*(180/pi);
}
double degreesToRadians(double deg)
{
return deg*pi/180.0;
}
long double pi_to_pi(long double angle)
{
long double tmp = 2.0*pi;
angle = fmod(angle,tmp);
if(angle>pi)
{
angle = angle - 2.0*pi;
}
if(angle<-pi)
{
angle = angle + 2.0*pi;
}
return angle;
}
MatrixXld calculateRealPos(long double relX, long double relY, long double x, long double y, long double angle)
{
MatrixXld pos(1,2);
pos(0,0) = x + relX*cosl(angle) - relY*sinl(angle);
pos(0,1) = y + relX*sinl(angle) - relY*cosl(angle);
return pos;
}
void drive(double *forwardSpeed, double *turnSpeed, RangerProxy &rp)
{
double avoidDistance = 0.5;
double corridorWidth = 1;
int avoidTurnSpeed = 45;
double minSpeed = 0.1;
static bool tracking = false;
if(!tracking && (rp.GetRange(2) < avoidDistance + corridorWidth))
{
tracking = true;
cout << "tracking on" << endl;
}
cout << "bb" << endl;
if(rp.GetRangeCount()==0)
{
cout << "fuck" << endl;
return;
}
cout << "cc" << endl;
if(rp.GetRange(1) < avoidDistance) //left
{
*forwardSpeed = std::max(minSpeed,*forwardSpeed * 0.9);
*turnSpeed = (-1)*avoidTurnSpeed; // draai naar rechts
return;
}
else if(rp.GetRange(2) < avoidDistance) //right
{
*forwardSpeed = std::max(minSpeed,*forwardSpeed * 0.9);
*turnSpeed = avoidTurnSpeed; // draai naar links
return;
} else if(rp.GetRange(0) < avoidDistance * 1.5) //center
{
*forwardSpeed = std::max(minSpeed,*forwardSpeed * 0.7);
*turnSpeed = avoidTurnSpeed * 1.5; // draai naar links
return;
} else
// geen enkele sensor meet iets binnen de avoidDistance
// er is ruimte.
if(rp.GetRange(1) < *forwardSpeed) // linkerafstand < snelheid vooruit
{
// cout << "obstacle @ left, reduce speed from " << *forwardSpeed << " to ";
*forwardSpeed = std::max(minSpeed,rp.GetRange(1) * 0.5); //halve afstand links
// cout << *forwardSpeed << "m/s"<< endl;
return;
}
else if(rp.GetRange(2) < *forwardSpeed)
{
// cout << "obstacle @ right, reduce speed from " << *forwardSpeed << " to ";
*forwardSpeed = std::max(minSpeed,rp.GetRange(2) * 0.5); //halve afstand rechts
// cout << *forwardSpeed << "m/s"<< endl;
return;
}
else if(rp.GetRange(0) < *forwardSpeed) //halve afstand rechtdoor
{
// cout << "obstacle @ mid, reduce speed from " << *forwardSpeed << " to ";
*forwardSpeed = std::max(minSpeed,rp.GetRange(0) * 0.5);
// cout << *forwardSpeed << "m/s"<< endl;
return;
}
*turnSpeed = 0;
if(tracking)
{
if(rp.GetRange(2) > corridorWidth)
{
*turnSpeed = (-1)*avoidTurnSpeed; // draai naar rechts
// cout << "Stay in corridor!" <<endl;
return;
}
}
*forwardSpeed = std::min(*forwardSpeed * 1.1,1.0);
// cout << "No obstacles: Speed up!" << endl;
return;
}
int main()
{
cout.precision(15);
// START CONFIG
long double V = 3; // m/s (current velocity?)
long double sigmaV = 0.3; // m/s
long double sigmaG = (3.0*pi/180.0); // radians
MatrixXld Q(2,2); Q(0,0) = pow(sigmaV,2); Q(0,1) = 0.0; Q(1,0) = 0.0; Q(1,1) = pow(sigmaG,2);
long double sigmaR = 0.1; // metres
long double sigmaB = (1.0*pi/180.0); // radians
MatrixXld R(2,2); R(0,0) = pow(sigmaR,2); R(0,1) = 0; R(1,0) = 0; R(1,1) = pow(sigmaB,2);
// END CONFIG
// START INIT
long double G = 0; // start angle
MatrixXld x(3,1);
MatrixXld P(3,3);
double dt;
MatrixXld QE(2,2); QE = 2*Q;
MatrixXld RE(2,2); RE = 8*R;
long double Gn = 0;
long double Vn = 0;
long double s,c,vts,vtc;
MatrixXld Gv(3,3);
MatrixXld Gu(3,2);
Gv(0,0) = 1; Gv(0,1) = 0;
Gv(1,0) = 0; Gv(1,1) = 1;
Gv(2,0) = 0; Gv(2,1) = 0; Gv(2,2) = 1;
MatrixXld lm(2,6);
lm << -2.9, 0, 2.9, -2.9, 0, 2.9,
1.9, 1.9, 1.9, -1.9, -1.9, -1.9;
MatrixXld idf(0,2);
MatrixXld z(0,2);
MatrixXld ftag(lm.cols(),1);
for(int i = 0; i < lm.rows(); i++)
{
ftag(i) = i;
}
MatrixXld da_table(lm.rows(),1);
MatrixXld zf;
MatrixXld udf;
MatrixXld zn;
int Nxv,Nf;
int jbest;
long double nbest,outer;
double dtsum = 0;
int fpos;
MatrixXld H;
long double dX,dY,d2,d,xd,yd,xd2,yd2;
MatrixXld zp(2,1);
MatrixXld caz(2,1);
MatrixXld v(2,1);
MatrixXld S,Eigennis,Eigennd;
long double nis,nd;
MatrixXld daidf;
MatrixXld Gvv(2,3);
MatrixXld Gz(2,2);
MatrixXld rng(1,2);
MatrixXld rnm(1,2);
int lenz, lenx;
MatrixXld HA,v2,RR,i2,HHH;
MatrixXld zp2(2,1);
MatrixXld PHt,S2, SChol, SCholInv, W1,W;
int teller = 0;
string line;
int innerI = -1;
int fidsFound;
MatrixXld pos;
// END INIT EKF
// INIT PLAYER/STAGE
double forwardSpeed = 0.1, turnSpeed;
double startX = -2.5;
double startY = 1.5;
double startYaw = 0; // Degrees
bool first = true;
Timer t;
cout << "Init..." << endl;
PlayerClient robot("localhost");
cout << "proxies..." << endl;
Position2dProxy p2dProxy(&robot,0);
cout << "Ranger Proxy..." << endl;
RangerProxy rangerProxy(&robot,0);
cout << "Simulation Proxy..." << endl;
SimulationProxy simProxy(&robot,0);
cout << "Fiducial finder Proxy..." << endl;
FiducialProxy fidProxy(&robot,0);
srand(time(NULL));
cout << "enable motors..." << endl;
p2dProxy.SetMotorEnable(1);
cout << endl;
try
{
rangerProxy.RequestGeom();
p2dProxy.RequestGeom();
while(1)
{
if(!first)
{
t.stop();
dt = t.getElapsedTimeInMicroSec()/1000000;
} else {
dt = 0.025;
first = false;
}
t.start();
teller++;
cout << "teller:" << teller << " time: " << dt << endl;
robot.Read();
// tijdelijk tot MAP based loop algoritme
drive(&forwardSpeed, &turnSpeed, rangerProxy);
cout << "setSpeed(" << forwardSpeed << ", " << turnSpeed << ");" << endl;
p2dProxy.SetSpeed(forwardSpeed,dtor(turnSpeed));
V = forwardSpeed;
G = dtor(turnSpeed);
Vn = V + randn()*sqrt(Q(0,0));
Gn = G + randn()*sqrt(Q(1,1));
s = sinl(Gn+x(2,0));
c = cosl(Gn+x(2,0));
vts = Vn*dt*s;
vtc = Vn*dt*c;
Gv(0,2) = -vts;
Gv(1,2) = vtc;
Gu(0,0) = dt*c; Gu(0,1) = -vts;
Gu(1,0) = dt*s; Gu(1,1) = vtc;
Gu(2,0) = dt*sinl(Gn)/4.0; Gu(2,1) = Vn*dt*cosl(Gn)/4.0;
P.block(0,0,3,3) = Gv*P.block(0,0,3,3)*Gv.transpose() + Gu*QE*Gu.transpose();
if(P.rows() > 3)
{
P.block(0,3,3,P.rows()-3) = Gv*P.block(0,3,3,P.rows()-3);
P.block(3,0,P.cols()-3,3) = P.block(0,3,3,P.rows()-3).transpose();
}
x(0,0) += vtc;
x(1,0) += vts;
x(2,0) = x(2,0) + Vn*dt*sinl(Gn)/4.0;
dtsum += dt;
if(dtsum >= 0.5)
{
cout << "observe my dear" << endl;
dtsum = 0;
fidsFound = fidProxy.GetCount();
for(int i=0;i<fidsFound;i++)
{
pos = calculateRealPos(fidProxy.GetFiducialItem(i).pose.px,fidProxy.GetFiducialItem(i).pose.py,x(0,0),x(1,0),fidProxy.GetFiducialItem(i).pose.pyaw+G);
idf.conservativeResize(idf.rows()+1,2);
idf(i,0) = pos(0,0);
idf(i,1) = pos(0,1);
z.conservativeResize(z.rows()+1,2);
z(i,0) = sqrt(exp(fidProxy.GetFiducialItem(i).pose.px) + exp(fidProxy.GetFiducialItem(i).pose.py));
z(i,1) = fidProxy.GetFiducialItem(i).pose.pyaw + G;
}
cout << "z:" << endl << z << endl;
cout << "z.cols" << z.cols() << endl;
for(int o=0;o < z.rows();o++)
{
z(0,o) += randn()*sqrt(R(0,0));
z(1,o) += randn()*sqrt(R(1,1));
}
zf.resize(0,0);
zn.resize(0,0);
daidf.resize(0,0);
Nxv = 3;
Nf = (x.rows() - Nxv)/2;
for(int i=0;i<z.rows();i++)
{
jbest=0;
nbest=std::numeric_limits<long double>::max();
outer=std::numeric_limits<long double>::max();
for(int j=1;j<=Nf;j++)
{
caz(0,0) = z(0,i);
caz(1,0) = z(1,i);
fpos = Nxv + j*2 -1;
HHH = MatrixXld::Zero(2,x.rows());
dX = x(fpos-1) -x(0,0);
dY = x(fpos) -x(1,0);
d2 = pow(dX,2) + pow(dY,2);
d=sqrt(d2);
xd = dX/d;
yd = dY/d;
xd2 = dX/d2;
yd2 = dY/d2;
zp(0,0) = d; zp(1,0) = atan2(dY,dX) - x(2,0);
HHH(0,0) = -xd; HHH(0,1) = -yd; HHH(0,2) = 0;
HHH(1,0) = yd2; HHH(1,1) = -xd2; HHH(1,2) = -1;
HHH(0,fpos-1) = xd; HHH(0,fpos) = yd;
HHH(1,fpos-1) = -yd2; HHH(1,fpos) = xd2;
v=caz-zp;
v(1,0)=pi_to_pi(v(1,0));
S=HHH*P*HHH.transpose() + RE;
Eigennis = v.transpose()*S.inverse()*v;
nis = Eigennis(0,0);
nd = nis + log(S.determinant());
if (nis < 4 && nd < nbest)
{
nbest = nd;
jbest = j;
} else if(nis < outer)
{
outer = nis;
}
}
if(jbest!=0)
{
zf.conservativeResize(2,zf.cols()+1);
zf(0,zf.cols()-1) = z(0,i);
zf(1,zf.cols()-1) = z(1,i);
daidf.conservativeResize(1,daidf.cols()+1);
daidf(0,daidf.cols()-1) = jbest;
} else if(outer > 25) {
zn.conservativeResize(2,zn.cols()+1);
zn(0,zn.cols()-1) = z(0,i);
zn(1,zn.cols()-1) = z(1,i);
}
}
if((bool)z.rows())
idf = daidf;
lenz = zf.cols();
lenx = x.rows();
HA = MatrixXld::Zero(2*lenz,lenx);
v2 = MatrixXld::Zero(2*lenz,1);
RR = MatrixXld::Zero(2*lenz,2*lenz);
for(int i=0;i<lenz;i++)
{
i2.resize(2,1);
i2(0,0) = 2*(i+1)-1;
i2(1,0) = 2*(i+1);
fpos = Nxv + idf(i)*2 -1;
HHH = MatrixXld::Zero(2,x.rows());
dX = x(fpos-1) -x(0,0);
dY = x(fpos) -x(1,0);
d2 = pow(dX,2) + pow(dY,2);
d=sqrt(d2);
xd = dX/d;
yd = dY/d;
xd2 = dX/d2;
yd2 = dY/d2;
zp2(0,0) = d; zp2(1,0) = atan2(dY,dX) - x(2,0);
HHH(0,0) = -xd; HHH(0,1) = -yd; HHH(0,2) = 0;
HHH(1,0) = yd2; HHH(1,1) = -xd2; HHH(1,2) = -1;
HHH(0,fpos-1) = xd; HHH(0,fpos) = yd;
HHH(1,fpos-1) = -yd2; HHH(1,fpos) = xd2;
HA.block(i2(0,0)-1,0, i2(1,0)-i2(0,0)+1, HA.cols()) = HHH;
v2(i2(0,0)-1,0) = zf(0,i)-zp2(0,0);
v2(i2(1,0)-1,0) = pi_to_pi(zf(1,i)-zp2(1,0));
RR.block(i2(0,0)-1,i2(0,0)-1,i2(1,0)-i2(0,0)+1,i2(1,0)-i2(0,0)+1) = RE;
}
if(HA.size() > 0)
{
PHt = P*HA.transpose();
S2 = HA*PHt + RR;
S2 = (S2+S2.transpose())*0.5;
LLT<MatrixXld> llt;
llt.compute(S2);
SChol = llt.matrixL().transpose();
SCholInv = SChol.inverse();
W1 = PHt * SCholInv;
W = W1 * SCholInv.transpose();
x = x + W*v2;
P = P - W1*W1.transpose();
}
for(int i=0;i<zn.cols();i++)
{
int len = x.rows();
long double r = zn(0,i);
long double b = zn(1,i);
long double s = sinl(x(2,0)+b);
long double c = cosl(x(2,0)+b);
x.conservativeResize(len+2,1);
x(len,0) = x(0,0)+r*c;
x(len+1,0) = x(1,0)+r*s;
Gvv(0,0) = 1; Gvv(0,1) = 0; Gvv(0,2) = -r*s;
Gvv(1,0) = 0; Gvv(1,1) = 1; Gvv(1,2) = r*c;
Gz(0,0) = c; Gz(0,1) = -r*s;
Gz(1,0) = s; Gz(1,1) = r*c;
rng(0,0) = len+1;
rng(0,1) = len+2;
P.conservativeResize(rng(0,1),rng(0,1));
P.block(rng(0,0)-1,rng(0,0)-1,2,2) = Gvv*P.block(0,0,3,3)*Gvv.transpose() + Gz*RE*Gz.transpose();
P.block(rng(0,0)-1,0,2,3) = Gvv*P.block(0,0,3,3);
P.block(0, rng(0,0)-1,3,2) = P.block(rng(0,0)-1,0,2,3).transpose();
if(len>3)
{
rnm(0,0) = 4;
rnm(0,1) = len;
P.block(rng(0,0)-1,3,2,rnm(0,1)-rnm(0,0)+1) = Gvv*P.block(0,3,3,rnm(0,1)-rnm(0,0)+1);
P.block(3,rng(0,0)-1,rnm(0,1)-rnm(0,0)+1,2) = P.block(rng(0,0)-1,3,2,rnm(0,1)-rnm(0,0)+1).transpose();
}
}
}
simProxy.SetPose2d("rg",(double)x(0,0)+startX,(double)x(1,0)+startY,(double)x(2,0));
sleep(0.05);
}
}
catch(PlayerError error)
{
cout << "Error: " << error.GetErrorStr() << endl;
}
}