void GQuiver::poseStampedCallback(const geometry_msgs::PoseStamped msg)
{
sourceFrame = Frame::findFrame(msg.header.frame_id);
if (sourceFrame < 0)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
if (!availableFrames[sourceFrame].valid)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
Ogre::SceneNode * node;
// If none exists, create a blank rendered frame
if (xCones.size() == 0)
{
// Create scene node
node = graphicNode->createChildSceneNode();
sceneNodes.push_back(node);
addFrameToNode(node);
}
else
{
node = sceneNodes[0];
}
tf::StampedTransform & tran = availableFrames[sourceFrame].extrapTransform;
tf::Vector3 & pos = tran.getOrigin();
tf::Quaternion rot = tran.getRotation();
Ogre::Quaternion Qf(rot.w(), rot.x(), rot.y(), rot.z());
Ogre::Vector3 Xf(pos.x(), pos.y(), pos.z());
Ogre::Quaternion Qe(msg.pose.orientation.w,
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z);
Ogre::Vector3 Xe(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z);
// Set pose indicator position
node->setPosition(Xf + Qf * Xe);
node->setOrientation(Qf * Qe);
node->setScale(scale, scale, scale);
if (drawXAxis)
{
xCones[0]->setColor(Ogre::ColourValue(xColour[0] / 255.f, xColour[1] / 255.f, xColour[2] / 255.f, 1));
xCylinders[0]->setColor(Ogre::ColourValue(xColour[0] / 255.f, xColour[1] / 255.f, xColour[2] / 255.f, 1));
}
if (drawYAxis)
{
yCones[0]->setColor(Ogre::ColourValue(yColour[0] / 255.f, yColour[1] / 255.f, yColour[2] / 255.f, 1));
yCylinders[0]->setColor(Ogre::ColourValue(yColour[0] / 255.f, yColour[1] / 255.f, yColour[2] / 255.f, 1));
}
if (drawZAxis)
{
zCones[0]->setColor(Ogre::ColourValue(zColour[0] / 255.f, zColour[1] / 255.f, zColour[2] / 255.f, 1));
zCylinders[0]->setColor(Ogre::ColourValue(zColour[0] / 255.f, zColour[1] / 255.f, zColour[2] / 255.f, 1));
}
publishStatus(Gadget::OKAY, "Okay");
msgsRecieved++;
}
void GQuiver::odometryCallback(const nav_msgs::Odometry msg)
{
sourceFrame = Frame::findFrame(msg.header.frame_id);
if (sourceFrame < 0)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
if (!availableFrames[sourceFrame].valid)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
// Cull axes to keep their number within the limit
while (sceneNodes.size() > countLimit)
{
removeFirstIn();
}
tf::StampedTransform & tran = availableFrames[sourceFrame].extrapTransform;
tf::Vector3 & pos = tran.getOrigin();
tf::Quaternion rot = tran.getRotation();
Ogre::Quaternion Qf(rot.w(), rot.x(), rot.y(), rot.z());
Ogre::Vector3 Xf(pos.x(), pos.y(), pos.z());
Ogre::Quaternion Qe(msg.pose.pose.orientation.w,
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z);
Ogre::Vector3 Xe(msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z);
Ogre::Vector3 X1 = Xf + Qf * Xe;
Ogre::Quaternion Q1 = Qf * Qe;
if (sceneNodes.size() > 0)
{
Ogre::Vector3 X2 = sceneNodes.back()->getPosition();
Ogre::Quaternion Q2 = sceneNodes.back()->getOrientation();
if ((X1 - X2).normalise() < positionTolerance)
{
return;
}
if (acos((Q2.Inverse() * Q1).w) * 2 < angleTolerance)
{
return;
}
}
// Create scene node at odo position
Ogre::SceneNode * newNode = graphicNode->createChildSceneNode();
newNode->setPosition(X1);
newNode->setOrientation(Q1);
newNode->setScale(scale, scale, scale);
sceneNodes.push_back(newNode);
addFrameToNode(newNode);
publishStatus(Gadget::OKAY, "Okay");
msgsRecieved++;
}
void GQuiver::poseArrayCallback(const geometry_msgs::PoseArray msg)
{
sourceFrame = Frame::findFrame(msg.header.frame_id);
if (sourceFrame < 0)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
if (!availableFrames[sourceFrame].valid)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
// Cull axes to keep their number within the limit
while (sceneNodes.size() > msg.poses.size())
{
removeFirstIn();
}
while (sceneNodes.size() < msg.poses.size())
{
// Create scene node at odo position
Ogre::SceneNode * newNode = graphicNode->createChildSceneNode();
sceneNodes.push_back(newNode);
addFrameToNode(newNode);
}
tf::StampedTransform & tran = availableFrames[sourceFrame].extrapTransform;
tf::Vector3 & pos = tran.getOrigin();
tf::Quaternion rot = tran.getRotation();
Ogre::Quaternion Qf(rot.w(), rot.x(), rot.y(), rot.z());
Ogre::Vector3 Xf(pos.x(), pos.y(), pos.z());
for (int i = 0; i < msg.poses.size(); i++)
{
Ogre::Quaternion Qe(msg.poses[i].orientation.w,
msg.poses[i].orientation.x,
msg.poses[i].orientation.y,
msg.poses[i].orientation.z);
Ogre::Vector3 Xe(msg.poses[i].position.x,
msg.poses[i].position.y,
msg.poses[i].position.z);
Ogre::Vector3 X1 = Xf + Qf * Xe;
Ogre::Quaternion Q1 = Qf * Qe;
Ogre::SceneNode * node = sceneNodes[i];
node->setPosition(X1);
node->setOrientation(Q1);
node->setScale(scale, scale, scale);
if (drawXAxis)
{
xCones[i]->setColor(Ogre::ColourValue(xColour[0] / 255.f, xColour[1] / 255.f, xColour[2] / 255.f, 1));
xCylinders[i]->setColor(Ogre::ColourValue(xColour[0] / 255.f, xColour[1] / 255.f, xColour[2] / 255.f, 1));
}
if (drawYAxis)
{
yCones[i]->setColor(Ogre::ColourValue(yColour[0] / 255.f, yColour[1] / 255.f, yColour[2] / 255.f, 1));
yCylinders[i]->setColor(Ogre::ColourValue(yColour[0] / 255.f, yColour[1] / 255.f, yColour[2] / 255.f, 1));
}
if (drawZAxis)
{
zCones[i]->setColor(Ogre::ColourValue(zColour[0] / 255.f, zColour[1] / 255.f, zColour[2] / 255.f, 1));
zCylinders[i]->setColor(Ogre::ColourValue(zColour[0] / 255.f, zColour[1] / 255.f, zColour[2] / 255.f, 1));
}
}
publishStatus(Gadget::OKAY, "Okay");
msgsRecieved++;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4;
// x, y, z : position
// vx, vy, vz : linear velocity
// phi, theta, psi : orientation (Yaw-Pitch-Roll = Euler(3,2,1))
// p, q, r : angular velocity
// u1, u2, u3, u4 : velocity of the propellers
Control vu1,vu2,vu3,vu4;
// vu1, vu2, vu3, vu4 : derivative of u1, u2, u3, u4
DifferentialEquation f;
// Quad constants
const double c = 0.00001;
const double Cf = 0.00065;
const double d = 0.250;
const double Jx = 0.018;
const double Jy = 0.018;
const double Jz = 0.026;
const double Im = 0.0001;
const double m = 0.9;
const double g = 9.81;
const double Cx = 0.1;
// Minimization Weights
double coeffX = .00001;
double coeffU = coeffX*0.0001;//0.000000000000001;
double coeffX2 = coeffX * 100.;
double coeffX3 = coeffX * 0.00001;
double coeffO = -coeffX * 0.1;
// final position (used in the cost function)
double xf = 0., yf = 0., zf = 20.;
//
double T = 8.; //length (in second) of the trajectory predicted in the MPC
int nb_nodes = 20.; //number of nodes used in the Optimal Control Problem
// 20 nodes means that the algorithm will discretized the trajectory equally into 20 pieces
// If you increase the number of node, the solution will be more precise but calculation will take longer (~nb_nodes^2)
// In ACADO, the commands are piecewise constant functions, constant between each node.
double tmpc = 0.2; //time (in second) between two activation of the MPC algorithm
// DEFINE A OPTIMAL CONTROL PROBLEM
// -------------------------------
OCP ocp( 0.0, T, nb_nodes );
// DEFINE THE COST FUNCTION
// -------------------------------
Function h, hf;
h << x;
h << y;
h << z;
h << vu1;
h << vu2;
h << vu3;
h << vu4;
h << p;
h << q;
h << r;
hf << x;
hf << y;
hf << z;
DMatrix Q(10,10), Qf(3,3);
Q(0,0) = coeffX;
Q(1,1) = coeffX;
Q(2,2) = coeffX;
Q(3,3) = coeffU;
Q(4,4) = coeffU;
Q(5,5) = coeffU;
Q(6,6) = coeffU;
Q(7,7) = coeffX2;
Q(8,8) = coeffX2;
Q(9,9) = coeffX2;
Qf(0,0) = 1.;
Qf(1,1) = 1.;
Qf(2,2) = 1.;
DVector reff(3), ref(10);
ref(0) = xf;
ref(1) = yf;
ref(2) = zf;
ref(3) = 58.;
ref(4) = 58.;
ref(5) = 58.;
ref(6) = 58.;
ref(7) = 0.;
ref(8) = 0.;
ref(9) = 0.;
reff(0) = xf;
reff(1) = yf;
reff(2) = zf;
// The cost function is define as : integrale from 0 to T { transpose(h(x,u,t)-ref)*Q*(h(x,u,t)-ref) } + transpose(hf(x,u,T))*Qf*hf(x,u,T)
// == integrale cost (also called running cost or Lagrange Term) + final cost (Mayer Term)
ocp.minimizeLSQ ( Q, h, ref);
// ocp.minimizeLSQEndTerm(Qf, hf, reff);
// When doing MPC, you need terms in the cost function to stabilised the system => p, q, r and vu1, vu2, vu3, vu4. You can check that if you reduce their weights "coeffX2" or "coeffU" too low, the optimization will crashed.
// DEFINE THE DYNAMIC EQUATION OF THE SYSTEM:
// ----------------------------------
f << dot(x) == vx;
f << dot(y) == vy;
f << dot(z) == vz;
f << dot(vx) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m;
f << dot(vy) == -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(psi)*cos(theta)/m;
f << dot(vz) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(psi)*cos(theta)/m - g;
f << dot(phi) == -cos(phi)*tan(theta)*p+sin(phi)*tan(theta)*q+r;
f << dot(theta) == sin(phi)*p+cos(phi)*q;
f << dot(psi) == cos(phi)/cos(theta)*p-sin(phi)/cos(theta)*q;
f << dot(p) == (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx;
f << dot(q) == (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy;
f << dot(r) == (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz;
f << dot(u1) == vu1;
f << dot(u2) == vu2;
f << dot(u3) == vu3;
f << dot(u4) == vu4;
// ---------------------------- DEFINE CONTRAINTES --------------------------------- //
//Dynamic
ocp.subjectTo( f );
//State constraints
//Constraints on the velocity of each propeller
ocp.subjectTo( 16 <= u1 <= 95 );
ocp.subjectTo( 16 <= u2 <= 95 );
ocp.subjectTo( 16 <= u3 <= 95 );
ocp.subjectTo( 16 <= u4 <= 95 );
//Command constraints
//Constraints on the acceleration of each propeller
ocp.subjectTo( -100 <= vu1 <= 100 );
ocp.subjectTo( -100 <= vu2 <= 100 );
ocp.subjectTo( -100 <= vu3 <= 100 );
ocp.subjectTo( -100 <= vu4 <= 100 );
#if AVOID_SINGULARITIES == 1
//Constraint to avoid singularity
// In this example I used Yaw-Pitch-Roll convention to describe orientation of the quadrotor
// when using Euler Angles representation, you always have a singularity, and we need to
// avoid it otherwise the algorithm will crashed.
ocp.subjectTo( -1. <= theta <= 1.);
#endif
//Example of Eliptic obstacle constraints (here, cylinders with eliptic basis)
ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-5)*(z-5)) );
ocp.subjectTo( 16 <= ((x-3)*(x-3)+2*(z-9)*(z-9)) );
ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-15)*(z-15)) );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
// The next line define the equation used to simulate the system :
// here "f" is used (=the same as the one used for the ocp) so the trajectory simulated
// from the commands will be exactly the same as the one calculated by the MPC.
// If for instance you want to test robusness, you can define an other dynamic equation "f2"
// with changed parameters and put it here.
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_RK45 );
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
RealTimeAlgorithm alg( ocp, tmpc );
//Usually, you do only one step of the optimisation algorithm (~Gauss-Newton here)
//at each activation of the MPC, that way the delay between getting the state and
//sending a command is as quick as possible.
alg.set( MAX_NUM_ITERATIONS, 1 );
StaticReferenceTrajectory zeroReference;
Controller controller( alg,zeroReference );
// SET AN INITIAL GUESS FOR THE FIRST MPC LOOP (NEXT LOOPS WILL USE AS INITIAL GUESS THE SOLUTION FOUND AT THE PREVIOUS MPC LOOP)
Grid timeGrid(0.0,T,nb_nodes+1);
VariablesGrid x_init(16, timeGrid);
// init with static
for (int i = 0 ; i<nb_nodes+1 ; i++ ) {
x_init(i,0) = 0.;
x_init(i,1) = 0.;
x_init(i,2) = 0.;
x_init(i,3) = 0.;
x_init(i,4) = 0.;
x_init(i,5) = 0.;
x_init(i,6) = 0.;
x_init(i,7) = 0.;
x_init(i,8) = 0.;
x_init(i,9) = 0.;
x_init(i,10) = 0.;
x_init(i,11) = 0.;
x_init(i,12) = 58.; //58. is the propeller rotation speed so the total thrust balance the weight of the quad
x_init(i,13) = 58.;
x_init(i,14) = 58.;
x_init(i,15) = 58.;
}
alg.initializeDifferentialStates(x_init);
// SET OPTION AND PLOTS WINDOW
// ---------------------------
// Linesearch is an algorithm which will try several points along the descent direction to choose a better step length.
// It looks like activating this option produice more stable trajectories.198
alg.set( GLOBALIZATION_STRATEGY, GS_LINESEARCH );
alg.set(INTEGRATOR_TYPE, INT_RK45);
// You can uncomment those lines to see how the predicted trajectory involve along time
// (but be carefull because you will have 1 ploting window per MPC loop)
// GnuplotWindow window1(PLOT_AT_EACH_ITERATION);
// window1.addSubplot( z,"DifferentialState z" );
// window1.addSubplot( x,"DifferentialState x" );
// window1.addSubplot( theta,"DifferentialState theta" );
// window1.addSubplot( 16./((x+3)*(x+3)+4*(z-5)*(z-5)),"Dist obs1" );
// window1.addSubplot( 16./((x-3)*(x-3)+4*(z-9)*(z-9)),"Dist obs2" );
// window1.addSubplot( 16./((x+2)*(x+2)+4*(z-15)*(z-15)),"Dist obs3" );
// alg<<window1;
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
// The first argument is the starting time, the second the end time.
SimulationEnvironment sim( 0.0,10.,process,controller );
//Setting the state of the sytem at the beginning of the simulation.
DVector x0(16);
x0.setZero();
x0(0) = 0.;
x0(12) = 58.;
x0(13) = 58.;
x0(14) = 58.;
x0(15) = 58.;
t = clock();
if (sim.init( x0 ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
if (sim.run( ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
t = clock() - t;
std::cout << "total time : " << (((float)t)/CLOCKS_PER_SEC)<<std::endl;
// ...SAVE THE RESULTS IN FILES
// ----------------------------------------------------------
std::ofstream file;
file.open("/tmp/log_state.txt",std::ios::out);
std::ofstream file2;
file2.open("/tmp/log_control.txt",std::ios::out);
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
sampledProcessOutput.print(file);
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
feedbackControl.print(file2);
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
GnuplotWindow window;
window.addSubplot( sampledProcessOutput(0), "x " );
window.addSubplot( sampledProcessOutput(1), "y " );
window.addSubplot( sampledProcessOutput(2), "z " );
window.addSubplot( sampledProcessOutput(6),"phi" );
window.addSubplot( sampledProcessOutput(7),"theta" );
window.addSubplot( sampledProcessOutput(8),"psi" );
window.plot( );
graphics::corbaServer::ClientCpp client = graphics::corbaServer::ClientCpp();
client.createWindow("window");
return 0;
}
