Example shows FreeTimeTrajectory use.
#include <fstream>
#include <boost/assign/list_of.hpp>
#include <boost/mpl/vector.hpp>
#include <roboptim/core/io.hh>
#include <roboptim/core/finite-difference-gradient.hh>
#include <roboptim/core/solver-factory.hh>
#include <roboptim/core/visualization/gnuplot.hh>
#include <roboptim/core/visualization/gnuplot-commands.hh>
#include <roboptim/core/visualization/gnuplot-function.hh>
#include <roboptim/core/plugin/cfsqp.hh>
#include "common.hh"
using namespace roboptim;
using namespace roboptim::visualization;
using namespace roboptim::visualization::gnuplot;
typedef CFSQPSolver::problem_t::constraints_t constraint_t;
typedef CFSQPSolver solver_t;
const unsigned nControlPoints = 15;
const unsigned nConstraintsPerCtrlPts = 10;
const double vMax = 85.;
int run_test ()
{
using namespace boost;
using namespace boost::assign;
const double finalPos = 200.;
params[0] = 0;
params[1] = 0;
for (unsigned i = 0; i < nControlPoints-4; ++i)
params[i+2] = finalPos / (nControlPoints - 5) * i;
params[nControlPoints-2] = finalPos;
params[nControlPoints-1] = finalPos;
freeTime_t freeTimeTraj (spline, 1.);
Function::matrix_t a (1, freeTimeTraj.parameters ().size ());
a.clear ();
a (0, 0) = -1.;
Function::vector_t b (1);
b.clear ();
roboptim::NumericLinearFunction cost (a, b);
solver_t::problem_t problem (cost);
problem.startingPoint () = freeTimeTraj.parameters ();
problem.argumentBounds ()[0] = Function::makeLowerInterval (0.);
const freeTime_t::vector_t freeTimeParams = freeTimeTraj.parameters ();
std::vector<Function::size_type> indices;
indices.push_back (1);
indices.push_back (2);
indices.push_back (3);
indices.push_back (freeTimeParams.size () - 3);
indices.push_back (freeTimeParams.size () - 2);
indices.push_back (freeTimeParams.size () - 1);
Function::interval_t vRange = Function::makeUpperInterval (.5 * vMax * vMax);
(freeTimeTraj, problem, vRange, nControlPoints * nConstraintsPerCtrlPts);
std::ofstream limitSpeedStream ("limit-speed.gp");
Gnuplot gnuplot = Gnuplot::make_interactive_gnuplot ();
gnuplot
<< set ("multiplot layout 1,2 title "
"'variation of speed before and after optimization'")
<< set ("grid");
SolverFactory<solver_t> factory ("cfsqp", problem);
solver_t& solver = factory ();
std::cout << solver << std::endl;
solver_t::result_t res = solver.minimum ();
std::cerr << res << std::endl;
freeTimeTraj;
switch (solver.minimumType ())
{
case GenericSolver::SOLVER_VALUE:
{
const Result& result = solver.getMinimum<Result> ();
break;
}
case GenericSolver::SOLVER_VALUE_WARNINGS:
{
const ResultWithWarnings& result =
solver.getMinimum<ResultWithWarnings> ();
optimizedTrajectory.setParameters (result.x);
break;
}
case GenericSolver::SOLVER_NO_SOLUTION:
case GenericSolver::SOLVER_ERROR:
return 1;
}
limitSpeedStream << (gnuplot << unset ("multiplot"));
return 0;
}
GENERATE_TEST ()