Example shows freeze use.
// Copyright (C) 2009 by Thomas Moulard, AIST, CNRS, INRIA. // // This file is part of the roboptim. // // roboptim is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // roboptim is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU Lesser General Public License for more details. // // You should have received a copy of the GNU Lesser General Public License // along with roboptim. If not, see <http://www.gnu.org/licenses/>. #include <roboptim/trajectory/sys.hh> #include <boost/assign/list_of.hpp> #include <boost/mpl/vector.hpp> #include <boost/numeric/ublas/io.hpp> #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/trajectory/freeze.hh> #include <roboptim/trajectory/fwd.hh> #include <roboptim/trajectory/spline-length.hh> #include <roboptim/trajectory/cubic-b-spline.hh> #include <roboptim/trajectory/trajectory-cost.hh> #include <roboptim/trajectory/visualization/trajectory.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; int run_test () { using namespace boost::assign; CubicBSpline::vector_t params (16); // Initial position. params[0] = 0., params[1] = 0.; params[2] = 0., params[3] = 0.; params[4] = 0., params[5] = 0.; // Control point 3. params[6] = 25., params[7] = 100.; // Control point 4. params[8] = 75., params[9] = 0.; // Final position. params[10] = 100., params[11] = 100.; params[12] = 100., params[13] = 100.; params[14] = 100., params[15] = 100.; CubicBSpline::interval_t timeRange = CubicBSpline::makeInterval (0., 4.); CubicBSpline spline (timeRange, 2, params, "before"); discreteInterval_t interval (0., 4., 0.01); std::cout << "# Values:" << std::endl << "# " << normalize (spline (0.)) << std::endl << "# " << normalize (spline (2.5)) << std::endl << "# " << normalize (spline (4.)) << std::endl << "# 1st derivative:" << std::endl << "# " << normalize (spline.derivative (0., 1)) << std::endl << "# " << normalize (spline.derivative (2.5, 1)) << std::endl << "# " << normalize (spline.derivative (4., 1)) << std::endl << "# 2nd derivative:" << std::endl << "# " << normalize (spline.derivative (0., 2)) << std::endl << "# " << normalize (spline.derivative (2.5, 2)) << std::endl << "# " << normalize (spline.derivative (4., 2)) << std::endl << "# variationConfigWrtParam:" << std::endl << "# " << normalize (spline.variationConfigWrtParam (0.)) << std::endl << "# " << normalize (spline.variationConfigWrtParam (2.5)) << std::endl << "# " << normalize (spline.variationConfigWrtParam (4.)) << std::endl; Gnuplot gnuplot = Gnuplot::make_interactive_gnuplot (); gnuplot << set ("multiplot layout 1,2") << set ("grid") << plot_xy (spline); // Optimize. SplineLength cost (spline); // Check cost gradient. try { Function::vector_t x (params.size ()); x.clear (); checkGradientAndThrow (cost, 0, x, 2e-3); x = params; checkGradientAndThrow (cost, 0, x, 2e-3); } catch (BadGradient& bg) { std::cerr << bg << std::endl; return 1; } solver_t::problem_t problem (cost); problem.startingPoint () = params; std::vector<Function::size_type> indices; indices.push_back (0); indices.push_back (1); indices.push_back (2); indices.push_back (3); indices.push_back (4); indices.push_back (5); indices.push_back (params.size () - 6); indices.push_back (params.size () - 5); indices.push_back (params.size () - 4); indices.push_back (params.size () - 3); indices.push_back (params.size () - 2); indices.push_back (params.size () - 1); makeFreeze (problem) (indices, params); SolverFactory<solver_t> factory ("cfsqp", problem); solver_t& solver = factory (); std::cerr << "Cost function (before): " << cost (params) << std::endl; std::cerr << "Parameters (before): " << params << std::endl; std::cerr << solver << std::endl; solver_t::result_t res = solver.minimum (); switch (res.which ()) { case GenericSolver::SOLVER_VALUE: { Result& result = boost::get<Result> (res); CubicBSpline optimizedSpline (timeRange, 2, result.x, "after"); params = result.x; gnuplot << plot_xy (optimizedSpline); break; } case GenericSolver::SOLVER_NO_SOLUTION: { std::cerr << "No solution" << std::endl; return 1; } case GenericSolver::SOLVER_VALUE_WARNINGS: { ResultWithWarnings& result = boost::get<ResultWithWarnings> (res); CubicBSpline optimizedSpline (timeRange, 2, result.x, "after"); params = result.x; std::cerr << result << std::endl; gnuplot << plot_xy (optimizedSpline); break; } case GenericSolver::SOLVER_ERROR: { SolverError& result = boost::get<SolverError> (res); std::cerr << result << std::endl; return 1; } } std::cerr << "Parameters (after): " << params << std::endl; // Check cost gradient (final). try { checkGradientAndThrow (cost, 0, params, 2e-3); } catch (BadGradient& bg) { std::cerr << bg << std::endl; return 1; } std::cout << (gnuplot << unset ("multiplot")); return 0; } GENERATE_TEST ()