diff --git a/README.md b/README.md index 8685fc6..0cbe4a4 100644 --- a/README.md +++ b/README.md @@ -74,6 +74,27 @@ Docker builds). ./scripts/run.sh ``` +Every executable now evaluates all supported solver/strategy combinations for both `float` and `double` problem instances by +default. The command-line flags allow you to focus on a subset: + +```bash +# Only run double-precision OSQP for the rocket example and print trajectories +./build/release/rocket_max_altitude --solvers osqp --scalars double --dump + +# Compare iLQR and CGD for single-track coordination without trajectories +./build/release/multi_agent_single_track --solvers ilqr,cgd --strategies centralized,sequential --max-outer 5 +``` + +The output now contains a concise summary line for each run. For example: + +``` +scalar=float solver=ilqr strategy=centralized agents=4 cost=9.128435 time_ms=0.713421 +scalar=double solver=ilqr strategy=centralized agents=4 cost=9.128431 time_ms=0.926587 +scalar=double solver=osqp strategy=centralized agents=4 unsupported (skipping) +``` + +Use `--dump-trajectories` on any executable to restore the CSV-style trajectory printouts for the evaluated configurations. + ### **Utility scripts** The repository includes Python helpers for benchmarking and visualising the example executables. Both scripts accept `--help` for the full list of options. @@ -116,44 +137,6 @@ The repository includes Python helpers for benchmarking and visualising the exam -## Results - -Times and costs for different methods in the example code: -``` -πŸš— Single-Track Lane Following Test πŸš— ---------------------------------------------- -Solver Cost Time (ms) - --------------------------------------------- -CGD 24.0465 20.6444 -OSQP 30.1889 2.33275 -OSQP Collocation 23.9809 5.11993 -iLQR 24.4039 1.06887 -``` - -``` - -Multi-Agent Single Track Test - -Method Cost Time (ms) ----------------------------------------------------------------------- -Centralized CGD 7928.151 1214.919 -Centralized iLQR 7928.501 135.472 -Centralized OSQP 7929.011 285.711 -Centralized OSQP-collocation 7929.392 1071.582 -Nash Sequential CGD 7928.153 26.612 -Nash Sequential iLQR 7928.327 11.053 -Nash Sequential OSQP 7928.384 38.514 -Nash Sequential OSQP-collocation 7928.158 2098.299 -Nash LineSearch CGD 7928.153 27.597 -Nash LineSearch iLQR 7928.327 13.807 -Nash LineSearch OSQP 7928.384 40.643 -Nash LineSearch OSQP-collocation 7928.152 2010.733 -Nash TrustRegion CGD 7928.153 34.767 -Nash TrustRegion iLQR 7928.199 14.093 -Nash TrustRegion OSQP 7928.417 46.087 -Nash TrustRegion OSQP-collocation 7928.152 1596.460 -``` - ## License Apache 2.0 diff --git a/examples/example_utils.hpp b/examples/example_utils.hpp index c0739cb..64fe44b 100644 --- a/examples/example_utils.hpp +++ b/examples/example_utils.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -12,6 +13,7 @@ #include "multi_agent_solver/solvers/solver.hpp" #include "multi_agent_solver/strategies/strategy.hpp" +#include "multi_agent_solver/types.hpp" namespace examples { @@ -63,52 +65,124 @@ canonical_strategy_name( const std::string& name ) throw std::invalid_argument( "Unknown strategy '" + name + "'." ); } +inline std::string +canonical_scalar_name( const std::string& name ) +{ + const std::string key = normalize_key( name ); + if( key == "float" || key == "single" || key == "f32" ) + return "float"; + if( key == "double" || key == "doubleprecision" || key == "f64" ) + return "double"; + throw std::invalid_argument( "Unknown scalar type '" + name + "'." ); +} + +template +inline std::string +scalar_label() +{ + if constexpr( std::is_same_v ) + return "float"; + if constexpr( std::is_same_v ) + return "double"; + return "unknown"; +} + +template inline std::vector available_solver_names() { std::vector names{ "ilqr", "cgd" }; #ifdef MAS_HAVE_OSQP - names.push_back( "osqp" ); - names.push_back( "osqp_collocation" ); + if constexpr( std::is_same_v ) + { + names.push_back( "osqp" ); + names.push_back( "osqp_collocation" ); + } #endif return names; } -inline mas::Solver +inline std::vector +available_solver_names() +{ + return available_solver_names(); +} + +template +inline bool +solver_supported_for_scalar( const std::string& canonical ) +{ +#ifdef MAS_HAVE_OSQP + if constexpr( !std::is_same_v ) + { + if( canonical == "osqp" || canonical == "osqp_collocation" ) + return false; + } +#else + if( canonical == "osqp" || canonical == "osqp_collocation" ) + return false; +#endif + return true; +} + +template +inline mas::SolverVariant make_solver( const std::string& name ) { const std::string canonical = canonical_solver_name( name ); if( canonical == "ilqr" ) - return mas::Solver{ std::in_place_type }; + return mas::SolverVariant{ std::in_place_type> }; if( canonical == "cgd" ) - return mas::Solver{ std::in_place_type }; + return mas::SolverVariant{ std::in_place_type> }; #ifdef MAS_HAVE_OSQP if( canonical == "osqp" ) - return mas::Solver{ std::in_place_type }; + { + if constexpr( std::is_same_v ) + return mas::SolverVariant{ std::in_place_type> }; + } if( canonical == "osqp_collocation" ) - return mas::Solver{ std::in_place_type }; + { + if constexpr( std::is_same_v ) + return mas::SolverVariant{ std::in_place_type> }; + } #endif + if( !solver_supported_for_scalar( canonical ) ) + throw std::invalid_argument( "Solver '" + name + "' is not available for scalar type '" + scalar_label() + "'." ); throw std::invalid_argument( "Unknown solver '" + name + "'." ); } -inline mas::Strategy -make_strategy( const std::string& name, mas::Solver solver, const mas::SolverParams& params, int max_outer ) +inline mas::Solver +make_solver( const std::string& name ) +{ + return make_solver( name ); +} + +template +inline mas::StrategyT +make_strategy( const std::string& name, mas::SolverVariant solver, const mas::SolverParamsT& params, + int max_outer ) { const std::string canonical = canonical_strategy_name( name ); if( canonical == "centralized" ) { mas::set_params( solver, params ); - return mas::Strategy{ mas::CentralizedStrategy{ std::move( solver ) } }; + return mas::StrategyT{ mas::CentralizedStrategy{ std::move( solver ) } }; } if( canonical == "sequential" ) - return mas::Strategy{ mas::SequentialNashStrategy{ max_outer, std::move( solver ), params } }; + return mas::StrategyT{ mas::SequentialNashStrategy{ max_outer, std::move( solver ), params } }; if( canonical == "linesearch" ) - return mas::Strategy{ mas::LineSearchNashStrategy{ max_outer, std::move( solver ), params } }; + return mas::StrategyT{ mas::LineSearchNashStrategy{ max_outer, std::move( solver ), params } }; if( canonical == "trustregion" ) - return mas::Strategy{ mas::TrustRegionNashStrategy{ max_outer, std::move( solver ), params } }; + return mas::StrategyT{ mas::TrustRegionNashStrategy{ max_outer, std::move( solver ), params } }; throw std::invalid_argument( "Unknown strategy '" + name + "'." ); } +inline mas::Strategy +make_strategy( const std::string& name, mas::Solver solver, const mas::SolverParams& params, int max_outer ) +{ + return make_strategy( name, std::move( solver ), params, max_outer ); +} + inline void print_available( std::ostream& os ) { @@ -116,12 +190,14 @@ print_available( std::ostream& os ) os << "Available solvers:"; for( const auto& solver : solvers ) os << ' ' << solver; - os << '\n'; + os << " (float and double; OSQP variants require double)\n"; os << "Available strategies: centralized, sequential, linesearch, trustregion\n"; + os << "Scalar precisions: float, double\n"; } +template inline void -print_state_trajectory( std::ostream& os, const Eigen::MatrixXd& states, double dt, const std::string& label ) +print_state_trajectory( std::ostream& os, const mas::StateTrajectoryT& states, Scalar dt, const std::string& label ) { if( states.size() == 0 ) return; @@ -134,17 +210,20 @@ print_state_trajectory( std::ostream& os, const Eigen::MatrixXd& states, double for( int col = 0; col < states.cols(); ++col ) { - const double time_value = dt > 0.0 ? static_cast( col ) * dt : static_cast( col ); + const double time_value + = dt > static_cast( 0 ) ? static_cast( col ) * static_cast( dt ) : static_cast( col ); os << time_value; for( int row = 0; row < states.rows(); ++row ) - os << ',' << states( row, col ); + os << ',' << static_cast( states( row, col ) ); os << '\n'; } os << '\n'; } +template inline void -print_control_trajectory( std::ostream& os, const Eigen::MatrixXd& controls, double dt, const std::string& label ) +print_control_trajectory( std::ostream& os, const mas::ControlTrajectoryT& controls, Scalar dt, + const std::string& label ) { if( controls.size() == 0 ) return; @@ -157,13 +236,26 @@ print_control_trajectory( std::ostream& os, const Eigen::MatrixXd& controls, dou for( int col = 0; col < controls.cols(); ++col ) { - const double time_value = dt > 0.0 ? static_cast( col ) * dt : static_cast( col ); + const double time_value + = dt > static_cast( 0 ) ? static_cast( col ) * static_cast( dt ) : static_cast( col ); os << time_value; for( int row = 0; row < controls.rows(); ++row ) - os << ',' << controls( row, col ); + os << ',' << static_cast( controls( row, col ) ); os << '\n'; } os << '\n'; } +inline void +print_state_trajectory( std::ostream& os, const Eigen::MatrixXd& states, double dt, const std::string& label ) +{ + print_state_trajectory( os, states, dt, label ); +} + +inline void +print_control_trajectory( std::ostream& os, const Eigen::MatrixXd& controls, double dt, const std::string& label ) +{ + print_control_trajectory( os, controls, dt, label ); +} + } // namespace examples diff --git a/examples/models/pendulum_model.hpp b/examples/models/pendulum_model.hpp index 3b9df7f..920d951 100644 --- a/examples/models/pendulum_model.hpp +++ b/examples/models/pendulum_model.hpp @@ -1,40 +1,66 @@ #pragma once + +#include + #include #include "multi_agent_solver/types.hpp" namespace mas { -inline StateDerivative -pendulum_dynamics( const State& x, const Control& u ) + +template +inline StateDerivativeT +pendulum_dynamics( const StateT& x, const ControlT& u ) { - const double g = 9.81; // [m/s^2] - const double l = 1.0; // [m] - const double m = 1.0; // [kg] - StateDerivative dxdt( 2 ); + const Scalar g = static_cast( 9.81 ); // [m/s^2] + const Scalar l = static_cast( 1.0 ); // [m] + const Scalar m = static_cast( 1.0 ); // [kg] + StateDerivativeT dxdt( 2 ); dxdt( 0 ) = x( 1 ); dxdt( 1 ) = ( g / l ) * std::sin( x( 0 ) ) + u( 0 ) / ( m * l * l ); return dxdt; } -inline Eigen::MatrixXd -pendulum_state_jacobian( const State& x, const Control& ) +inline StateDerivative +pendulum_dynamics( const State& x, const Control& u ) { - const double g = 9.81; - const double l = 1.0; - Eigen::MatrixXd A = Eigen::MatrixXd::Zero( 2, 2 ); - A( 0, 1 ) = 1.0; - A( 1, 0 ) = ( g / l ) * std::cos( x( 0 ) ); + return pendulum_dynamics( x, u ); +} + +template +inline Eigen::Matrix +pendulum_state_jacobian( const StateT& x, const ControlT& ) +{ + const Scalar g = static_cast( 9.81 ); + const Scalar l = static_cast( 1.0 ); + Eigen::Matrix A = Eigen::Matrix::Zero( 2, 2 ); + A( 0, 1 ) = static_cast( 1.0 ); + A( 1, 0 ) = ( g / l ) * std::cos( x( 0 ) ); return A; } inline Eigen::MatrixXd -pendulum_control_jacobian( const State&, const Control& ) +pendulum_state_jacobian( const State& x, const Control& u ) { - const double m = 1.0; - const double l = 1.0; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero( 2, 1 ); - B( 1, 0 ) = 1.0 / ( m * l * l ); + return pendulum_state_jacobian( x, u ); +} + +template +inline Eigen::Matrix +pendulum_control_jacobian( const StateT&, const ControlT& ) +{ + const Scalar m = static_cast( 1.0 ); + const Scalar l = static_cast( 1.0 ); + Eigen::Matrix B = Eigen::Matrix::Zero( 2, 1 ); + B( 1, 0 ) = static_cast( 1.0 ) / ( m * l * l ); return B; } + +inline Eigen::MatrixXd +pendulum_control_jacobian( const State& x, const Control& u ) +{ + return pendulum_control_jacobian( x, u ); +} + } // namespace mas \ No newline at end of file diff --git a/examples/models/rocket_model.hpp b/examples/models/rocket_model.hpp index fd0fdc9..fb29ed0 100644 --- a/examples/models/rocket_model.hpp +++ b/examples/models/rocket_model.hpp @@ -9,20 +9,25 @@ namespace mas { -struct RocketParameters +template +struct RocketParametersT { - double initial_mass = 1.0; ///< Initial vehicle mass [kg] - double gravity = 9.81; ///< Gravity [m/s^2] - double exhaust_velocity = 25.0; ///< Effective exhaust velocity [m/s] + Scalar initial_mass = static_cast( 1.0 ); ///< Initial vehicle mass [kg] + Scalar gravity = static_cast( 9.81 ); ///< Gravity [m/s^2] + Scalar exhaust_velocity = static_cast( 25.0 ); ///< Effective exhaust velocity [m/s] }; -inline mas::State -rocket_dynamics( const RocketParameters& params, const mas::State& state, const mas::Control& control ) +using RocketParameters = RocketParametersT; +using RocketParametersf = RocketParametersT; + +template +inline StateT +rocket_dynamics( const RocketParametersT& params, const StateT& state, const ControlT& control ) { - mas::State derivative = mas::State::Zero( 3 ); + StateT derivative = StateT::Zero( 3 ); - const double mass = std::max( state( 2 ), 1e-6 ); - const double thrust = mass > 0 ? control( 0 ) : 0.0; + const Scalar mass = std::max( state( 2 ), static_cast( 1e-6 ) ); + const Scalar thrust = mass > static_cast( 0 ) ? control( 0 ) : static_cast( 0 ); derivative( 0 ) = state( 1 ); derivative( 1 ) = thrust / mass - params.gravity; @@ -31,20 +36,37 @@ rocket_dynamics( const RocketParameters& params, const mas::State& state, const return derivative; } +inline mas::State +rocket_dynamics( const RocketParameters& params, const mas::State& state, const mas::Control& control ) +{ + return rocket_dynamics( params, state, control ); +} + +template +inline MotionModelT +make_rocket_dynamics( const RocketParametersT& params ) +{ + return [params]( const StateT& state, const ControlT& control ) { + return rocket_dynamics( params, state, control ); + }; +} + inline mas::MotionModel make_rocket_dynamics( const RocketParameters& params ) { - return [params]( const mas::State& state, const mas::Control& control ) { return rocket_dynamics( params, state, control ); }; + return make_rocket_dynamics( params ); } -inline Eigen::MatrixXd -rocket_state_jacobian( const RocketParameters&, const mas::State& state, const mas::Control& control ) +template +inline Eigen::Matrix +rocket_state_jacobian( const RocketParametersT&, const StateT& state, const ControlT& control ) { - Eigen::MatrixXd A = Eigen::MatrixXd::Zero( 3, 3 ); - A( 0, 1 ) = 1.0; + Eigen::Matrix A + = Eigen::Matrix::Zero( 3, 3 ); + A( 0, 1 ) = static_cast( 1.0 ); - const double thrust = control( 0 ); - const double mass = std::max( state( 2 ), 1e-6 ); + const Scalar thrust = control( 0 ); + const Scalar mass = std::max( state( 2 ), static_cast( 1e-6 ) ); A( 1, 2 ) = -thrust / ( mass * mass ); @@ -52,14 +74,28 @@ rocket_state_jacobian( const RocketParameters&, const mas::State& state, const m } inline Eigen::MatrixXd -rocket_control_jacobian( const RocketParameters& params, const mas::State& state, const mas::Control& ) +rocket_state_jacobian( const RocketParameters& params, const mas::State& state, const mas::Control& control ) +{ + return rocket_state_jacobian( params, state, control ); +} + +template +inline Eigen::Matrix +rocket_control_jacobian( const RocketParametersT& params, const StateT& state, const ControlT& ) { - Eigen::MatrixXd B = Eigen::MatrixXd::Zero( 3, 1 ); - const double mass = std::max( state( 2 ), 1e-6 ); + Eigen::Matrix B + = Eigen::Matrix::Zero( 3, 1 ); + const Scalar mass = std::max( state( 2 ), static_cast( 1e-6 ) ); - B( 1, 0 ) = 1.0 / mass; - B( 2, 0 ) = -1.0 / params.exhaust_velocity; + B( 1, 0 ) = static_cast( 1.0 ) / mass; + B( 2, 0 ) = -static_cast( 1.0 ) / params.exhaust_velocity; return B; } +inline Eigen::MatrixXd +rocket_control_jacobian( const RocketParameters& params, const mas::State& state, const mas::Control& control ) +{ + return rocket_control_jacobian( params, state, control ); +} + } // namespace mas \ No newline at end of file diff --git a/examples/models/single_track_model.hpp b/examples/models/single_track_model.hpp index 1c7f3d3..6d6c979 100644 --- a/examples/models/single_track_model.hpp +++ b/examples/models/single_track_model.hpp @@ -1,4 +1,7 @@ #pragma once + +#include + #include #include "multi_agent_solver/types.hpp" @@ -15,26 +18,26 @@ * Controls (u): * u(0) = delta [rad] - Steering angle * u(1) = a [m/sΒ²] - Acceleration - * - * */ namespace mas { -inline StateDerivative -single_track_model( const State& x, const Control& u ) + +template +inline StateDerivativeT +single_track_model( const StateT& x, const ControlT& u ) { - double psi = x( 2 ); - double v = x( 3 ); + const Scalar psi = x( 2 ); + const Scalar v = x( 3 ); // Unpack controls - double delta = u( 0 ); - double a = u( 1 ); + const Scalar delta = u( 0 ); + const Scalar a = u( 1 ); // Vehicle parameters - const double L = 2.5; // Wheelbase length [m] + const Scalar L = static_cast( 2.5 ); // Wheelbase length [m] // Compute derivatives - StateDerivative dxdt( 4 ); + StateDerivativeT dxdt( 4 ); dxdt( 0 ) = v * std::cos( psi ); // X_dot dxdt( 1 ) = v * std::sin( psi ); // Y_dot dxdt( 2 ) = v * std::tan( delta ) / L; // Psi_dot @@ -43,41 +46,65 @@ single_track_model( const State& x, const Control& u ) return dxdt; } +inline StateDerivative +single_track_model( const State& x, const Control& u ) +{ + return single_track_model( x, u ); +} + /** * @brief Compute the state Jacobian A = βˆ‚f/βˆ‚x for the single-track model. */ -inline Eigen::MatrixXd -single_track_state_jacobian( const State& x, const Control& u ) +template +inline Eigen::Matrix +single_track_state_jacobian( const StateT& x, const ControlT& u ) { - double psi = x( 2 ); - double v = x( 3 ); - double delta = u( 0 ); - const double L = 2.5; // Wheelbase - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero( 4, 4 ); - A( 0, 2 ) = -v * std::sin( psi ); // βˆ‚X_dot / βˆ‚psi - A( 0, 3 ) = std::cos( psi ); // βˆ‚X_dot / βˆ‚v - A( 1, 2 ) = v * std::cos( psi ); // βˆ‚Y_dot / βˆ‚psi - A( 1, 3 ) = std::sin( psi ); // βˆ‚Y_dot / βˆ‚v - A( 2, 3 ) = std::tan( delta ) / L; // βˆ‚psi_dot / βˆ‚v + const Scalar psi = x( 2 ); + const Scalar v = x( 3 ); + const Scalar delta = u( 0 ); + const Scalar L = static_cast( 2.5 ); // Wheelbase + + Eigen::Matrix A + = Eigen::Matrix::Zero( 4, 4 ); + A( 0, 2 ) = -v * std::sin( psi ); // βˆ‚X_dot / βˆ‚psi + A( 0, 3 ) = std::cos( psi ); // βˆ‚X_dot / βˆ‚v + A( 1, 2 ) = v * std::cos( psi ); // βˆ‚Y_dot / βˆ‚psi + A( 1, 3 ) = std::sin( psi ); // βˆ‚Y_dot / βˆ‚v + A( 2, 3 ) = std::tan( delta ) / L; // βˆ‚psi_dot / βˆ‚v return A; } +inline Eigen::MatrixXd +single_track_state_jacobian( const State& x, const Control& u ) +{ + return single_track_state_jacobian( x, u ); +} + /** * @brief Compute the control Jacobian B = βˆ‚f/βˆ‚u for the single-track model. */ -inline Eigen::MatrixXd -single_track_control_jacobian( const State& x, const Control& u ) +template +inline Eigen::Matrix +single_track_control_jacobian( const StateT& x, const ControlT& u ) { - double v = x( 3 ); - double delta = u( 0 ); - const double L = 2.5; + const Scalar v = x( 3 ); + const Scalar delta = u( 0 ); + const Scalar L = static_cast( 2.5 ); - Eigen::MatrixXd B = Eigen::MatrixXd::Zero( 4, 2 ); - B( 2, 0 ) = v / ( L * std::cos( delta ) * std::cos( delta ) ); // βˆ‚psi_dot / βˆ‚delta - B( 3, 1 ) = 1.0; // βˆ‚v_dot / βˆ‚a + Eigen::Matrix B + = Eigen::Matrix::Zero( 4, 2 ); + const Scalar cos_delta = std::cos( delta ); + B( 2, 0 ) = v / ( L * cos_delta * cos_delta ); // βˆ‚psi_dot / βˆ‚delta + B( 3, 1 ) = static_cast( 1.0 ); // βˆ‚v_dot / βˆ‚a return B; } + +inline Eigen::MatrixXd +single_track_control_jacobian( const State& x, const Control& u ) +{ + return single_track_control_jacobian( x, u ); +} + } // namespace mas \ No newline at end of file diff --git a/examples/multi_agent_lqr.cpp b/examples/multi_agent_lqr.cpp index 0cbad8e..3fbaa05 100644 --- a/examples/multi_agent_lqr.cpp +++ b/examples/multi_agent_lqr.cpp @@ -1,71 +1,80 @@ #include #include #include -#include #include #include #include #include #include +#include +#include #include "Eigen/Dense" +#include "example_utils.hpp" #include "multi_agent_solver/agent.hpp" #include "multi_agent_solver/multi_agent_problem.hpp" #include "multi_agent_solver/solvers/solver.hpp" #include "multi_agent_solver/strategies/strategy.hpp" #include "multi_agent_solver/types.hpp" -#include "example_utils.hpp" +namespace +{ -/*──────────────── create simple LQR OCP (unchanged) ───────────────*/ -mas::OCP -create_linear_lqr_ocp( int n_x, int n_u, double dt, int T ) +template +mas::OCP +create_linear_lqr_ocp( int n_x, int n_u, Scalar dt, int horizon ) { - using namespace mas; + using OCP = mas::OCP; + using State = typename OCP::State; + using Control = typename OCP::Control; + using Matrix = Eigen::Matrix; + using StageCostFunction = typename OCP::StageCostFunction; + using TerminalCostFunction = typename OCP::TerminalCostFunction; + OCP ocp; ocp.state_dim = n_x; ocp.control_dim = n_u; ocp.dt = dt; - ocp.horizon_steps = T; - Eigen::VectorXd initial_state = Eigen::VectorXd::Zero( n_x ); + ocp.horizon_steps = horizon; + + State initial_state = State::Zero( n_x ); if( n_x > 0 ) - initial_state[0] = 1.0; + initial_state( 0 ) = static_cast( 1.0 ); ocp.initial_state = initial_state; - Eigen::MatrixXd A = Eigen::MatrixXd::Identity( n_x, n_x ); - Eigen::MatrixXd B = Eigen::MatrixXd::Identity( n_x, n_u ); + Matrix A = Matrix::Identity( n_x, n_x ); + Matrix B = Matrix::Identity( n_x, n_u ); ocp.dynamics = [A, B]( const State& x, const Control& u ) { return A * x + B * u; }; ocp.dynamics_state_jacobian - = [A]( const MotionModel&, const State&, const Control& ) { return A; }; + = [A]( const typename OCP::MotionModel&, const State&, const Control& ) { return A; }; ocp.dynamics_control_jacobian - = [B]( const MotionModel&, const State&, const Control& ) { return B; }; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Identity( n_x, n_x ); - Eigen::MatrixXd R = Eigen::MatrixXd::Identity( n_u, n_u ); - Eigen::MatrixXd Qf = Q; - const Eigen::MatrixXd Qt = Q + Q.transpose(); - const Eigen::MatrixXd Rt = R + R.transpose(); - const Eigen::MatrixXd Qf_sym = Qf + Qf.transpose(); - ocp.stage_cost = [Q, R]( const State& x, const Control& u, std::size_t ) { + = [B]( const typename OCP::MotionModel&, const State&, const Control& ) { return B; }; + + Matrix Q = Matrix::Identity( n_x, n_x ); + Matrix R = Matrix::Identity( n_u, n_u ); + Matrix Qf = Q; + const Matrix Qt = Q + Q.transpose(); + const Matrix Rt = R + R.transpose(); + const Matrix Qf_sym = Qf + Qf.transpose(); + + ocp.stage_cost = [Q, R]( const State& x, const Control& u, std::size_t ) { return ( x.transpose() * Q * x ).value() + ( u.transpose() * R * u ).value(); }; - ocp.cost_state_gradient = [Qt]( const StageCostFunction&, const State& x, const Control&, std::size_t ) { - return Qt * x; - }; - ocp.cost_control_gradient = [Rt]( const StageCostFunction&, const State&, const Control& u, std::size_t ) { - return Rt * u; - }; - ocp.cost_state_hessian = [Qt]( const StageCostFunction&, const State&, const Control&, std::size_t ) { - return Qt; - }; - ocp.cost_control_hessian = [Rt]( const StageCostFunction&, const State&, const Control&, std::size_t ) { - return Rt; - }; - ocp.cost_cross_term = [n_x, n_u]( const StageCostFunction&, const State&, const Control&, std::size_t ) { - return Eigen::MatrixXd::Zero( n_u, n_x ); - }; - ocp.terminal_cost = [Qf]( const State& x ) { return ( x.transpose() * Qf * x ).value(); }; + ocp.cost_state_gradient + = [Qt]( const StageCostFunction&, const State& x, const Control&, std::size_t ) { return Qt * x; }; + ocp.cost_control_gradient + = [Rt]( const StageCostFunction&, const State&, const Control& u, std::size_t ) { return Rt * u; }; + ocp.cost_state_hessian + = [Qt]( const StageCostFunction&, const State&, const Control&, std::size_t ) { return Qt; }; + ocp.cost_control_hessian + = [Rt]( const StageCostFunction&, const State&, const Control&, std::size_t ) { return Rt; }; + ocp.cost_cross_term + = [n_x, n_u]( const StageCostFunction&, const State&, const Control&, std::size_t ) { + return Eigen::Matrix::Zero( n_u, n_x ); + }; + ocp.terminal_cost + = [Qf]( const State& x ) { return ( x.transpose() * Qf * x ).value(); }; ocp.terminal_cost_gradient = [Qf_sym]( const TerminalCostFunction&, const State& x ) { return Qf_sym * x; }; ocp.terminal_cost_hessian @@ -76,17 +85,60 @@ create_linear_lqr_ocp( int n_x, int n_u, double dt, int T ) return ocp; } +template +mas::MultiAgentProblemT +create_problem( int agents, int n_x, int n_u, Scalar dt, int horizon ) +{ + mas::MultiAgentProblemT problem; + for( int i = 0; i < agents; ++i ) + { + auto ocp = std::make_shared>( create_linear_lqr_ocp( n_x, n_u, dt, horizon ) ); + problem.add_agent( std::make_shared>( static_cast( i ), ocp ) ); + } + return problem; +} + struct Options { - bool show_help = false; - int agents = 10; - int max_outer = 10; - std::string solver = "ilqr"; - std::string strategy = "centralized"; + bool show_help = false; + int agents = 10; + int max_outer = 10; + bool dump_trajectories = false; + std::vector solvers; + std::vector strategies; + std::vector scalars; }; -namespace +void +trim_in_place( std::string& value ) { + const auto first = value.find_first_not_of( " \t" ); + if( first == std::string::npos ) + { + value.clear(); + return; + } + const auto last = value.find_last_not_of( " \t" ); + value = value.substr( first, last - first + 1 ); +} + +void +append_list( std::vector& target, const std::string& csv, const auto& canonicalizer ) +{ + std::size_t start = 0; + while( start <= csv.size() ) + { + const auto comma = csv.find( ',', start ); + std::string token + = csv.substr( start, comma == std::string::npos ? std::string::npos : comma - start ); + trim_in_place( token ); + if( !token.empty() ) + target.push_back( canonicalizer( token ) ); + if( comma == std::string::npos ) + break; + start = comma + 1; + } +} int parse_int( const std::string& label, const std::string& value ) @@ -114,7 +166,7 @@ parse_options( int argc, char** argv ) const auto end = eq_pos == std::string::npos ? arg.size() : eq_pos; std::replace( arg.begin() + 2, arg.begin() + static_cast( end ), '_', '-' ); } - auto match_with_value = [&]( const std::string& name, std::string& out ) { + auto match_with_value = [&]( const std::string& name, std::string& out ) { const std::string prefix = name + "="; if( arg == name ) { @@ -136,6 +188,11 @@ parse_options( int argc, char** argv ) options.show_help = true; continue; } + if( arg == "--dump-trajectories" ) + { + options.dump_trajectories = true; + continue; + } std::string value; if( match_with_value( "--agents", value ) ) @@ -144,11 +201,27 @@ parse_options( int argc, char** argv ) } else if( match_with_value( "--solver", value ) ) { - options.solver = value; + append_list( options.solvers, value, examples::canonical_solver_name ); + } + else if( match_with_value( "--solvers", value ) ) + { + append_list( options.solvers, value, examples::canonical_solver_name ); } else if( match_with_value( "--strategy", value ) ) { - options.strategy = value; + append_list( options.strategies, value, examples::canonical_strategy_name ); + } + else if( match_with_value( "--strategies", value ) ) + { + append_list( options.strategies, value, examples::canonical_strategy_name ); + } + else if( match_with_value( "--scalar", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); + } + else if( match_with_value( "--scalars", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); } else if( match_with_value( "--max-outer", value ) ) { @@ -156,8 +229,8 @@ parse_options( int argc, char** argv ) } else if( !arg.empty() && arg.front() != '-' && !positional_agents ) { - options.agents = parse_int( "agents", arg ); - positional_agents = true; + options.agents = parse_int( "agents", arg ); + positional_agents = true; } else { @@ -170,19 +243,82 @@ parse_options( int argc, char** argv ) void print_usage() { - std::cout << "Usage: multi_agent_lqr [--agents N] [--solver NAME] [--strategy NAME] [--max-outer N]\n"; + std::cout << "Usage: multi_agent_lqr [--agents N] [--solvers NAMES] [--strategies NAMES] [--max-outer N]" + " [--scalars float,double] [--dump-trajectories]\n"; std::cout << " multi_agent_lqr N\n"; std::cout << '\n'; examples::print_available( std::cout ); } +template +void +run_for_scalar( const Options& options, const std::vector& solver_names, + const std::vector& strategy_names ) +{ + using Problem = mas::MultiAgentProblemT; + + const mas::SolverParamsT params{ { "max_iterations", static_cast( 100 ) }, + { "tolerance", static_cast( 1e-5 ) }, + { "max_ms", static_cast( 100 ) } }; + + constexpr int n_x = 4; + constexpr int n_u = 4; + constexpr int horizon = 10; + const Scalar dt = static_cast( 0.1 ); + const std::string scalar_label = examples::scalar_label(); + + for( const auto& solver_name : solver_names ) + { + if( !examples::solver_supported_for_scalar( solver_name ) ) + { + std::cout << "scalar=" << scalar_label << " solver=" << solver_name + << " unsupported (skipping)\n"; + continue; + } + + for( const auto& strategy_name : strategy_names ) + { + Problem problem = create_problem( options.agents, n_x, n_u, dt, horizon ); + auto solver = examples::make_solver( solver_name ); + auto strategy + = examples::make_strategy( strategy_name, std::move( solver ), params, options.max_outer ); + + const auto start = std::chrono::steady_clock::now(); + auto solution = mas::solve( strategy, problem ); + const auto end = std::chrono::steady_clock::now(); + const double elapsed_ms = std::chrono::duration( end - start ).count(); + + std::cout << std::fixed << std::setprecision( 6 ) + << "scalar=" << scalar_label + << " solver=" << solver_name + << " strategy=" << strategy_name + << " agents=" << options.agents + << " cost=" << static_cast( solution.total_cost ) + << " time_ms=" << elapsed_ms + << '\n'; + + if( options.dump_trajectories ) + { + if( problem.blocks.empty() ) + problem.compute_offsets(); + for( std::size_t idx = 0; idx < solution.states.size() && idx < problem.blocks.size(); ++idx ) + { + const auto& block = problem.blocks[idx]; + const auto& ocp = *block.agent->ocp; + const std::string base = "agent_" + std::to_string( block.agent_id ); + examples::print_state_trajectory( std::cout, solution.states[idx], ocp.dt, base ); + examples::print_control_trajectory( std::cout, solution.controls[idx], ocp.dt, base ); + } + } + } + } +} + } // namespace -/*──────────────────────────── main ──────────────────────────────*/ int main( int argc, char** argv ) { - using namespace mas; try { const Options options = parse_options( argc, argv ); @@ -192,49 +328,24 @@ main( int argc, char** argv ) return 0; } - constexpr int n_x = 4, n_u = 4, T = 10; - constexpr double dt = 0.1; - - MultiAgentProblem problem; - for( int i = 0; i < options.agents; ++i ) - { - auto ocp = std::make_shared( create_linear_lqr_ocp( n_x, n_u, dt, T ) ); - problem.add_agent( std::make_shared( i, ocp ) ); - } + const std::vector solver_names = options.solvers.empty() + ? examples::available_solver_names() + : options.solvers; + const std::vector default_strategies{ "centralized", "sequential", "linesearch", "trustregion" }; + const std::vector strategy_names + = options.strategies.empty() ? default_strategies : options.strategies; - SolverParams params{ - { "max_iterations", 100 }, - { "tolerance", 1e-5 }, - { "max_ms", 100 } - }; + const std::vector scalar_names + = options.scalars.empty() ? std::vector{ "float", "double" } : options.scalars; - auto solver = examples::make_solver( options.solver ); - Strategy strategy = examples::make_strategy( options.strategy, std::move( solver ), params, options.max_outer ); - const auto start = std::chrono::steady_clock::now(); - const auto solution = mas::solve( strategy, problem ); - const auto end = std::chrono::steady_clock::now(); - const double elapsed_ms = std::chrono::duration( end - start ).count(); - const std::string solver_name = examples::canonical_solver_name( options.solver ); - const std::string strategy_name = examples::canonical_strategy_name( options.strategy ); - - std::cout << std::fixed << std::setprecision( 6 ) - << "solver=" << solver_name - << " strategy=" << strategy_name - << " agents=" << options.agents - << " cost=" << solution.total_cost - << " time_ms=" << elapsed_ms - << '\n'; - - if( problem.blocks.empty() ) - problem.compute_offsets(); - - for( std::size_t idx = 0; idx < solution.states.size() && idx < problem.blocks.size(); ++idx ) - { - const auto& block = problem.blocks[idx]; - const auto& ocp = *block.agent->ocp; - const std::string base = "agent_" + std::to_string( block.agent_id ); - examples::print_state_trajectory( std::cout, solution.states[idx], ocp.dt, base ); - examples::print_control_trajectory( std::cout, solution.controls[idx], ocp.dt, base ); + for( const auto& scalar_name : scalar_names ) + { + if( scalar_name == "float" ) + run_for_scalar( options, solver_names, strategy_names ); + else if( scalar_name == "double" ) + run_for_scalar( options, solver_names, strategy_names ); + else + std::cout << "Unknown scalar '" << scalar_name << "' -- skipping\n"; } } catch( const std::exception& e ) diff --git a/examples/multi_agent_single_track.cpp b/examples/multi_agent_single_track.cpp index 3ee9159..9e649e6 100644 --- a/examples/multi_agent_single_track.cpp +++ b/examples/multi_agent_single_track.cpp @@ -1,8 +1,7 @@ -#include - #include #include #include +#include #include #include #include @@ -17,61 +16,128 @@ #include "multi_agent_solver/multi_agent_problem.hpp" #include "multi_agent_solver/solvers/solver.hpp" #include "multi_agent_solver/strategies/strategy.hpp" +#include "multi_agent_solver/types.hpp" -mas::OCP -create_single_track_circular_ocp( double initial_theta, double track_radius, double target_velocity, int time_steps ) +namespace { - using namespace mas; + +constexpr double kTwoPi = 6.28318530717958647692; + +template +mas::OCP +create_single_track_circular_ocp( Scalar initial_theta, Scalar track_radius, Scalar target_velocity, int time_steps ) +{ + using OCP = mas::OCP; + using State = typename OCP::State; + using Control = typename OCP::Control; + using StageCostFunction = typename OCP::StageCostFunction; + using TerminalCostFunction = typename OCP::TerminalCostFunction; + OCP problem; problem.state_dim = 4; problem.control_dim = 2; problem.horizon_steps = time_steps; - problem.dt = 0.5; - - double x0 = track_radius * cos( initial_theta ); - double y0 = track_radius * sin( initial_theta ); - problem.initial_state = Eigen::VectorXd::Zero( problem.state_dim ); - problem.initial_state << x0, y0, 1.57 + initial_theta, 4.0; - - problem.dynamics = single_track_model; - - problem.stage_cost = [target_velocity, track_radius]( const State& state, const Control& control, size_t ) { - const double w_track = 1.0, w_speed = 1.0, w_delta = 0.001, w_acc = 0.001; - double x = state( 0 ), y = state( 1 ), vx = state( 3 ); - double delta = control( 0 ), a_cmd = control( 1 ); - double distance_from_track = std::abs( std::sqrt( x * x + y * y ) - track_radius ); - double speed_error = vx - target_velocity; - return w_track * distance_from_track * distance_from_track + w_speed * speed_error * speed_error + w_delta * delta * delta + problem.dt = static_cast( 0.5 ); + + State initial_state = State::Zero( problem.state_dim ); + const Scalar x0 = track_radius * std::cos( initial_theta ); + const Scalar y0 = track_radius * std::sin( initial_theta ); + initial_state << x0, y0, static_cast( 1.57 ) + initial_theta, static_cast( 4.0 ); + problem.initial_state = initial_state; + + problem.dynamics = mas::single_track_model; + + const Scalar w_track = static_cast( 1.0 ); + const Scalar w_speed = static_cast( 1.0 ); + const Scalar w_delta = static_cast( 0.001 ); + const Scalar w_acc = static_cast( 0.001 ); + + problem.stage_cost = [=]( const State& state, const Control& control, std::size_t ) { + const Scalar x = state( 0 ); + const Scalar y = state( 1 ); + const Scalar vx = state( 3 ); + const Scalar delta = control( 0 ); + const Scalar a_cmd = control( 1 ); + const Scalar radius = std::sqrt( x * x + y * y ); + const Scalar distance = std::abs( radius - track_radius ); + const Scalar speed_error = vx - target_velocity; + return w_track * distance * distance + w_speed * speed_error * speed_error + w_delta * delta * delta + w_acc * a_cmd * a_cmd; }; - problem.terminal_cost = []( const State& ) { return 0.0; }; - problem.input_lower_bounds = Eigen::VectorXd::Constant( problem.control_dim, -0.5 ); - problem.input_upper_bounds = Eigen::VectorXd::Constant( problem.control_dim, 0.5 ); + problem.terminal_cost = []( const State& ) { return static_cast( 0 ); }; + + problem.input_lower_bounds = Control::Constant( problem.control_dim, static_cast( -0.5 ) ); + problem.input_upper_bounds = Control::Constant( problem.control_dim, static_cast( 0.5 ) ); problem.initialize_problem(); problem.verify_problem(); return problem; } +template +mas::MultiAgentProblemT +create_problem( int agents, Scalar track_radius, Scalar target_velocity, int time_steps ) +{ + mas::MultiAgentProblemT problem; + for( int i = 0; i < agents; ++i ) + { + const Scalar theta = static_cast( kTwoPi * static_cast( i ) / static_cast( agents ) ); + auto ocp = std::make_shared>( create_single_track_circular_ocp( + theta, track_radius, target_velocity, time_steps ) ); + problem.add_agent( std::make_shared>( static_cast( i ), ocp ) ); + } + return problem; +} + struct Options { - bool show_help = false; - int agents = 10; - int max_outer = 10; - std::string solver = "ilqr"; - std::string strategy = "centralized"; + bool show_help = false; + int agents = 10; + int max_outer = 10; + bool dump_trajectories = false; + std::vector solvers; + std::vector strategies; + std::vector scalars; }; -namespace +void +trim_in_place( std::string& value ) { + const auto first = value.find_first_not_of( " \t" ); + if( first == std::string::npos ) + { + value.clear(); + return; + } + const auto last = value.find_last_not_of( " \t" ); + value = value.substr( first, last - first + 1 ); +} + +void +append_list( std::vector& target, const std::string& csv, const auto& canonicalizer ) +{ + std::size_t start = 0; + while( start <= csv.size() ) + { + const auto comma = csv.find( ',', start ); + std::string token + = csv.substr( start, comma == std::string::npos ? std::string::npos : comma - start ); + trim_in_place( token ); + if( !token.empty() ) + target.push_back( canonicalizer( token ) ); + if( comma == std::string::npos ) + break; + start = comma + 1; + } +} int parse_int( const std::string& label, const std::string& value ) { - int result = 0; - const char* begin = value.data(); - const char* end = begin + value.size(); - const auto [ptr, ec] = std::from_chars( begin, end, result ); + int result = 0; + const char* begin = value.data(); + const char* end = begin + value.size(); + const auto [ptr, ec] = std::from_chars( begin, end, result ); if( ec != std::errc() || ptr != end ) throw std::invalid_argument( "Invalid value for " + label + ": '" + value + "'" ); return result; @@ -113,6 +179,11 @@ parse_options( int argc, char** argv ) options.show_help = true; continue; } + if( arg == "--dump-trajectories" ) + { + options.dump_trajectories = true; + continue; + } std::string value; if( match_with_value( "--agents", value ) ) @@ -121,11 +192,27 @@ parse_options( int argc, char** argv ) } else if( match_with_value( "--solver", value ) ) { - options.solver = value; + append_list( options.solvers, value, examples::canonical_solver_name ); + } + else if( match_with_value( "--solvers", value ) ) + { + append_list( options.solvers, value, examples::canonical_solver_name ); } else if( match_with_value( "--strategy", value ) ) { - options.strategy = value; + append_list( options.strategies, value, examples::canonical_strategy_name ); + } + else if( match_with_value( "--strategies", value ) ) + { + append_list( options.strategies, value, examples::canonical_strategy_name ); + } + else if( match_with_value( "--scalar", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); + } + else if( match_with_value( "--scalars", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); } else if( match_with_value( "--max-outer", value ) ) { @@ -147,18 +234,81 @@ parse_options( int argc, char** argv ) void print_usage() { - std::cout << "Usage: multi_agent_single_track [--agents N] [--solver NAME] [--strategy NAME] [--max-outer N]\n"; + std::cout << "Usage: multi_agent_single_track [--agents N] [--solvers NAMES] [--strategies NAMES] [--max-outer N]" + " [--scalars float,double] [--dump-trajectories]\n"; std::cout << " multi_agent_single_track N\n"; std::cout << '\n'; examples::print_available( std::cout ); } +template +void +run_for_scalar( const Options& options, const std::vector& solver_names, + const std::vector& strategy_names ) +{ + using Problem = mas::MultiAgentProblemT; + + const mas::SolverParamsT params{ { "max_iterations", static_cast( 100 ) }, + { "tolerance", static_cast( 1e-5 ) }, + { "max_ms", static_cast( 1000 ) } }; + + const Scalar track_radius = static_cast( 20.0 ); + const Scalar target_velocity = static_cast( 5.0 ); + constexpr int time_steps = 10; + const std::string scalar_label = examples::scalar_label(); + + for( const auto& solver_name : solver_names ) + { + if( !examples::solver_supported_for_scalar( solver_name ) ) + { + std::cout << "scalar=" << scalar_label << " solver=" << solver_name + << " unsupported (skipping)\n"; + continue; + } + + for( const auto& strategy_name : strategy_names ) + { + Problem problem = create_problem( options.agents, track_radius, target_velocity, time_steps ); + auto solver = examples::make_solver( solver_name ); + auto strategy + = examples::make_strategy( strategy_name, std::move( solver ), params, options.max_outer ); + + const auto start = std::chrono::steady_clock::now(); + auto solution = mas::solve( strategy, problem ); + const auto end = std::chrono::steady_clock::now(); + const double elapsed_ms = std::chrono::duration( end - start ).count(); + + std::cout << std::fixed << std::setprecision( 6 ) + << "scalar=" << scalar_label + << " solver=" << solver_name + << " strategy=" << strategy_name + << " agents=" << options.agents + << " cost=" << static_cast( solution.total_cost ) + << " time_ms=" << elapsed_ms + << '\n'; + + if( options.dump_trajectories ) + { + if( problem.blocks.empty() ) + problem.compute_offsets(); + for( std::size_t idx = 0; idx < solution.states.size() && idx < problem.blocks.size(); ++idx ) + { + const auto& block = problem.blocks[idx]; + const auto& ocp = *block.agent->ocp; + const std::string base = "agent_" + std::to_string( block.agent_id ); + examples::print_state_trajectory( std::cout, solution.states[idx], ocp.dt, base ); + examples::print_control_trajectory( std::cout, solution.controls[idx], ocp.dt, base ); + } + } + } + } +} + } // namespace int main( int argc, char** argv ) { - using namespace mas; try { const Options options = parse_options( argc, argv ); @@ -168,45 +318,24 @@ main( int argc, char** argv ) return 0; } - SolverParams params{ - { "max_iterations", 100 }, - { "tolerance", 1e-5 }, - { "max_ms", 1000 } - }; - constexpr int time_steps = 10; - constexpr double track_radius = 20.0; - constexpr double target_velocity = 5.0; - - MultiAgentProblem problem; - for( int i = 0; i < options.agents; ++i ) - { - double theta = 2.0 * M_PI * i / options.agents; - auto ocp = std::make_shared( create_single_track_circular_ocp( theta, track_radius, target_velocity, time_steps ) ); - problem.add_agent( std::make_shared( i, ocp ) ); - } - - auto solver = examples::make_solver( options.solver ); - Strategy strategy = examples::make_strategy( options.strategy, std::move( solver ), params, options.max_outer ); - const auto start = std::chrono::steady_clock::now(); - const auto solution = mas::solve( strategy, problem ); - const auto end = std::chrono::steady_clock::now(); - const double elapsed_ms = std::chrono::duration( end - start ).count(); - const std::string solver_name = examples::canonical_solver_name( options.solver ); - const std::string strategy_name = examples::canonical_strategy_name( options.strategy ); - - std::cout << std::fixed << std::setprecision( 6 ) << "solver=" << solver_name << " strategy=" << strategy_name - << " agents=" << options.agents << " cost=" << solution.total_cost << " time_ms=" << elapsed_ms << '\n'; + const std::vector solver_names = options.solvers.empty() + ? examples::available_solver_names() + : options.solvers; + const std::vector default_strategies{ "centralized", "sequential", "linesearch", "trustregion" }; + const std::vector strategy_names + = options.strategies.empty() ? default_strategies : options.strategies; - if( problem.blocks.empty() ) - problem.compute_offsets(); + const std::vector scalar_names + = options.scalars.empty() ? std::vector{ "float", "double" } : options.scalars; - for( std::size_t idx = 0; idx < solution.states.size() && idx < problem.blocks.size(); ++idx ) + for( const auto& scalar_name : scalar_names ) { - const auto& block = problem.blocks[idx]; - const auto& ocp = *block.agent->ocp; - const std::string base = "agent_" + std::to_string( block.agent_id ); - examples::print_state_trajectory( std::cout, solution.states[idx], ocp.dt, base ); - examples::print_control_trajectory( std::cout, solution.controls[idx], ocp.dt, base ); + if( scalar_name == "float" ) + run_for_scalar( options, solver_names, strategy_names ); + else if( scalar_name == "double" ) + run_for_scalar( options, solver_names, strategy_names ); + else + std::cout << "Unknown scalar '" << scalar_name << "' -- skipping\n"; } } catch( const std::exception& e ) diff --git a/examples/pendulum_swing_up.cpp b/examples/pendulum_swing_up.cpp index 08e9a98..5122d01 100644 --- a/examples/pendulum_swing_up.cpp +++ b/examples/pendulum_swing_up.cpp @@ -1,10 +1,12 @@ -#include - +#include #include +#include #include #include +#include #include #include +#include #include "example_utils.hpp" #include "models/pendulum_model.hpp" @@ -12,72 +14,49 @@ #include "multi_agent_solver/solvers/solver.hpp" #include "multi_agent_solver/types.hpp" -mas::OCP +namespace +{ + +template +mas::OCP create_pendulum_swingup_ocp() { - using namespace mas; - OCP problem; + using OCP = mas::OCP; + using State = typename OCP::State; + using Control = typename OCP::Control; + OCP problem; problem.state_dim = 2; problem.control_dim = 1; problem.horizon_steps = 100; - problem.dt = 0.05; + problem.dt = static_cast( 0.05 ); - problem.initial_state = Eigen::Vector2d::Zero(); + problem.initial_state = State::Zero( problem.state_dim ); + problem.dynamics = mas::pendulum_dynamics; - problem.dynamics = pendulum_dynamics; + const Scalar theta_goal = std::numbers::pi_v; + const Scalar w_theta = static_cast( 10.0 ); + const Scalar w_omega = static_cast( 10.0 ); + const Scalar w_torque = static_cast( 0.01 ); + const Scalar torque_max = static_cast( 5.0 ); - const double theta_goal = M_PI; - const double w_theta = 10.0; - const double w_omega = 10.0; - const double w_torque = 0.01; - const double torque_max = 5.0; - - problem.stage_cost = [=]( const State& x, const Control& u, size_t t_idx ) { - double theta = x( 0 ); - double omega = x( 1 ); - double tau = u( 0 ); - return ( w_theta * std::pow( theta - theta_goal, 2 ) + w_omega * std::pow( omega, 2 ) + w_torque * std::pow( tau, 2 ) ) / 100.0; + problem.stage_cost = [=]( const State& x, const Control& u, std::size_t ) { + const Scalar theta = x( 0 ); + const Scalar omega = x( 1 ); + const Scalar tau = u( 0 ); + const Scalar theta_err = theta - theta_goal; + return ( w_theta * theta_err * theta_err + w_omega * omega * omega + w_torque * tau * tau ) + / static_cast( 100.0 ); }; problem.terminal_cost = [=]( const State& x ) { - double theta = x( 0 ); - double omega = x( 1 ); - return w_theta * std::pow( theta - theta_goal, 2 ) + w_omega * std::pow( omega, 2 ); + const Scalar theta = x( 0 ); + const Scalar omega = x( 1 ); + const Scalar theta_err = theta - theta_goal; + return w_theta * theta_err * theta_err + w_omega * omega * omega; }; - // problem.cost_state_gradient = [=]( const StageCostFunction&, const State& x, const Control&, size_t ) { - // Eigen::Vector2d grad; - // grad( 0 ) = 2.0 * w_theta * ( x( 0 ) - theta_goal ); - // grad( 1 ) = 2.0 * w_omega * x( 1 ); - // return grad; - // }; - - // problem.cost_control_gradient = [=]( const StageCostFunction&, const State&, const Control& u, size_t ) { - // Eigen::VectorXd grad = Eigen::VectorXd::Zero( 1 ); - // grad( 0 ) = 2.0 * w_torque * u( 0 ); - // return grad; - // }; - - // problem.cost_state_hessian = [=]( const StageCostFunction&, const State&, const Control&, size_t ) { - // Eigen::MatrixXd H = Eigen::MatrixXd::Zero( 2, 2 ); - // H( 0, 0 ) = 2.0 * w_theta; - // H( 1, 1 ) = 2.0 * w_omega; - // return H; - // }; - - // problem.cost_control_hessian = [=]( const StageCostFunction&, const State&, const Control&, size_t ) { - // Eigen::MatrixXd H = Eigen::MatrixXd::Zero( 1, 1 ); - // H( 0, 0 ) = 2.0 * w_torque; - // return H; - // }; - - // problem.dynamics_state_jacobian = []( const MotionModel&, const State& x, const Control& u ) { return pendulum_state_jacobian( x, u ); - // }; problem.dynamics_control_jacobian = []( const MotionModel&, const State& x, const Control& u ) { - // return pendulum_control_jacobian( x, u ); - // }; - - Eigen::VectorXd lower( 1 ), upper( 1 ); + Control lower( 1 ), upper( 1 ); lower << -torque_max; upper << torque_max; problem.input_lower_bounds = lower; @@ -90,12 +69,42 @@ create_pendulum_swingup_ocp() struct Options { - bool show_help = false; - std::string solver = "ilqr"; + bool show_help = false; + bool dump_trajectories = false; + std::vector solvers; + std::vector scalars; }; -namespace +void +trim_in_place( std::string& value ) { + const auto first = value.find_first_not_of( " \t" ); + if( first == std::string::npos ) + { + value.clear(); + return; + } + const auto last = value.find_last_not_of( " \t" ); + value = value.substr( first, last - first + 1 ); +} + +void +append_list( std::vector& target, const std::string& csv, const auto& canonicalizer ) +{ + std::size_t start = 0; + while( start <= csv.size() ) + { + const auto comma = csv.find( ',', start ); + std::string token + = csv.substr( start, comma == std::string::npos ? std::string::npos : comma - start ); + trim_in_place( token ); + if( !token.empty() ) + target.push_back( canonicalizer( token ) ); + if( comma == std::string::npos ) + break; + start = comma + 1; + } +} Options parse_options( int argc, char** argv ) @@ -103,8 +112,14 @@ parse_options( int argc, char** argv ) Options options; for( int i = 1; i < argc; ++i ) { - std::string arg = argv[i]; - auto match_with_value = [&]( const std::string& name, std::string& out ) { + std::string arg = argv[i]; + if( arg.rfind( "--", 0 ) == 0 ) + { + const auto eq_pos = arg.find( '=' ); + const auto end = eq_pos == std::string::npos ? arg.size() : eq_pos; + std::replace( arg.begin() + 2, arg.begin() + static_cast( end ), '_', '-' ); + } + auto match_with_value = [&]( const std::string& name, std::string& out ) { const std::string prefix = name + "="; if( arg == name ) { @@ -126,11 +141,28 @@ parse_options( int argc, char** argv ) options.show_help = true; continue; } + if( arg == "--dump-trajectories" ) + { + options.dump_trajectories = true; + continue; + } std::string value; if( match_with_value( "--solver", value ) ) { - options.solver = value; + append_list( options.solvers, value, examples::canonical_solver_name ); + } + else if( match_with_value( "--solvers", value ) ) + { + append_list( options.solvers, value, examples::canonical_solver_name ); + } + else if( match_with_value( "--scalar", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); + } + else if( match_with_value( "--scalars", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); } else { @@ -143,17 +175,58 @@ parse_options( int argc, char** argv ) void print_usage() { - std::cout << "Usage: pendulum_swing_up [--solver NAME]\n"; + std::cout << "Usage: pendulum_swing_up [--solvers NAMES] [--scalars float,double] [--dump-trajectories]\n"; std::cout << '\n'; examples::print_available( std::cout ); } +template +void +run_for_scalar( const Options& options, const std::vector& solver_names ) +{ + const mas::SolverParamsT params{ { "max_iterations", static_cast( 500 ) }, + { "tolerance", static_cast( 1e-5 ) }, + { "max_ms", static_cast( 1000 ) } }; + + const std::string scalar_label = examples::scalar_label(); + + for( const auto& solver_name : solver_names ) + { + if( !examples::solver_supported_for_scalar( solver_name ) ) + { + std::cout << "scalar=" << scalar_label << " solver=" << solver_name << " unsupported (skipping)\n"; + continue; + } + + auto problem = create_pendulum_swingup_ocp(); + auto solver = examples::make_solver( solver_name ); + mas::set_params( solver, params ); + + const auto start = std::chrono::steady_clock::now(); + mas::solve( solver, problem ); + const auto end = std::chrono::steady_clock::now(); + const double elapsed_ms = std::chrono::duration( end - start ).count(); + + std::cout << std::fixed << std::setprecision( 6 ) + << "scalar=" << scalar_label + << " solver=" << solver_name + << " cost=" << static_cast( problem.best_cost ) + << " time_ms=" << elapsed_ms + << '\n'; + + if( options.dump_trajectories ) + { + examples::print_state_trajectory( std::cout, problem.best_states, problem.dt, "pendulum" ); + examples::print_control_trajectory( std::cout, problem.best_controls, problem.dt, "pendulum" ); + } + } +} + } // namespace int main( int argc, char** argv ) { - using namespace mas; try { const Options options = parse_options( argc, argv ); @@ -163,27 +236,21 @@ main( int argc, char** argv ) return 0; } - OCP problem = create_pendulum_swingup_ocp(); - - SolverParams params; - params["max_iterations"] = 500; - params["tolerance"] = 1e-5; - params["max_ms"] = 1000; - - auto solver = examples::make_solver( options.solver ); - mas::set_params( solver, params ); + const std::vector solver_names = options.solvers.empty() + ? examples::available_solver_names() + : options.solvers; + const std::vector scalar_names + = options.scalars.empty() ? std::vector{ "float", "double" } : options.scalars; - const auto start = std::chrono::steady_clock::now(); - mas::solve( solver, problem ); - const auto end = std::chrono::steady_clock::now(); - const double elapsed_ms = std::chrono::duration( end - start ).count(); - - const std::string solver_name = examples::canonical_solver_name( options.solver ); - std::cout << std::fixed << std::setprecision( 6 ) << "solver=" << solver_name << " cost=" << problem.best_cost - << " time_ms=" << elapsed_ms << '\n'; - - examples::print_state_trajectory( std::cout, problem.best_states, problem.dt, "pendulum" ); - examples::print_control_trajectory( std::cout, problem.best_controls, problem.dt, "pendulum" ); + for( const auto& scalar_name : scalar_names ) + { + if( scalar_name == "float" ) + run_for_scalar( options, solver_names ); + else if( scalar_name == "double" ) + run_for_scalar( options, solver_names ); + else + std::cout << "Unknown scalar '" << scalar_name << "' -- skipping\n"; + } } catch( const std::exception& e ) { @@ -192,4 +259,4 @@ main( int argc, char** argv ) return 1; } return 0; -} \ No newline at end of file +} diff --git a/examples/rocket_max_altitude.cpp b/examples/rocket_max_altitude.cpp index d176d05..d87a930 100644 --- a/examples/rocket_max_altitude.cpp +++ b/examples/rocket_max_altitude.cpp @@ -1,6 +1,11 @@ +#include +#include +#include #include +#include #include #include +#include #include "example_utils.hpp" #include "models/rocket_model.hpp" @@ -8,104 +13,110 @@ #include "multi_agent_solver/solvers/solver.hpp" #include "multi_agent_solver/types.hpp" -namespace mas +namespace { -mas::OCP +template +mas::OCP create_max_altitude_rocket_ocp() { - RocketParameters params; - params.initial_mass = 1.0; - params.gravity = 9.81; - params.exhaust_velocity = 50.0; + using OCP = mas::OCP; + using State = typename OCP::State; + using Control = typename OCP::Control; + + mas::RocketParametersT params; + params.initial_mass = static_cast( 1.0 ); + params.gravity = static_cast( 9.81 ); + params.exhaust_velocity = static_cast( 50.0 ); OCP problem; problem.state_dim = 3; problem.control_dim = 1; problem.horizon_steps = 50; - problem.dt = 0.1; + problem.dt = static_cast( 0.1 ); problem.initial_state = State::Zero( problem.state_dim ); problem.initial_state( 2 ) = params.initial_mass; - problem.dynamics = make_rocket_dynamics( params ); + problem.dynamics = mas::make_rocket_dynamics( params ); - const double max_thrust = 20.0; // [N] - const double w_thrust = 5e-3; - const double w_terminal_altitude = 15.0; - const double w_terminal_velocity = 2.0; - const double desired_terminal_vel = 0.0; + const Scalar max_thrust = static_cast( 20.0 ); + const Scalar w_thrust = static_cast( 5e-3 ); + const Scalar w_terminal_altitude = static_cast( 15.0 ); + const Scalar w_terminal_velocity = static_cast( 2.0 ); + const Scalar desired_terminal_vel = static_cast( 0.0 ); - problem.stage_cost = [=]( const State& state, const Control& control, size_t ) { - const double thrust = control( 0 ); - return 0.5 * w_thrust * thrust * thrust; + problem.stage_cost = [=]( const State&, const Control& control, std::size_t ) { + const Scalar thrust = control( 0 ); + return static_cast( 0.5 ) * w_thrust * thrust * thrust; }; - - problem.cost_control_gradient = [=]( const StageCostFunction&, const State&, const Control& control, size_t ) { - Eigen::VectorXd gradient( 1 ); + problem.cost_control_gradient = [=]( const typename OCP::StageCostFunction&, const State&, const Control& control, + std::size_t ) { + Eigen::Matrix gradient( 1 ); gradient( 0 ) = w_thrust * control( 0 ); return gradient; }; - problem.cost_control_hessian = [=]( const StageCostFunction&, const State&, const Control&, size_t ) { - Eigen::MatrixXd hessian( 1, 1 ); + problem.cost_control_hessian = [=]( const typename OCP::StageCostFunction&, const State&, const Control&, std::size_t ) { + Eigen::Matrix hessian( 1, 1 ); hessian( 0, 0 ) = w_thrust; return hessian; }; - problem.cost_state_gradient = []( const StageCostFunction&, const State& state, const Control&, size_t ) { - return Eigen::VectorXd::Zero( state.size() ); + problem.cost_state_gradient = []( const typename OCP::StageCostFunction&, const State& state, const Control&, std::size_t ) { + return Eigen::Matrix::Zero( state.size() ); }; - problem.cost_state_hessian = []( const StageCostFunction&, const State& state, const Control&, size_t ) { - return Eigen::MatrixXd::Zero( state.size(), state.size() ); + problem.cost_state_hessian = []( const typename OCP::StageCostFunction&, const State& state, const Control&, std::size_t ) { + return Eigen::Matrix::Zero( state.size(), state.size() ); }; problem.terminal_cost = [=]( const State& state ) { - const double altitude = state( 0 ); - const double velocity_error = state( 1 ) - desired_terminal_vel; - return -w_terminal_altitude * altitude + 0.5 * w_terminal_velocity * velocity_error * velocity_error; + const Scalar altitude = state( 0 ); + const Scalar velocity_error = state( 1 ) - desired_terminal_vel; + return -w_terminal_altitude * altitude + static_cast( 0.5 ) * w_terminal_velocity * velocity_error * velocity_error; }; - problem.terminal_cost_gradient = [=]( const TerminalCostFunction&, const State& state ) { - Eigen::VectorXd gradient = Eigen::VectorXd::Zero( state.size() ); - gradient( 0 ) = -w_terminal_altitude; - gradient( 1 ) = w_terminal_velocity * ( state( 1 ) - desired_terminal_vel ); + problem.terminal_cost_gradient = [=]( const typename OCP::TerminalCostFunction&, const State& state ) { + Eigen::Matrix gradient = Eigen::Matrix::Zero( state.size() ); + gradient( 0 ) = -w_terminal_altitude; + gradient( 1 ) = w_terminal_velocity * ( state( 1 ) - desired_terminal_vel ); return gradient; }; - problem.terminal_cost_hessian = [=]( const TerminalCostFunction&, const State& state ) { - Eigen::MatrixXd hessian = Eigen::MatrixXd::Zero( state.size(), state.size() ); - hessian( 1, 1 ) = w_terminal_velocity; + problem.terminal_cost_hessian = [=]( const typename OCP::TerminalCostFunction&, const State& state ) { + Eigen::Matrix hessian + = Eigen::Matrix::Zero( state.size(), state.size() ); + hessian( 1, 1 ) = w_terminal_velocity; return hessian; }; - - problem.dynamics_state_jacobian = [=]( const MotionModel&, const State& state, const Control& control ) { - return rocket_state_jacobian( params, state, control ); + problem.dynamics_state_jacobian = [=]( const typename OCP::MotionModel&, const State& state, const Control& control ) { + return mas::rocket_state_jacobian( params, state, control ); }; - problem.dynamics_control_jacobian = [=]( const MotionModel&, const State& state, const Control& control ) { - return rocket_control_jacobian( params, state, control ); + problem.dynamics_control_jacobian = [=]( const typename OCP::MotionModel&, const State& state, const Control& control ) { + return mas::rocket_control_jacobian( params, state, control ); }; - Eigen::VectorXd u_lower( 1 ), u_upper( 1 ); - u_lower << 0.0; + Control u_lower( 1 ), u_upper( 1 ); + u_lower << static_cast( 0.0 ); u_upper << max_thrust; problem.input_lower_bounds = u_lower; problem.input_upper_bounds = u_upper; - Eigen::VectorXd x_lower = Eigen::VectorXd::Constant( problem.state_dim, std::numeric_limits::min() ); - x_lower( 2 ) = 0.0; + State x_lower = State::Constant( problem.state_dim, std::numeric_limits::lowest() ); + x_lower( 2 ) = static_cast( 0.0 ); problem.state_lower_bounds = x_lower; - Eigen::VectorXd x_upper = Eigen::VectorXd::Constant( problem.state_dim, std::numeric_limits::max() ); - x_upper( 2 ) = params.initial_mass; + State x_upper = State::Constant( problem.state_dim, std::numeric_limits::max() ); + x_upper( 2 ) = params.initial_mass; problem.state_upper_bounds = x_upper; - // initialize controls with just constant steady thrust - problem.initial_controls = ControlTrajectory::Constant( problem.control_dim, problem.horizon_steps, max_thrust / 2.0 ); + problem.initial_controls + = OCP::ControlTrajectory::Constant( problem.control_dim, problem.horizon_steps, + max_thrust / static_cast( 2.0 ) ); problem.initialize_problem(); problem.verify_problem(); @@ -115,45 +126,105 @@ create_max_altitude_rocket_ocp() struct Options { - bool show_help = false; - bool dump_traces = false; - std::string solver = "osqp"; + bool show_help = false; + bool dump_trajectories = false; + std::vector solvers; + std::vector scalars; }; +void +trim_in_place( std::string& value ) +{ + const auto first = value.find_first_not_of( " \t" ); + if( first == std::string::npos ) + { + value.clear(); + return; + } + const auto last = value.find_last_not_of( " \t" ); + value = value.substr( first, last - first + 1 ); +} + +void +append_list( std::vector& target, const std::string& csv, const auto& canonicalizer ) +{ + std::size_t start = 0; + while( start <= csv.size() ) + { + const auto comma = csv.find( ',', start ); + std::string token + = csv.substr( start, comma == std::string::npos ? std::string::npos : comma - start ); + trim_in_place( token ); + if( !token.empty() ) + target.push_back( canonicalizer( token ) ); + if( comma == std::string::npos ) + break; + start = comma + 1; + } +} + Options parse_options( int argc, char** argv ) { Options options; for( int i = 1; i < argc; ++i ) { - const std::string arg = argv[i]; + std::string arg = argv[i]; + if( arg.rfind( "--", 0 ) == 0 ) + { + const auto eq_pos = arg.find( '=' ); + const auto end = eq_pos == std::string::npos ? arg.size() : eq_pos; + std::replace( arg.begin() + 2, arg.begin() + static_cast( end ), '_', '-' ); + } + auto match_with_value = [&]( const std::string& name, std::string& out ) { + const std::string prefix = name + "="; + if( arg == name ) + { + if( i + 1 >= argc ) + throw std::invalid_argument( "Missing value for option '" + name + "'" ); + out = argv[++i]; + return true; + } + if( arg.rfind( prefix, 0 ) == 0 ) + { + out = arg.substr( prefix.size() ); + return true; + } + return false; + }; + if( arg == "--help" || arg == "-h" ) { options.show_help = true; continue; } - if( arg == "--dump" ) + if( arg == "--dump" || arg == "--dump-trajectories" ) { - options.dump_traces = true; + options.dump_trajectories = true; continue; } - const std::string solver_prefix = "--solver="; - if( arg.rfind( solver_prefix, 0 ) == 0 ) + std::string value; + if( match_with_value( "--solver", value ) ) { - options.solver = arg.substr( solver_prefix.size() ); - continue; + append_list( options.solvers, value, examples::canonical_solver_name ); } - - if( arg == "--solver" ) + else if( match_with_value( "--solvers", value ) ) { - if( i + 1 >= argc ) - throw std::invalid_argument( "Missing value for --solver" ); - options.solver = argv[++i]; - continue; + append_list( options.solvers, value, examples::canonical_solver_name ); + } + else if( match_with_value( "--scalar", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); + } + else if( match_with_value( "--scalars", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); + } + else + { + throw std::invalid_argument( "Unknown argument '" + arg + "'" ); } - - throw std::invalid_argument( "Unknown argument '" + arg + "'" ); } return options; } @@ -161,18 +232,58 @@ parse_options( int argc, char** argv ) void print_usage() { - std::cout << "Usage: rocket_max_altitude [--solver NAME] [--dump]\n"; + std::cout << "Usage: rocket_max_altitude [--solvers NAMES] [--scalars float,double] [--dump]\n"; std::cout << '\n'; examples::print_available( std::cout ); } -} // namespace mas +template +void +run_for_scalar( const Options& options, const std::vector& solver_names ) +{ + const mas::SolverParamsT params{ { "max_iterations", static_cast( 25 ) }, + { "tolerance", static_cast( 1e-6 ) }, + { "max_ms", static_cast( 200 ) } }; + + const std::string scalar_label = examples::scalar_label(); + + for( const auto& solver_name : solver_names ) + { + if( !examples::solver_supported_for_scalar( solver_name ) ) + { + std::cout << "scalar=" << scalar_label << " solver=" << solver_name << " unsupported (skipping)\n"; + continue; + } + + auto problem = create_max_altitude_rocket_ocp(); + auto solver = examples::make_solver( solver_name ); + mas::set_params( solver, params ); + + const auto start = std::chrono::steady_clock::now(); + mas::solve( solver, problem ); + const auto end = std::chrono::steady_clock::now(); + const double elapsed_ms = std::chrono::duration( end - start ).count(); + + std::cout << std::fixed << std::setprecision( 6 ) + << "scalar=" << scalar_label + << " solver=" << solver_name + << " cost=" << static_cast( problem.best_cost ) + << " time_ms=" << elapsed_ms + << '\n'; + + if( options.dump_trajectories ) + { + examples::print_state_trajectory( std::cout, problem.best_states, problem.dt, "rocket" ); + examples::print_control_trajectory( std::cout, problem.best_controls, problem.dt, "rocket" ); + } + } +} + +} // namespace int main( int argc, char** argv ) { - using namespace mas; - try { const Options options = parse_options( argc, argv ); @@ -182,33 +293,21 @@ main( int argc, char** argv ) return 0; } - OCP problem = create_max_altitude_rocket_ocp(); - - SolverParams params; - params["max_iterations"] = 25; - params["tolerance"] = 1e-6; - params["max_ms"] = 200; - - Solver solver = examples::make_solver( options.solver ); - set_params( solver, params ); - const auto start = std::chrono::steady_clock::now(); - mas::solve( solver, problem ); - const auto end = std::chrono::steady_clock::now(); - const double elapsed_ms = std::chrono::duration( end - start ).count(); - const auto& X = problem.best_states; - const int T = problem.horizon_steps; - - const double final_altitude = X( 0, T ); - const double final_velocity = X( 1, T ); - const double final_mass = X( 2, T ); - - const std::string solver_name = examples::canonical_solver_name( options.solver ); - std::cout << std::fixed << std::setprecision( 6 ) << "solver=" << solver_name << " cost=" << problem.best_cost - << " time_ms=" << elapsed_ms << '\n'; - + const std::vector solver_names = options.solvers.empty() + ? examples::available_solver_names() + : options.solvers; + const std::vector scalar_names + = options.scalars.empty() ? std::vector{ "float", "double" } : options.scalars; - examples::print_state_trajectory( std::cout, problem.best_states, problem.dt, "rocket" ); - examples::print_control_trajectory( std::cout, problem.best_controls, problem.dt, "rocket" ); + for( const auto& scalar_name : scalar_names ) + { + if( scalar_name == "float" ) + run_for_scalar( options, solver_names ); + else if( scalar_name == "double" ) + run_for_scalar( options, solver_names ); + else + std::cout << "Unknown scalar '" << scalar_name << "' -- skipping\n"; + } } catch( const std::exception& ex ) { @@ -216,4 +315,4 @@ main( int argc, char** argv ) return 1; } return 0; -} \ No newline at end of file +} diff --git a/examples/single_track_ocp.cpp b/examples/single_track_ocp.cpp index 43e3168..a55a10b 100644 --- a/examples/single_track_ocp.cpp +++ b/examples/single_track_ocp.cpp @@ -1,8 +1,10 @@ +#include #include #include #include #include #include +#include #include "example_utils.hpp" #include "models/single_track_model.hpp" @@ -10,104 +12,98 @@ #include "multi_agent_solver/solvers/solver.hpp" #include "multi_agent_solver/types.hpp" -mas::OCP +namespace +{ + +template +mas::OCP create_single_track_lane_following_ocp() { - using namespace mas; - OCP problem; + using OCP = mas::OCP; + using State = typename OCP::State; + using Control = typename OCP::Control; - // Dimensions + OCP problem; problem.state_dim = 4; problem.control_dim = 2; problem.horizon_steps = 80; - problem.dt = 0.1; // 0.1 s per step. + problem.dt = static_cast( 0.1 ); - // Initial state: for example, X=1, Y=1, psi=1, vx=1 - problem.initial_state = Eigen::VectorXd::Zero( problem.state_dim ); - problem.initial_state << 0.0, 1.0, 0.0, 0.0; + State initial_state = State::Zero( problem.state_dim ); + initial_state << static_cast( 0.0 ), static_cast( 1.0 ), static_cast( 0.0 ), static_cast( 0.0 ); + problem.initial_state = initial_state; - // Dynamics: use the dynamic_bicycle_model defined in your code. - problem.dynamics = single_track_model; + problem.dynamics = mas::single_track_model; - // Desired velocity. - const double desired_velocity = 1.0; // [m/s] + const Scalar desired_velocity = static_cast( 1.0 ); + const Scalar w_lane = static_cast( 10.0 ); + const Scalar w_speed = static_cast( 1.0 ); + const Scalar w_delta = static_cast( 0.1 ); + const Scalar w_acc = static_cast( 0.1 ); - // Cost weights. - const double w_lane = 10.0; // Penalize lateral deviation. - const double w_speed = 1.0; // Penalize speed error. - const double w_delta = 0.1; // Penalize steering. - const double w_acc = 0.1; // Penalize acceleration. + problem.stage_cost = [=]( const State& state, const Control& control, std::size_t ) { + const Scalar y = state( 1 ); + const Scalar vx = state( 3 ); + const Scalar delta = control( 0 ); + const Scalar a_command = control( 1 ); - // Stage cost function. - problem.stage_cost = [=]( const State& state, const Control& control, size_t idx ) -> double { - // Unpack state: we only use Y (index 1) and vx (index 3). - double y = state( 1 ); - double vx = state( 3 ); - - // Unpack control: steering delta and acceleration. - double delta = control( 0 ); - double a_cmd = control( 1 ); - - // Compute errors. - double lane_error = y; - double speed_error = ( vx - desired_velocity ); - - double cost = w_lane * ( lane_error * lane_error ) + w_speed * ( speed_error * speed_error ) + w_delta * ( delta * delta ) - + w_acc * ( a_cmd * a_cmd ); - return cost; - }; + const Scalar lane_error = y; + const Scalar speed_error = vx - desired_velocity; - // Terminal cost (set to zero here, can be modified if needed). - problem.terminal_cost = [=]( const State& state ) -> double { return 0.0; }; - problem.cost_state_gradient = [=]( const StageCostFunction&, const State& state, const Control&, size_t time_idx ) -> Eigen::VectorXd { - Eigen::VectorXd grad = Eigen::VectorXd::Zero( state.size() ); - // Only Y (index 1) and vx (index 3) appear in the cost. - grad( 1 ) = 2.0 * w_lane * state( 1 ); - grad( 3 ) = 2.0 * w_speed * ( state( 3 ) - desired_velocity ); - return grad; + return w_lane * lane_error * lane_error + w_speed * speed_error * speed_error + w_delta * delta * delta + + w_acc * a_command * a_command; }; - // Gradient with respect to control. - problem.cost_control_gradient = [=]( const StageCostFunction&, const State&, const Control& control, - size_t time_idx ) -> Eigen::VectorXd { - Eigen::VectorXd grad = Eigen::VectorXd::Zero( control.size() ); - grad( 0 ) = 2.0 * w_delta * control( 0 ); - grad( 1 ) = 2.0 * w_acc * control( 1 ); - return grad; + problem.terminal_cost = []( const State& ) { return static_cast( 0 ); }; + + problem.cost_state_gradient + = [=]( const typename OCP::StageCostFunction&, const State& state, const Control&, std::size_t ) { + Eigen::Matrix grad = Eigen::Matrix::Zero( state.size() ); + grad( 1 ) = static_cast( 2.0 ) * w_lane * state( 1 ); + grad( 3 ) = static_cast( 2.0 ) * w_speed * ( state( 3 ) - desired_velocity ); + return grad; + }; + + problem.cost_control_gradient + = [=]( const typename OCP::StageCostFunction&, const State&, const Control& control, std::size_t ) { + Eigen::Matrix grad = Eigen::Matrix::Zero( control.size() ); + grad( 0 ) = static_cast( 2.0 ) * w_delta * control( 0 ); + grad( 1 ) = static_cast( 2.0 ) * w_acc * control( 1 ); + return grad; + }; + + problem.cost_state_hessian + = [=]( const typename OCP::StageCostFunction&, const State& state, const Control&, std::size_t ) { + Eigen::Matrix H + = Eigen::Matrix::Zero( state.size(), state.size() ); + H( 1, 1 ) = static_cast( 2.0 ) * w_lane; + H( 3, 3 ) = static_cast( 2.0 ) * w_speed; + return H; + }; + + problem.cost_control_hessian + = [=]( const typename OCP::StageCostFunction&, const State&, const Control&, std::size_t ) { + Eigen::Matrix H + = Eigen::Matrix::Zero( 2, 2 ); + H( 0, 0 ) = static_cast( 2.0 ) * w_delta; + H( 1, 1 ) = static_cast( 2.0 ) * w_acc; + return H; + }; + + problem.dynamics_state_jacobian = []( const typename OCP::MotionModel&, const State& x, const Control& u ) { + return mas::single_track_state_jacobian( x, u ); }; - // Hessian with respect to state. - problem.cost_state_hessian = [=]( const StageCostFunction&, const State& state, const Control&, size_t ) -> Eigen::MatrixXd { - Eigen::MatrixXd H = Eigen::MatrixXd::Zero( state.size(), state.size() ); - H( 1, 1 ) = 2.0 * w_lane; - H( 3, 3 ) = 2.0 * w_speed; - return H; + problem.dynamics_control_jacobian = []( const typename OCP::MotionModel&, const State& x, const Control& u ) { + return mas::single_track_control_jacobian( x, u ); }; - // Hessian with respect to control. - problem.cost_control_hessian = [=]( const StageCostFunction&, const State&, const Control&, size_t time_idx ) -> Eigen::MatrixXd { - Eigen::MatrixXd H = Eigen::MatrixXd::Zero( 2, 2 ); - H( 0, 0 ) = 2.0 * w_delta; - H( 1, 1 ) = 2.0 * w_acc; - return H; - }; - - - problem.dynamics_state_jacobian = []( const MotionModel& /*dyn*/, const State& x, const Control& u ) -> Eigen::MatrixXd { - return single_track_state_jacobian( x, u ); - }; - problem.dynamics_control_jacobian = []( const MotionModel& /*dyn*/, const State& x, const Control& u ) -> Eigen::MatrixXd { - return single_track_control_jacobian( x, u ); - }; - - - Eigen::VectorXd lower_bounds( 2 ), upper_bounds( 2 ); - lower_bounds << -0.7, -1.0; - upper_bounds << 0.7, 1.0; + Control lower_bounds( problem.control_dim ), upper_bounds( problem.control_dim ); + lower_bounds << static_cast( -0.7 ), static_cast( -1.0 ); + upper_bounds << static_cast( 0.7 ), static_cast( 1.0 ); problem.input_lower_bounds = lower_bounds; problem.input_upper_bounds = upper_bounds; - // Initialize and verify the problem. problem.initialize_problem(); problem.verify_problem(); @@ -116,12 +112,42 @@ create_single_track_lane_following_ocp() struct Options { - bool show_help = false; - std::string solver = "ilqr"; + bool show_help = false; + bool dump_trajectories = false; + std::vector solvers; + std::vector scalars; }; -namespace +void +trim_in_place( std::string& value ) { + const auto first = value.find_first_not_of( " \t" ); + if( first == std::string::npos ) + { + value.clear(); + return; + } + const auto last = value.find_last_not_of( " \t" ); + value = value.substr( first, last - first + 1 ); +} + +void +append_list( std::vector& target, const std::string& csv, const auto& canonicalizer ) +{ + std::size_t start = 0; + while( start <= csv.size() ) + { + const auto comma = csv.find( ',', start ); + std::string token + = csv.substr( start, comma == std::string::npos ? std::string::npos : comma - start ); + trim_in_place( token ); + if( !token.empty() ) + target.push_back( canonicalizer( token ) ); + if( comma == std::string::npos ) + break; + start = comma + 1; + } +} Options parse_options( int argc, char** argv ) @@ -129,8 +155,14 @@ parse_options( int argc, char** argv ) Options options; for( int i = 1; i < argc; ++i ) { - std::string arg = argv[i]; - auto match_with_value = [&]( const std::string& name, std::string& out ) { + std::string arg = argv[i]; + if( arg.rfind( "--", 0 ) == 0 ) + { + const auto eq_pos = arg.find( '=' ); + const auto end = eq_pos == std::string::npos ? arg.size() : eq_pos; + std::replace( arg.begin() + 2, arg.begin() + static_cast( end ), '_', '-' ); + } + auto match_with_value = [&]( const std::string& name, std::string& out ) { const std::string prefix = name + "="; if( arg == name ) { @@ -152,11 +184,28 @@ parse_options( int argc, char** argv ) options.show_help = true; continue; } + if( arg == "--dump-trajectories" ) + { + options.dump_trajectories = true; + continue; + } std::string value; if( match_with_value( "--solver", value ) ) { - options.solver = value; + append_list( options.solvers, value, examples::canonical_solver_name ); + } + else if( match_with_value( "--solvers", value ) ) + { + append_list( options.solvers, value, examples::canonical_solver_name ); + } + else if( match_with_value( "--scalar", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); + } + else if( match_with_value( "--scalars", value ) ) + { + append_list( options.scalars, value, examples::canonical_scalar_name ); } else { @@ -169,17 +218,58 @@ parse_options( int argc, char** argv ) void print_usage() { - std::cout << "Usage: single_track_ocp [--solver NAME]\n"; + std::cout << "Usage: single_track_ocp [--solvers NAMES] [--scalars float,double] [--dump-trajectories]\n"; std::cout << '\n'; examples::print_available( std::cout ); } +template +void +run_for_scalar( const Options& options, const std::vector& solver_names ) +{ + const mas::SolverParamsT params{ { "max_iterations", static_cast( 10 ) }, + { "tolerance", static_cast( 1e-5 ) }, + { "max_ms", static_cast( 100 ) } }; + + const std::string scalar_label = examples::scalar_label(); + + for( const auto& solver_name : solver_names ) + { + if( !examples::solver_supported_for_scalar( solver_name ) ) + { + std::cout << "scalar=" << scalar_label << " solver=" << solver_name << " unsupported (skipping)\n"; + continue; + } + + auto problem = create_single_track_lane_following_ocp(); + auto solver = examples::make_solver( solver_name ); + mas::set_params( solver, params ); + + const auto start = std::chrono::steady_clock::now(); + mas::solve( solver, problem ); + const auto end = std::chrono::steady_clock::now(); + const double elapsed_ms = std::chrono::duration( end - start ).count(); + + std::cout << std::fixed << std::setprecision( 6 ) + << "scalar=" << scalar_label + << " solver=" << solver_name + << " cost=" << static_cast( problem.best_cost ) + << " time_ms=" << elapsed_ms + << '\n'; + + if( options.dump_trajectories ) + { + examples::print_state_trajectory( std::cout, problem.best_states, problem.dt, "single_track" ); + examples::print_control_trajectory( std::cout, problem.best_controls, problem.dt, "single_track" ); + } + } +} + } // namespace int main( int argc, char** argv ) { - using namespace mas; try { const Options options = parse_options( argc, argv ); @@ -189,27 +279,21 @@ main( int argc, char** argv ) return 0; } - OCP problem = create_single_track_lane_following_ocp(); + const std::vector solver_names = options.solvers.empty() + ? examples::available_solver_names() + : options.solvers; + const std::vector scalar_names + = options.scalars.empty() ? std::vector{ "float", "double" } : options.scalars; - SolverParams params; - params["max_iterations"] = 10; - params["tolerance"] = 1e-5; - params["max_ms"] = 100; - - auto solver = examples::make_solver( options.solver ); - mas::set_params( solver, params ); - - const auto start = std::chrono::steady_clock::now(); - mas::solve( solver, problem ); - const auto end = std::chrono::steady_clock::now(); - const double elapsed_ms = std::chrono::duration( end - start ).count(); - - const std::string solver_name = examples::canonical_solver_name( options.solver ); - std::cout << std::fixed << std::setprecision( 6 ) << "solver=" << solver_name << " cost=" << problem.best_cost - << " time_ms=" << elapsed_ms << '\n'; - - examples::print_state_trajectory( std::cout, problem.best_states, problem.dt, "single_track" ); - examples::print_control_trajectory( std::cout, problem.best_controls, problem.dt, "single_track" ); + for( const auto& scalar_name : scalar_names ) + { + if( scalar_name == "float" ) + run_for_scalar( options, solver_names ); + else if( scalar_name == "double" ) + run_for_scalar( options, solver_names ); + else + std::cout << "Unknown scalar '" << scalar_name << "' -- skipping\n"; + } } catch( const std::exception& e ) { @@ -218,4 +302,4 @@ main( int argc, char** argv ) return 1; } return 0; -} \ No newline at end of file +} diff --git a/include/multi_agent_solver/agent.hpp b/include/multi_agent_solver/agent.hpp index d407c8d..9164b08 100644 --- a/include/multi_agent_solver/agent.hpp +++ b/include/multi_agent_solver/agent.hpp @@ -6,12 +6,17 @@ namespace mas { -struct Agent +template +struct AgentBase { - std::size_t id; - std::shared_ptr ocp; + using ScalarType = Scalar; + using OCPType = OCP; + using OCPPtr = std::shared_ptr; - Agent( std::size_t id_, std::shared_ptr ocp_ ) : + std::size_t id; + OCPPtr ocp; + + AgentBase( std::size_t id_, OCPPtr ocp_ ) : id( id_ ), ocp( std::move( ocp_ ) ) {} @@ -41,6 +46,18 @@ struct Agent } }; -using AgentPtr = std::shared_ptr; +template +using AgentPtrT = std::shared_ptr>; + +template +using AgentT = AgentBase; + +using Agentd = AgentBase; +using Agentf = AgentBase; +using Agent = Agentd; + +using AgentPtr = AgentPtrT; +using AgentPtrd = AgentPtrT; +using AgentPtrf = AgentPtrT; } // namespace mas diff --git a/include/multi_agent_solver/constraint_helpers.hpp b/include/multi_agent_solver/constraint_helpers.hpp index 755a567..0a394d0 100644 --- a/include/multi_agent_solver/constraint_helpers.hpp +++ b/include/multi_agent_solver/constraint_helpers.hpp @@ -1,4 +1,6 @@ #pragma once +#include + #include #include @@ -13,62 +15,80 @@ namespace mas { -// Helper function to compute the augmented cost -inline double -compute_augmented_cost( const OCP& problem, const ConstraintViolations& equality_multipliers, - const ConstraintViolations& inequality_multipliers, double penalty_parameter, const StateTrajectory& states, - const ControlTrajectory& controls ) +template +inline Scalar +compute_augmented_cost( const Problem& problem, const ConstraintViolationsT& equality_multipliers, + const ConstraintViolationsT& inequality_multipliers, Scalar penalty_parameter, + const StateTrajectoryT& states, const ControlTrajectoryT& controls ) { - double cost = problem.objective_function( states, controls ); + Scalar cost = problem.objective_function( states, controls ); for( int t = 0; t < controls.cols(); ++t ) { if( problem.equality_constraints ) { - ConstraintViolations eq_residuals = problem.equality_constraints( states.col( t ), controls.col( t ) ); - cost += equality_multipliers.dot( eq_residuals ) + 0.5 * penalty_parameter * eq_residuals.squaredNorm(); + ConstraintViolationsT eq_residuals = problem.equality_constraints( states.col( t ), controls.col( t ) ); + cost += equality_multipliers.dot( eq_residuals ) + static_cast( 0.5 ) * penalty_parameter * eq_residuals.squaredNorm(); } if( problem.inequality_constraints ) { - ConstraintViolations ineq_residuals = problem.inequality_constraints( states.col( t ), controls.col( t ) ); - ConstraintViolations slack = ( ineq_residuals.array() > 0 ).select( ineq_residuals, 0 ); - cost += inequality_multipliers.dot( slack ) + 0.5 * penalty_parameter * slack.squaredNorm(); + ConstraintViolationsT ineq_residuals = problem.inequality_constraints( states.col( t ), controls.col( t ) ); + ConstraintViolationsT slack = ineq_residuals.array().cwiseMax( Scalar( 0 ) ).matrix(); + cost += inequality_multipliers.dot( slack ) + static_cast( 0.5 ) * penalty_parameter * slack.squaredNorm(); } } return cost; } -// Helper function to update Lagrange multipliers +template +inline double +compute_augmented_cost( const Problem& problem, const ConstraintViolations& equality_multipliers, + const ConstraintViolations& inequality_multipliers, double penalty_parameter, const StateTrajectory& states, + const ControlTrajectory& controls ) +{ + return compute_augmented_cost( problem, equality_multipliers, inequality_multipliers, penalty_parameter, states, controls ); +} + +template inline void -update_lagrange_multipliers( const OCP& problem, const StateTrajectory& states, const ControlTrajectory& controls, - ConstraintViolations& equality_multipliers, ConstraintViolations& inequality_multipliers, - double penalty_parameter ) +update_lagrange_multipliers( const Problem& problem, const StateTrajectoryT& states, const ControlTrajectoryT& controls, + ConstraintViolationsT& equality_multipliers, ConstraintViolationsT& inequality_multipliers, + Scalar penalty_parameter ) { for( int t = 0; t < controls.cols(); ++t ) { if( problem.equality_constraints ) { - ConstraintViolations eq_residuals = problem.equality_constraints( states.col( t ), controls.col( t ) ); - equality_multipliers += penalty_parameter * eq_residuals; + ConstraintViolationsT eq_residuals = problem.equality_constraints( states.col( t ), controls.col( t ) ); + equality_multipliers += penalty_parameter * eq_residuals; } if( problem.inequality_constraints ) { - ConstraintViolations ineq_residuals = problem.inequality_constraints( states.col( t ), controls.col( t ) ); - inequality_multipliers += penalty_parameter * ( ineq_residuals.array() > 0 ).select( ineq_residuals, 0 ); + ConstraintViolationsT ineq_residuals = problem.inequality_constraints( states.col( t ), controls.col( t ) ); + inequality_multipliers += penalty_parameter * ineq_residuals.array().cwiseMax( Scalar( 0 ) ).matrix(); } } } -// Helper function to increase penalty parameter +template inline void -increase_penalty_parameter( double& penalty_parameter, const OCP& problem, const StateTrajectory& states, const ControlTrajectory& controls, - double tolerance ) +update_lagrange_multipliers( const Problem& problem, const StateTrajectory& states, const ControlTrajectory& controls, + ConstraintViolations& equality_multipliers, ConstraintViolations& inequality_multipliers, + double penalty_parameter ) { - double eq_violation_norm = 0.0; - double ineq_violation_norm = 0.0; + update_lagrange_multipliers( problem, states, controls, equality_multipliers, inequality_multipliers, penalty_parameter ); +} + +template +inline void +increase_penalty_parameter( Scalar& penalty_parameter, const Problem& problem, const StateTrajectoryT& states, + const ControlTrajectoryT& controls, Scalar tolerance ) +{ + Scalar eq_violation_norm = static_cast( 0 ); + Scalar ineq_violation_norm = static_cast( 0 ); for( int t = 0; t < controls.cols(); ++t ) { @@ -78,25 +98,42 @@ increase_penalty_parameter( double& penalty_parameter, const OCP& problem, const } if( problem.inequality_constraints ) { - ineq_violation_norm += problem.inequality_constraints( states.col( t ), controls.col( t ) ).cwiseMax( 0 ).squaredNorm(); + ineq_violation_norm + += problem.inequality_constraints( states.col( t ), controls.col( t ) ).array().cwiseMax( Scalar( 0 ) ).matrix().squaredNorm(); } } - eq_violation_norm = std::sqrt( eq_violation_norm ); - ineq_violation_norm = std::sqrt( ineq_violation_norm ); + eq_violation_norm = static_cast( std::sqrt( static_cast( eq_violation_norm ) ) ); + ineq_violation_norm = static_cast( std::sqrt( static_cast( ineq_violation_norm ) ) ); if( eq_violation_norm > tolerance || ineq_violation_norm > tolerance ) { - penalty_parameter *= 1.5; + penalty_parameter *= static_cast( 1.5 ); } } +template inline void -clamp_controls( ControlTrajectory& controls, const Control& lower_limit, const Control& upper_limit ) +increase_penalty_parameter( double& penalty_parameter, const Problem& problem, const StateTrajectory& states, + const ControlTrajectory& controls, double tolerance ) +{ + increase_penalty_parameter( penalty_parameter, problem, states, controls, tolerance ); +} + +template +inline void +clamp_controls( ControlTrajectoryT& controls, const ControlT& lower_limit, const ControlT& upper_limit ) { for( int t = 0; t < controls.cols(); ++t ) { controls.col( t ) = controls.col( t ).cwiseMin( upper_limit ).cwiseMax( lower_limit ); } } -} \ No newline at end of file + +inline void +clamp_controls( ControlTrajectory& controls, const Control& lower_limit, const Control& upper_limit ) +{ + clamp_controls( controls, lower_limit, upper_limit ); +} + +} // namespace mas diff --git a/include/multi_agent_solver/finite_differences.hpp b/include/multi_agent_solver/finite_differences.hpp index 184d7ba..72846e5 100644 --- a/include/multi_agent_solver/finite_differences.hpp +++ b/include/multi_agent_solver/finite_differences.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include #include @@ -14,142 +16,200 @@ namespace mas //================================================================ // Finite Differences for the Overall Trajectory Cost //================================================================ -inline ControlGradient -finite_differences_gradient( const State& initial_state, const ControlTrajectory& controls, const MotionModel& dynamics, - const ObjectiveFunction& objective_function, double dt ) +template +inline ControlGradientT +finite_differences_gradient( const StateT& initial_state, const ControlTrajectoryT& controls, + const MotionModelT& dynamics, const ObjectiveFunctionT& objective_function, Scalar dt ) { - ControlGradient gradients = ControlGradient::Zero( controls.rows(), controls.cols() ); - ControlTrajectory controls_minus = controls; - ControlTrajectory controls_plus = controls; + ControlGradientT gradients = ControlGradientT::Zero( controls.rows(), controls.cols() ); + ControlTrajectoryT controls_minus = controls; + ControlTrajectoryT controls_plus = controls; - StateTrajectory trajectory_minus = integrate_horizon( initial_state, controls_minus, dt, dynamics, integrate_rk4 ); - double cost_minus = objective_function( trajectory_minus, controls_minus ); + StateTrajectoryT trajectory_minus = integrate_horizon( initial_state, controls_minus, dt, dynamics, + integrate_rk4 ); + Scalar cost_minus = objective_function( trajectory_minus, controls_minus ); for( int t = 0; t < controls.cols(); ++t ) { for( int i = 0; i < controls.rows(); ++i ) { - double epsilon = std::max( 1e-6, 1e-8 * std::abs( controls( i, t ) ) ); + const Scalar epsilon = std::max( static_cast( 1e-6 ), static_cast( 1e-8 ) * std::abs( controls( i, t ) ) ); - controls_plus = controls; - controls_plus( i, t ) += epsilon; - StateTrajectory trajectory_plus = integrate_horizon( initial_state, controls_plus, dt, dynamics, integrate_rk4 ); - double cost_plus = objective_function( trajectory_plus, controls_plus ); + controls_plus = controls; + controls_plus( i, t ) += epsilon; + StateTrajectoryT trajectory_plus = integrate_horizon( initial_state, controls_plus, dt, dynamics, + integrate_rk4 ); + Scalar cost_plus = objective_function( trajectory_plus, controls_plus ); controls_minus = controls; controls_minus( i, t ) -= epsilon; - trajectory_minus = integrate_horizon( initial_state, controls_minus, dt, dynamics, integrate_rk4 ); + trajectory_minus = integrate_horizon( initial_state, controls_minus, dt, dynamics, integrate_rk4 ); cost_minus = objective_function( trajectory_minus, controls_minus ); - gradients( i, t ) = ( cost_plus - cost_minus ) / ( 2 * epsilon ); + gradients( i, t ) = ( cost_plus - cost_minus ) / ( static_cast( 2 ) * epsilon ); } } return gradients; } +inline ControlGradient +finite_differences_gradient( const State& initial_state, const ControlTrajectory& controls, const MotionModel& dynamics, + const ObjectiveFunction& objective_function, double dt ) +{ + return finite_differences_gradient( initial_state, controls, dynamics, objective_function, dt ); +} + //================================================================ // Finite Differences for the Dynamics //================================================================ -inline Eigen::MatrixXd -compute_dynamics_state_jacobian( const MotionModel& dynamics, const State& x, const Control& u ) +template +inline Eigen::Matrix +compute_dynamics_state_jacobian( const MotionModelT& dynamics, const StateT& x, const ControlT& u ) { - const int state_dim = x.size(); - const double epsilon = 1e-6; - Eigen::MatrixXd A = Eigen::MatrixXd::Zero( state_dim, state_dim ); + const int state_dim = static_cast( x.size() ); + const Scalar epsilon = static_cast( 1e-6 ); + Eigen::Matrix A = Eigen::Matrix::Zero( state_dim, + state_dim ); for( int i = 0; i < state_dim; ++i ) { - State dx = State::Zero( state_dim ); - dx( i ) = epsilon; + StateT dx = StateT::Zero( state_dim ); + dx( i ) = epsilon; - State f_plus = dynamics( x + dx, u ); - State f_minus = dynamics( x - dx, u ); + StateT f_plus = dynamics( x + dx, u ); + StateT f_minus = dynamics( x - dx, u ); - A.col( i ) = ( f_plus - f_minus ) / ( 2 * epsilon ); + A.col( i ) = ( f_plus - f_minus ) / ( static_cast( 2 ) * epsilon ); } return A; } inline Eigen::MatrixXd -compute_dynamics_control_jacobian( const MotionModel& dynamics, const State& x, const Control& u ) +compute_dynamics_state_jacobian( const MotionModel& dynamics, const State& x, const Control& u ) +{ + return compute_dynamics_state_jacobian( dynamics, x, u ); +} + +template +inline Eigen::Matrix +compute_dynamics_control_jacobian( const MotionModelT& dynamics, const StateT& x, const ControlT& u ) { - const int state_dim = x.size(); - const int control_dim = u.size(); - const double epsilon = 1e-6; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero( state_dim, control_dim ); + const int state_dim = static_cast( x.size() ); + const int control_dim = static_cast( u.size() ); + const Scalar epsilon = static_cast( 1e-6 ); + Eigen::Matrix B = Eigen::Matrix::Zero( state_dim, + control_dim ); for( int i = 0; i < control_dim; ++i ) { - Control du = Control::Zero( control_dim ); - du( i ) = epsilon; + ControlT du = ControlT::Zero( control_dim ); + du( i ) = epsilon; - State f_plus = dynamics( x, u + du ); - State f_minus = dynamics( x, u - du ); + StateT f_plus = dynamics( x, u + du ); + StateT f_minus = dynamics( x, u - du ); - B.col( i ) = ( f_plus - f_minus ) / ( 2 * epsilon ); + B.col( i ) = ( f_plus - f_minus ) / ( static_cast( 2 ) * epsilon ); } return B; } +inline Eigen::MatrixXd +compute_dynamics_control_jacobian( const MotionModel& dynamics, const State& x, const Control& u ) +{ + return compute_dynamics_control_jacobian( dynamics, x, u ); +} + // Safe evaluation wrapper +template +inline Scalar +safe_eval( const StageCostFunctionT& stage_cost, const StateT& x, const ControlT& u, size_t time_idx ) +{ + const Scalar value = stage_cost( x, u, time_idx ); + return std::isfinite( static_cast( value ) ) ? value : static_cast( 0 ); +} + +template +inline Scalar +safe_eval_terminal( const TerminalCostFunctionT& terminal_cost, const StateT& x ) +{ + const Scalar value = terminal_cost( x ); + return std::isfinite( static_cast( value ) ) ? value : static_cast( 0 ); +} + inline double safe_eval( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) { - double value = stage_cost( x, u, time_idx ); - return std::isfinite( value ) ? value : 0.0; + return safe_eval( stage_cost, x, u, time_idx ); } inline double safe_eval_terminal( const TerminalCostFunction& terminal_cost, const State& x ) { - double value = terminal_cost( x ); - return std::isfinite( value ) ? value : 0.0; + return safe_eval_terminal( terminal_cost, x ); } // Cost derivatives -inline Eigen::VectorXd -compute_cost_state_gradient( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) +template +inline StateT +compute_cost_state_gradient( const StageCostFunctionT& stage_cost, const StateT& x, const ControlT& u, + size_t time_idx ) { - Eigen::VectorXd grad = Eigen::VectorXd::Zero( x.size() ); - const double epsilon = 1e-6; + StateT grad = StateT::Zero( x.size() ); + const Scalar epsilon = static_cast( 1e-6 ); for( int i = 0; i < x.size(); ++i ) { - State dx = State::Zero( x.size() ); - dx( i ) = epsilon; - grad( i ) = ( stage_cost( x + dx, u, time_idx ) - stage_cost( x - dx, u, time_idx ) ) / ( 2 * epsilon ); + StateT dx = StateT::Zero( x.size() ); + dx( i ) = epsilon; + grad( i ) = ( stage_cost( x + dx, u, time_idx ) - stage_cost( x - dx, u, time_idx ) ) / ( static_cast( 2 ) * epsilon ); } return grad; } inline Eigen::VectorXd -compute_cost_control_gradient( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) +compute_cost_state_gradient( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) +{ + return compute_cost_state_gradient( stage_cost, x, u, time_idx ); +} + +template +inline ControlT +compute_cost_control_gradient( const StageCostFunctionT& stage_cost, const StateT& x, const ControlT& u, + size_t time_idx ) { - Eigen::VectorXd grad = Eigen::VectorXd::Zero( u.size() ); - const double epsilon = 1e-6; + ControlT grad = ControlT::Zero( u.size() ); + const Scalar epsilon = static_cast( 1e-6 ); for( int i = 0; i < u.size(); ++i ) { - Control du = Control::Zero( u.size() ); - du( i ) = epsilon; - grad( i ) = ( stage_cost( x, u + du, time_idx ) - stage_cost( x, u - du, time_idx ) ) / ( 2 * epsilon ); + ControlT du = ControlT::Zero( u.size() ); + du( i ) = epsilon; + grad( i ) = ( stage_cost( x, u + du, time_idx ) - stage_cost( x, u - du, time_idx ) ) / ( static_cast( 2 ) * epsilon ); } return grad; } -inline Eigen::MatrixXd -compute_cost_state_hessian( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) +inline Eigen::VectorXd +compute_cost_control_gradient( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) { - const int n = x.size(); - Eigen::MatrixXd H = Eigen::MatrixXd::Zero( n, n ); - const double epsilon = 1e-5; + return compute_cost_control_gradient( stage_cost, x, u, time_idx ); +} + +template +inline Eigen::Matrix +compute_cost_state_hessian( const StageCostFunctionT& stage_cost, const StateT& x, const ControlT& u, + size_t time_idx ) +{ + const int n = static_cast( x.size() ); + Eigen::Matrix H = Eigen::Matrix::Zero( n, n ); + const Scalar epsilon = static_cast( 1e-5 ); for( int i = 0; i < n; ++i ) { - State dx = State::Zero( n ); - dx( i ) = epsilon; - double f_plus = safe_eval( stage_cost, x + dx, u, time_idx ); - double f = safe_eval( stage_cost, x, u, time_idx ); - double f_minus = safe_eval( stage_cost, x - dx, u, time_idx ); - H( i, i ) = ( f_plus - 2 * f + f_minus ) / ( epsilon * epsilon ); + StateT dx = StateT::Zero( n ); + dx( i ) = epsilon; + Scalar f_plus = safe_eval( stage_cost, x + dx, u, time_idx ); + Scalar f = safe_eval( stage_cost, x, u, time_idx ); + Scalar f_minus = safe_eval( stage_cost, x - dx, u, time_idx ); + H( i, i ) = ( f_plus - static_cast( 2 ) * f + f_minus ) / ( epsilon * epsilon ); } for( int i = 0; i < n; ++i ) @@ -158,14 +218,15 @@ compute_cost_state_hessian( const StageCostFunction& stage_cost, const State& x, { if( i != j ) { - State dx_i = State::Zero( n ), dx_j = State::Zero( n ); - dx_i( i ) = epsilon; - dx_j( j ) = epsilon; - double f_pp = safe_eval( stage_cost, x + dx_i + dx_j, u, time_idx ); - double f_pm = safe_eval( stage_cost, x + dx_i - dx_j, u, time_idx ); - double f_mp = safe_eval( stage_cost, x - dx_i + dx_j, u, time_idx ); - double f_mm = safe_eval( stage_cost, x - dx_i - dx_j, u, time_idx ); - H( i, j ) = ( f_pp - f_pm - f_mp + f_mm ) / ( 4 * epsilon * epsilon ); + StateT dx_i = StateT::Zero( n ); + StateT dx_j = StateT::Zero( n ); + dx_i( i ) = epsilon; + dx_j( j ) = epsilon; + const Scalar f_pp = safe_eval( stage_cost, x + dx_i + dx_j, u, time_idx ); + const Scalar f_pm = safe_eval( stage_cost, x + dx_i - dx_j, u, time_idx ); + const Scalar f_mp = safe_eval( stage_cost, x - dx_i + dx_j, u, time_idx ); + const Scalar f_mm = safe_eval( stage_cost, x - dx_i - dx_j, u, time_idx ); + H( i, j ) = ( f_pp - f_pm - f_mp + f_mm ) / ( static_cast( 4 ) * epsilon * epsilon ); } } } @@ -173,20 +234,28 @@ compute_cost_state_hessian( const StageCostFunction& stage_cost, const State& x, } inline Eigen::MatrixXd -compute_cost_control_hessian( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) +compute_cost_state_hessian( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) +{ + return compute_cost_state_hessian( stage_cost, x, u, time_idx ); +} + +template +inline Eigen::Matrix +compute_cost_control_hessian( const StageCostFunctionT& stage_cost, const StateT& x, const ControlT& u, + size_t time_idx ) { - const int m = u.size(); - Eigen::MatrixXd H = Eigen::MatrixXd::Zero( m, m ); - const double epsilon = 1e-5; + const int m = static_cast( u.size() ); + Eigen::Matrix H = Eigen::Matrix::Zero( m, m ); + const Scalar epsilon = static_cast( 1e-5 ); for( int i = 0; i < m; ++i ) { - Control du = Control::Zero( m ); - du( i ) = epsilon; - double f_plus = safe_eval( stage_cost, x, u + du, time_idx ); - double f = safe_eval( stage_cost, x, u, time_idx ); - double f_minus = safe_eval( stage_cost, x, u - du, time_idx ); - H( i, i ) = ( f_plus - 2 * f + f_minus ) / ( epsilon * epsilon ); + ControlT du = ControlT::Zero( m ); + du( i ) = epsilon; + Scalar f_plus = safe_eval( stage_cost, x, u + du, time_idx ); + Scalar f = safe_eval( stage_cost, x, u, time_idx ); + Scalar f_minus = safe_eval( stage_cost, x, u - du, time_idx ); + H( i, i ) = ( f_plus - static_cast( 2 ) * f + f_minus ) / ( epsilon * epsilon ); } for( int i = 0; i < m; ++i ) @@ -195,49 +264,43 @@ compute_cost_control_hessian( const StageCostFunction& stage_cost, const State& { if( i != j ) { - Control du_i = Control::Zero( m ), du_j = Control::Zero( m ); - du_i( i ) = epsilon; - du_j( j ) = epsilon; - double f_pp = safe_eval( stage_cost, x, u + du_i + du_j, time_idx ); - double f_pm = safe_eval( stage_cost, x, u + du_i - du_j, time_idx ); - double f_mp = safe_eval( stage_cost, x, u - du_i + du_j, time_idx ); - double f_mm = safe_eval( stage_cost, x, u - du_i - du_j, time_idx ); - H( i, j ) = ( f_pp - f_pm - f_mp + f_mm ) / ( 4 * epsilon * epsilon ); + ControlT du_i = ControlT::Zero( m ); + ControlT du_j = ControlT::Zero( m ); + du_i( i ) = epsilon; + du_j( j ) = epsilon; + const Scalar f_pp = safe_eval( stage_cost, x, u + du_i + du_j, time_idx ); + const Scalar f_pm = safe_eval( stage_cost, x, u + du_i - du_j, time_idx ); + const Scalar f_mp = safe_eval( stage_cost, x, u - du_i + du_j, time_idx ); + const Scalar f_mm = safe_eval( stage_cost, x, u - du_i - du_j, time_idx ); + H( i, j ) = ( f_pp - f_pm - f_mp + f_mm ) / ( static_cast( 4 ) * epsilon * epsilon ); } } } return H; } -inline Eigen::VectorXd -compute_terminal_cost_gradient( const TerminalCostFunction& terminal_cost, const State& x ) +inline Eigen::MatrixXd +compute_cost_control_hessian( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) { - Eigen::VectorXd grad = Eigen::VectorXd::Zero( x.size() ); - const double epsilon = 1e-6; - for( int i = 0; i < x.size(); ++i ) - { - State dx = State::Zero( x.size() ); - dx( i ) = epsilon; - grad( i ) = ( terminal_cost( x + dx ) - terminal_cost( x - dx ) ) / ( 2 * epsilon ); - } - return grad; + return compute_cost_control_hessian( stage_cost, x, u, time_idx ); } -inline Eigen::MatrixXd -compute_terminal_cost_hessian( const TerminalCostFunction& terminal_cost, const State& x ) +template +inline Eigen::Matrix +compute_terminal_cost_hessian( const TerminalCostFunctionT& terminal_cost, const StateT& x ) { - const int n = x.size(); - Eigen::MatrixXd H = Eigen::MatrixXd::Zero( n, n ); - const double epsilon = 1e-5; + const int n = static_cast( x.size() ); + Eigen::Matrix H = Eigen::Matrix::Zero( n, n ); + const Scalar epsilon = static_cast( 1e-5 ); for( int i = 0; i < n; ++i ) { - State dx = State::Zero( n ); - dx( i ) = epsilon; - double f_plus = safe_eval_terminal( terminal_cost, x + dx ); - double f = safe_eval_terminal( terminal_cost, x ); - double f_minus = safe_eval_terminal( terminal_cost, x - dx ); - H( i, i ) = ( f_plus - 2 * f + f_minus ) / ( epsilon * epsilon ); + StateT dx = StateT::Zero( n ); + dx( i ) = epsilon; + Scalar f_plus = safe_eval_terminal( terminal_cost, x + dx ); + Scalar f = safe_eval_terminal( terminal_cost, x ); + Scalar f_minus = safe_eval_terminal( terminal_cost, x - dx ); + H( i, i ) = ( f_plus - static_cast( 2 ) * f + f_minus ) / ( epsilon * epsilon ); } for( int i = 0; i < n; ++i ) @@ -246,101 +309,151 @@ compute_terminal_cost_hessian( const TerminalCostFunction& terminal_cost, const { if( i != j ) { - State dx_i = State::Zero( n ), dx_j = State::Zero( n ); - dx_i( i ) = epsilon; - dx_j( j ) = epsilon; - double f_pp = safe_eval_terminal( terminal_cost, x + dx_i + dx_j ); - double f_pm = safe_eval_terminal( terminal_cost, x + dx_i - dx_j ); - double f_mp = safe_eval_terminal( terminal_cost, x - dx_i + dx_j ); - double f_mm = safe_eval_terminal( terminal_cost, x - dx_i - dx_j ); - H( i, j ) = ( f_pp - f_pm - f_mp + f_mm ) / ( 4 * epsilon * epsilon ); + StateT dx_i = StateT::Zero( n ); + StateT dx_j = StateT::Zero( n ); + dx_i( i ) = epsilon; + dx_j( j ) = epsilon; + const Scalar f_pp = safe_eval_terminal( terminal_cost, x + dx_i + dx_j ); + const Scalar f_pm = safe_eval_terminal( terminal_cost, x + dx_i - dx_j ); + const Scalar f_mp = safe_eval_terminal( terminal_cost, x - dx_i + dx_j ); + const Scalar f_mm = safe_eval_terminal( terminal_cost, x - dx_i - dx_j ); + H( i, j ) = ( f_pp - f_pm - f_mp + f_mm ) / ( static_cast( 4 ) * epsilon * epsilon ); } } } return H; } +template +inline StateT +compute_terminal_cost_gradient( const TerminalCostFunctionT& terminal_cost, const StateT& x ) +{ + StateT grad = StateT::Zero( x.size() ); + const Scalar epsilon = static_cast( 1e-6 ); + for( int i = 0; i < x.size(); ++i ) + { + StateT dx = StateT::Zero( x.size() ); + dx( i ) = epsilon; + grad( i ) = ( terminal_cost( x + dx ) - terminal_cost( x - dx ) ) / ( static_cast( 2 ) * epsilon ); + } + return grad; +} + +inline Eigen::VectorXd +compute_terminal_cost_gradient( const TerminalCostFunction& terminal_cost, const State& x ) +{ + return compute_terminal_cost_gradient( terminal_cost, x ); +} + inline Eigen::MatrixXd -compute_cost_cross_term( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) +compute_terminal_cost_hessian( const TerminalCostFunction& terminal_cost, const State& x ) +{ + return compute_terminal_cost_hessian( terminal_cost, x ); +} + +template +inline Eigen::Matrix +compute_cost_cross_term( const StageCostFunctionT& stage_cost, const StateT& x, const ControlT& u, size_t time_idx ) { - const int m = u.size(); - const int n = x.size(); - Eigen::MatrixXd H = Eigen::MatrixXd::Zero( m, n ); - const double epsilon = 1e-6; + const int m = static_cast( u.size() ); + const int n = static_cast( x.size() ); + Eigen::Matrix H = Eigen::Matrix::Zero( m, n ); + const Scalar epsilon = static_cast( 1e-6 ); for( int i = 0; i < m; ++i ) { for( int j = 0; j < n; ++j ) { - Control du = Control::Zero( m ); - State dx = State::Zero( n ); - du( i ) = epsilon; - dx( j ) = epsilon; - double f_pp = safe_eval( stage_cost, x + dx, u + du, time_idx ); - double f_pm = safe_eval( stage_cost, x - dx, u + du, time_idx ); - double f_mp = safe_eval( stage_cost, x + dx, u - du, time_idx ); - double f_mm = safe_eval( stage_cost, x - dx, u - du, time_idx ); - H( i, j ) = ( f_pp - f_pm - f_mp + f_mm ) / ( 4 * epsilon * epsilon ); + ControlT du = ControlT::Zero( m ); + StateT dx = StateT::Zero( n ); + du( i ) = epsilon; + dx( j ) = epsilon; + const Scalar f_pp = safe_eval( stage_cost, x + dx, u + du, time_idx ); + const Scalar f_pm = safe_eval( stage_cost, x - dx, u + du, time_idx ); + const Scalar f_mp = safe_eval( stage_cost, x + dx, u - du, time_idx ); + const Scalar f_mm = safe_eval( stage_cost, x - dx, u - du, time_idx ); + H( i, j ) = ( f_pp - f_pm - f_mp + f_mm ) / ( static_cast( 4 ) * epsilon * epsilon ); } } return H; } -inline ConstraintsJacobian -compute_constraints_state_jacobian( const ConstraintsFunction& constraint, const State& x, const Control& u ) +inline Eigen::MatrixXd +compute_cost_cross_term( const StageCostFunction& stage_cost, const State& x, const Control& u, size_t time_idx ) +{ + return compute_cost_cross_term( stage_cost, x, u, time_idx ); +} + +template +inline ConstraintsJacobianT +compute_constraints_state_jacobian( const ConstraintsFunctionT& constraint, const StateT& x, const ControlT& u ) { if( !constraint ) - return ConstraintsJacobian{}; + return ConstraintsJacobianT{}; - const ConstraintViolations base = constraint( x, u ); - const int m = static_cast( base.size() ); + const ConstraintViolationsT base = constraint( x, u ); + const int m = static_cast( base.size() ); if( m == 0 ) - return ConstraintsJacobian{}; + return ConstraintsJacobianT{}; - const int n = static_cast( x.size() ); - const double epsilon = 1e-6; - ConstraintsJacobian J = ConstraintsJacobian::Zero( m, n ); + const int n = static_cast( x.size() ); + const Scalar epsilon = static_cast( 1e-6 ); + ConstraintsJacobianT J = ConstraintsJacobianT::Zero( m, n ); for( int i = 0; i < n; ++i ) { - State dx = State::Zero( n ); - dx( i ) = epsilon; + StateT dx = StateT::Zero( n ); + dx( i ) = epsilon; - ConstraintViolations f_plus = constraint( x + dx, u ); - ConstraintViolations f_minus = constraint( x - dx, u ); + ConstraintViolationsT f_plus = constraint( x + dx, u ); + ConstraintViolationsT f_minus = constraint( x - dx, u ); - J.col( i ) = ( f_plus - f_minus ) / ( 2 * epsilon ); + J.col( i ) = ( f_plus - f_minus ) / ( static_cast( 2 ) * epsilon ); } return J; } inline ConstraintsJacobian -compute_constraints_control_jacobian( const ConstraintsFunction& constraint, const State& x, const Control& u ) +compute_constraints_state_jacobian( const ConstraintsFunction& constraint, const State& x, const Control& u ) +{ + return compute_constraints_state_jacobian( constraint, x, u ); +} + +template +inline ConstraintsJacobianT +compute_constraints_control_jacobian( const ConstraintsFunctionT& constraint, const StateT& x, const ControlT& u ) { if( !constraint ) - return ConstraintsJacobian{}; + return ConstraintsJacobianT{}; - const ConstraintViolations base = constraint( x, u ); - const int m = static_cast( base.size() ); + const ConstraintViolationsT base = constraint( x, u ); + const int m = static_cast( base.size() ); if( m == 0 ) - return ConstraintsJacobian{}; + return ConstraintsJacobianT{}; - const int p = static_cast( u.size() ); - const double epsilon = 1e-6; - ConstraintsJacobian J = ConstraintsJacobian::Zero( m, p ); + const int p = static_cast( u.size() ); + const Scalar epsilon = static_cast( 1e-6 ); + ConstraintsJacobianT J = ConstraintsJacobianT::Zero( m, p ); for( int i = 0; i < p; ++i ) { - Control du = Control::Zero( p ); - du( i ) = epsilon; + ControlT du = ControlT::Zero( p ); + du( i ) = epsilon; - ConstraintViolations f_plus = constraint( x, u + du ); - ConstraintViolations f_minus = constraint( x, u - du ); + ConstraintViolationsT f_plus = constraint( x, u + du ); + ConstraintViolationsT f_minus = constraint( x, u - du ); - J.col( i ) = ( f_plus - f_minus ) / ( 2 * epsilon ); + J.col( i ) = ( f_plus - f_minus ) / ( static_cast( 2 ) * epsilon ); } return J; } + +inline ConstraintsJacobian +compute_constraints_control_jacobian( const ConstraintsFunction& constraint, const State& x, const Control& u ) +{ + return compute_constraints_control_jacobian( constraint, x, u ); +} + } // namespace mas diff --git a/include/multi_agent_solver/integrator.hpp b/include/multi_agent_solver/integrator.hpp index 7002ede..ef67c99 100644 --- a/include/multi_agent_solver/integrator.hpp +++ b/include/multi_agent_solver/integrator.hpp @@ -8,36 +8,57 @@ namespace mas { +template +using SingleStepIntegratorT + = std::function( const StateT&, const ControlT&, Scalar, const MotionModelT& )>; +using SingleStepIntegrator = SingleStepIntegratorT; + // Single-step Euler integration +template +inline StateT +integrate_euler( const StateT& current_state, const ControlT& control, Scalar dt, const MotionModelT& motion_model ) +{ + return current_state + dt * motion_model( current_state, control ); +} + inline State integrate_euler( const State& current_state, const Control& control, double dt, const MotionModel& motion_model ) { - return current_state + dt * motion_model( current_state, control ); + return integrate_euler( current_state, control, dt, motion_model ); } // Single-step RK4 integration +template +inline StateT +integrate_rk4( const StateT& current_state, const ControlT& control, Scalar dt, const MotionModelT& motion_model ) +{ + const Scalar half_dt = static_cast( 0.5 ) * dt; + const Scalar sixth = dt / static_cast( 6.0 ); + + StateT k1 = motion_model( current_state, control ); + StateT k2 = motion_model( current_state + half_dt * k1, control ); + StateT k3 = motion_model( current_state + half_dt * k2, control ); + StateT k4 = motion_model( current_state + dt * k3, control ); + + return current_state + sixth * ( k1 + static_cast( 2 ) * k2 + static_cast( 2 ) * k3 + k4 ); +} + inline State integrate_rk4( const State& current_state, const Control& control, double dt, const MotionModel& motion_model ) { - State k1 = motion_model( current_state, control ); - State k2 = motion_model( current_state + 0.5 * dt * k1, control ); - State k3 = motion_model( current_state + 0.5 * dt * k2, control ); - State k4 = motion_model( current_state + dt * k3, control ); - - return current_state + ( dt / 6.0 ) * ( k1 + 2 * k2 + 2 * k3 + k4 ); + return integrate_rk4( current_state, control, dt, motion_model ); } // Horizon integration function -inline StateTrajectory -integrate_horizon( const State& initial_state, const ControlTrajectory& controls, double dt, const MotionModel& motion_model, - const std::function& single_step_integrator ) +template +inline StateTrajectoryT +integrate_horizon( const StateT& initial_state, const ControlTrajectoryT& controls, Scalar dt, + const MotionModelT& motion_model, SingleStepIntegrator&& single_step_integrator ) { - // Initialize the state trajectory - StateTrajectory state_trajectory( initial_state.size(), controls.cols() + 1 ); + StateTrajectoryT state_trajectory( initial_state.size(), controls.cols() + 1 ); state_trajectory.col( 0 ) = initial_state; - // Integrate step by step - State state = initial_state; + StateT state = initial_state; for( int i = 0; i < controls.cols(); ++i ) { state = single_step_integrator( state, controls.col( i ), dt, motion_model ); @@ -46,4 +67,20 @@ integrate_horizon( const State& initial_state, const ControlTrajectory& controls return state_trajectory; } -} \ No newline at end of file + +template +inline StateTrajectoryT +integrate_horizon( const StateT& initial_state, const ControlTrajectoryT& controls, Scalar dt, + const MotionModelT& motion_model, const SingleStepIntegratorT& single_step_integrator ) +{ + return integrate_horizon&>( initial_state, controls, dt, motion_model, + single_step_integrator ); +} + +inline StateTrajectory +integrate_horizon( const State& initial_state, const ControlTrajectory& controls, double dt, const MotionModel& motion_model, + const SingleStepIntegrator& single_step_integrator ) +{ + return integrate_horizon( initial_state, controls, dt, motion_model, single_step_integrator ); +} +} // namespace mas diff --git a/include/multi_agent_solver/line_search.hpp b/include/multi_agent_solver/line_search.hpp index 6a5f617..b948b05 100644 --- a/include/multi_agent_solver/line_search.hpp +++ b/include/multi_agent_solver/line_search.hpp @@ -7,46 +7,57 @@ #include #include "multi_agent_solver/integrator.hpp" -#include "multi_agent_solver/ocp.hpp" #include "multi_agent_solver/types.hpp" namespace mas { // Generic line search function alias -using LineSearchFunction = std::function< - double( const State& initial_state, const ControlTrajectory& controls, const ControlGradient& gradients, const MotionModel& dynamics, - const ObjectiveFunction& objective_function, double dt, const std::map& parameters )>; +template +using LineSearchFunctionT = std::function& initial_state, const ControlTrajectoryT& controls, + const ControlGradientT& gradients, const MotionModelT& dynamics, + const ObjectiveFunctionT& objective_function, Scalar dt, + const std::map& parameters )>; +using LineSearchFunction = LineSearchFunctionT; // Utility function to get parameter value with default -inline double -get_parameter( const std::map& parameters, const std::string& key, double default_value ) +template +inline Scalar +get_parameter( const std::map& parameters, const std::string& key, Scalar default_value ) { auto it = parameters.find( key ); return ( it != parameters.end() ) ? it->second : default_value; } -// Armijo line search inline double -armijo_line_search( const State& initial_state, const ControlTrajectory& controls, const ControlGradient& gradients, - const MotionModel& dynamics, const ObjectiveFunction& objective_function, double dt, - const std::map& parameters ) +get_parameter( const std::map& parameters, const std::string& key, double default_value ) { + return get_parameter( parameters, key, default_value ); +} - double initial_step_size = get_parameter( parameters, "initial_step_size", 1.0 ); - double beta = get_parameter( parameters, "beta", 0.5 ); - double c1 = get_parameter( parameters, "c1", 1e-6 ); +// Armijo line search +template +inline Scalar +armijo_line_search( const StateT& initial_state, const ControlTrajectoryT& controls, + const ControlGradientT& gradients, const MotionModelT& dynamics, + const ObjectiveFunctionT& objective_function, Scalar dt, const std::map& parameters ) +{ + Scalar initial_step_size = get_parameter( parameters, "initial_step_size", static_cast( 1.0 ) ); + Scalar beta = get_parameter( parameters, "beta", static_cast( 0.5 ) ); + Scalar c1 = get_parameter( parameters, "c1", static_cast( 1e-6 ) ); - double alpha = initial_step_size; - double cost_ref = objective_function( integrate_horizon( initial_state, controls, dt, dynamics, integrate_rk4 ), controls ); - double directional_derivative = gradients.cwiseProduct( -gradients ).sum(); + Scalar alpha = initial_step_size; + Scalar cost_ref = objective_function( integrate_horizon( initial_state, controls, dt, dynamics, integrate_rk4 ), + controls ); + Scalar directional_derivative = gradients.cwiseProduct( -gradients ).sum(); while( true ) { // Compute trial controls and cost - ControlTrajectory trial_controls = controls - alpha * gradients; - StateTrajectory trial_trajectory = integrate_horizon( initial_state, trial_controls, dt, dynamics, integrate_rk4 ); - double trial_cost = objective_function( trial_trajectory, trial_controls ); + ControlTrajectoryT trial_controls = controls - alpha * gradients; + StateTrajectoryT trial_trajectory = integrate_horizon( initial_state, trial_controls, dt, dynamics, + integrate_rk4 ); + Scalar trial_cost = objective_function( trial_trajectory, trial_controls ); // Check Armijo condition. directional_derivative is negative, so the // right-hand side is less than cost_ref when a descent direction is @@ -60,7 +71,7 @@ armijo_line_search( const State& initial_state, const ControlTrajectory& control alpha *= beta; // Avoid excessively small step sizes - if( alpha < 1e-8 ) + if( alpha < static_cast( 1e-8 ) ) { break; } @@ -69,25 +80,35 @@ armijo_line_search( const State& initial_state, const ControlTrajectory& control return alpha; } -// Backtracking line search inline double -backtracking_line_search( const State& initial_state, const ControlTrajectory& controls, const ControlGradient& gradients, - const MotionModel& dynamics, const ObjectiveFunction& objective_function, double dt, - const std::map& parameters ) +armijo_line_search( const State& initial_state, const ControlTrajectory& controls, const ControlGradient& gradients, + const MotionModel& dynamics, const ObjectiveFunction& objective_function, double dt, + const std::map& parameters ) { + return armijo_line_search( initial_state, controls, gradients, dynamics, objective_function, dt, parameters ); +} - double initial_step_size = get_parameter( parameters, "initial_step_size", 1.0 ); - double beta = get_parameter( parameters, "beta", 0.5 ); +// Backtracking line search +template +inline Scalar +backtracking_line_search( const StateT& initial_state, const ControlTrajectoryT& controls, + const ControlGradientT& gradients, const MotionModelT& dynamics, + const ObjectiveFunctionT& objective_function, Scalar dt, const std::map& parameters ) +{ + Scalar initial_step_size = get_parameter( parameters, "initial_step_size", static_cast( 1.0 ) ); + Scalar beta = get_parameter( parameters, "beta", static_cast( 0.5 ) ); - double alpha = initial_step_size; - double cost_ref = objective_function( integrate_horizon( initial_state, controls, dt, dynamics, integrate_rk4 ), controls ); + Scalar alpha = initial_step_size; + Scalar cost_ref = objective_function( integrate_horizon( initial_state, controls, dt, dynamics, integrate_rk4 ), + controls ); while( true ) { // Compute trial controls and cost - ControlTrajectory trial_controls = controls - alpha * gradients; - StateTrajectory trial_trajectory = integrate_horizon( initial_state, trial_controls, dt, dynamics, integrate_rk4 ); - double trial_cost = objective_function( trial_trajectory, trial_controls ); + ControlTrajectoryT trial_controls = controls - alpha * gradients; + StateTrajectoryT trial_trajectory = integrate_horizon( initial_state, trial_controls, dt, dynamics, + integrate_rk4 ); + Scalar trial_cost = objective_function( trial_trajectory, trial_controls ); // Check if cost decreased if( trial_cost < cost_ref ) @@ -99,7 +120,7 @@ backtracking_line_search( const State& initial_state, const ControlTrajectory& c alpha *= beta; // Avoid excessively small step sizes - if( alpha < 1e-8 ) + if( alpha < static_cast( 1e-8 ) ) { break; } @@ -108,13 +129,31 @@ backtracking_line_search( const State& initial_state, const ControlTrajectory& c return alpha; } +inline double +backtracking_line_search( const State& initial_state, const ControlTrajectory& controls, const ControlGradient& gradients, + const MotionModel& dynamics, const ObjectiveFunction& objective_function, double dt, + const std::map& parameters ) +{ + return backtracking_line_search( initial_state, controls, gradients, dynamics, objective_function, dt, parameters ); +} + // Constant step size line search for simplicity +template +inline Scalar +constant_line_search( const StateT& /*initial_state*/, const ControlTrajectoryT& /*controls*/, + const ControlGradientT& /*gradients*/, const MotionModelT& /*dynamics*/, + const ObjectiveFunctionT& /*objective_function*/, Scalar /*dt*/, + const std::map& parameters ) +{ + return get_parameter( parameters, "step_size", static_cast( 0.1 ) ); +} + inline double -constant_line_search( const State& /*initial_state*/, const ControlTrajectory& /*controls*/, const ControlGradient& /*gradients*/, - const MotionModel& /*dynamics*/, const ObjectiveFunction& /*objective_function*/, double /*dt*/, +constant_line_search( const State& initial_state, const ControlTrajectory& controls, const ControlGradient& gradients, + const MotionModel& dynamics, const ObjectiveFunction& objective_function, double dt, const std::map& parameters ) { - - return get_parameter( parameters, "step_size", 0.1 ); + return constant_line_search( initial_state, controls, gradients, dynamics, objective_function, dt, parameters ); } -} // namespace mas \ No newline at end of file + +} // namespace mas diff --git a/include/multi_agent_solver/multi_agent_problem.hpp b/include/multi_agent_solver/multi_agent_problem.hpp index 9aa5c08..3534dde 100644 --- a/include/multi_agent_solver/multi_agent_problem.hpp +++ b/include/multi_agent_solver/multi_agent_problem.hpp @@ -11,22 +11,34 @@ namespace mas { -struct AgentBlockInfo +template +struct AgentBlockInfoT { std::size_t agent_id; int state_offset; int control_offset; int state_dim; int control_dim; - AgentPtr agent; + AgentPtrT agent; }; -class MultiAgentProblem +template +class MultiAgentProblemT { public: + using ScalarType = Scalar; + using AgentType = AgentT; + using AgentPtr = AgentPtrT; + using OCPType = OCP; + using State = typename OCPType::State; + using Control = typename OCPType::Control; + using StateTrajectory = typename OCPType::StateTrajectory; + using ControlTrajectory = typename OCPType::ControlTrajectory; + using StateDerivative = typename OCPType::StateDerivative; + std::vector agents; - std::vector blocks; + std::vector> blocks; void add_agent( const AgentPtr& a ) @@ -49,10 +61,10 @@ class MultiAgentProblem } } - OCP + OCPType build_global_ocp() const { - OCP g; + OCPType g; // assume offsets computed int total_x = 0, total_u = 0; for( auto& b : blocks ) @@ -67,7 +79,7 @@ class MultiAgentProblem g.horizon_steps = blocks.front().agent->ocp->horizon_steps; g.dt = blocks.front().agent->ocp->dt; } - g.initial_state = Eigen::VectorXd( total_x ); + g.initial_state = State::Zero( total_x ); for( auto& b : blocks ) { g.initial_state.segment( b.state_offset, b.state_dim ) = b.agent->ocp->initial_state; @@ -81,8 +93,8 @@ class MultiAgentProblem } if( all_bounds ) { - g.input_lower_bounds = Eigen::VectorXd( total_u ); - g.input_upper_bounds = Eigen::VectorXd( total_u ); + g.input_lower_bounds = Control::Zero( total_u ); + g.input_upper_bounds = Control::Zero( total_u ); for( auto& b : blocks ) { auto& ocp = *b.agent->ocp; @@ -102,7 +114,7 @@ class MultiAgentProblem return out; }; g.stage_cost = [bs = blocks]( const State& full_x, const Control& full_u, size_t t ) { - double cost = 0.0; + Scalar cost = static_cast( 0 ); for( auto& b : bs ) { State x = full_x.segment( b.state_offset, b.state_dim ); @@ -112,7 +124,7 @@ class MultiAgentProblem return cost; }; g.terminal_cost = [bs = blocks]( const State& full_x ) { - double cost = 0.0; + Scalar cost = static_cast( 0 ); for( auto& b : bs ) { State x = full_x.segment( b.state_offset, b.state_dim ); @@ -127,4 +139,11 @@ class MultiAgentProblem } }; +using AgentBlockInfo = AgentBlockInfoT; +using AgentBlockInfof = AgentBlockInfoT; + +using MultiAgentProblemd = MultiAgentProblemT; +using MultiAgentProblemf = MultiAgentProblemT; +using MultiAgentProblem = MultiAgentProblemd; + } // namespace mas diff --git a/include/multi_agent_solver/ocp.hpp b/include/multi_agent_solver/ocp.hpp index ba70437..979673c 100644 --- a/include/multi_agent_solver/ocp.hpp +++ b/include/multi_agent_solver/ocp.hpp @@ -1,6 +1,8 @@ - #pragma once + +#include #include +#include #include #include @@ -11,24 +13,55 @@ namespace mas { -inline double -compute_trajectory_cost( const StateTrajectory& X, const ControlTrajectory& U, const StageCostFunction& stage_cost, - const TerminalCostFunction& terminal_cost ) +template +inline Scalar +compute_trajectory_cost( const StateTrajectoryT& X, const ControlTrajectoryT& U, + const StageCostFunctionT& stage_cost, const TerminalCostFunctionT& terminal_cost ) { - int T = U.cols(); - int Tp1 = X.cols(); + const int T = U.cols(); + const int Tp1 = X.cols(); - double cost = 0.0; + Scalar cost = static_cast( 0 ); for( int t = 0; t < T; ++t ) { - cost += stage_cost( X.col( t ), U.col( t ), t ); + cost += stage_cost( X.col( t ), U.col( t ), static_cast( t ) ); } cost += terminal_cost( X.col( Tp1 - 1 ) ); return cost; } +inline double +compute_trajectory_cost( const StateTrajectory& X, const ControlTrajectory& U, const StageCostFunction& stage_cost, + const TerminalCostFunction& terminal_cost ) +{ + return compute_trajectory_cost( X, U, stage_cost, terminal_cost ); +} + +template struct OCP { + using ScalarType = Scalar; + using State = StateT; + using StateDerivative = StateDerivativeT; + using Control = ControlT; + using StateTrajectory = StateTrajectoryT; + using ControlTrajectory = ControlTrajectoryT; + using MotionModel = MotionModelT; + using ObjectiveFunction = ObjectiveFunctionT; + using StageCostFunction = StageCostFunctionT; + using TerminalCostFunction = TerminalCostFunctionT; + using ConstraintsFunction = ConstraintsFunctionT; + using ConstraintsJacobianFunction = ConstraintsJacobianFunctionT; + using ConstraintViolations = ConstraintViolationsT; + using DynamicsStateJacobian = DynamicsStateJacobianT; + using DynamicsControlJacobian = DynamicsControlJacobianT; + using CostStateGradient = CostStateGradientT; + using CostControlGradient = CostControlGradientT; + using CostStateHessian = CostStateHessianT; + using CostControlHessian = CostControlHessianT; + using CostCrossTerm = CostCrossTermT; + using TerminalCostGradient = TerminalCostGradientT; + using TerminalCostHessian = TerminalCostHessianT; // intial guess and output StateTrajectory initial_states; @@ -36,21 +69,22 @@ struct OCP StateTrajectory best_states; ControlTrajectory best_controls; - double best_cost = std::numeric_limits::max(); + Scalar best_cost = std::numeric_limits::max(); // Dynamics and Objective State initial_state; MotionModel dynamics; - StageCostFunction stage_cost = []( const State&, const Control&, size_t ) { return 0.0; }; - TerminalCostFunction terminal_cost = []( const State& ) { return 0.0; }; + StageCostFunction stage_cost + = []( const State&, const Control&, size_t ) { return static_cast( 0 ); }; + TerminalCostFunction terminal_cost = []( const State& ) { return static_cast( 0 ); }; // objective function is sum of all stage costs + terminal cost ObjectiveFunction objective_function; int control_dim = 0; int state_dim = 0; int horizon_steps = 0; - double dt; + Scalar dt = static_cast( 0 ); // Static bounds std::optional state_lower_bounds = std::nullopt; @@ -84,7 +118,8 @@ struct OCP reset() { initial_controls = ControlTrajectory::Zero( control_dim, horizon_steps ); - initial_states = integrate_horizon( initial_state, initial_controls, dt, dynamics, integrate_rk4 ); + initial_states + = integrate_horizon( initial_state, initial_controls, dt, dynamics, integrate_rk4 ); best_states = initial_states; best_controls = initial_controls; @@ -107,7 +142,7 @@ struct OCP { initial_controls = ControlTrajectory::Zero( control_dim, horizon_steps ); } - initial_states = integrate_horizon( initial_state, initial_controls, dt, dynamics, integrate_rk4 ); + initial_states = integrate_horizon( initial_state, initial_controls, dt, dynamics, integrate_rk4 ); best_states = initial_states; best_controls = initial_controls; @@ -115,24 +150,24 @@ struct OCP // use finite differences when derivatives are not specified if( !dynamics_state_jacobian ) - dynamics_state_jacobian = compute_dynamics_state_jacobian; + dynamics_state_jacobian = compute_dynamics_state_jacobian; if( !dynamics_control_jacobian ) - dynamics_control_jacobian = compute_dynamics_control_jacobian; + dynamics_control_jacobian = compute_dynamics_control_jacobian; if( !cost_state_gradient ) - cost_state_gradient = compute_cost_state_gradient; + cost_state_gradient = compute_cost_state_gradient; if( !cost_control_gradient ) - cost_control_gradient = compute_cost_control_gradient; + cost_control_gradient = compute_cost_control_gradient; if( !cost_state_hessian ) - cost_state_hessian = compute_cost_state_hessian; + cost_state_hessian = compute_cost_state_hessian; if( !cost_control_hessian ) - cost_control_hessian = compute_cost_control_hessian; + cost_control_hessian = compute_cost_control_hessian; if( !cost_cross_term ) - cost_cross_term = compute_cost_cross_term; + cost_cross_term = compute_cost_cross_term; if( !terminal_cost_gradient ) - terminal_cost_gradient = compute_terminal_cost_gradient; + terminal_cost_gradient = compute_terminal_cost_gradient; if( !terminal_cost_hessian ) - terminal_cost_hessian = compute_terminal_cost_hessian; + terminal_cost_hessian = compute_terminal_cost_hessian; if( equality_constraints ) { @@ -140,14 +175,14 @@ struct OCP { auto equality_fn = equality_constraints; equality_constraints_state_jacobian = [equality_fn]( const State& state, const Control& control ) { - return compute_constraints_state_jacobian( equality_fn, state, control ); + return compute_constraints_state_jacobian( equality_fn, state, control ); }; } if( !equality_constraints_control_jacobian ) { auto equality_fn = equality_constraints; equality_constraints_control_jacobian = [equality_fn]( const State& state, const Control& control ) { - return compute_constraints_control_jacobian( equality_fn, state, control ); + return compute_constraints_control_jacobian( equality_fn, state, control ); }; } } @@ -158,14 +193,14 @@ struct OCP { auto inequality_fn = inequality_constraints; inequality_constraints_state_jacobian = [inequality_fn]( const State& state, const Control& control ) { - return compute_constraints_state_jacobian( inequality_fn, state, control ); + return compute_constraints_state_jacobian( inequality_fn, state, control ); }; } if( !inequality_constraints_control_jacobian ) { auto inequality_fn = inequality_constraints; inequality_constraints_control_jacobian = [inequality_fn]( const State& state, const Control& control ) { - return compute_constraints_control_jacobian( inequality_fn, state, control ); + return compute_constraints_control_jacobian( inequality_fn, state, control ); }; } } @@ -174,9 +209,9 @@ struct OCP { auto stage_cost_local = stage_cost; auto terminal_cost_local = terminal_cost; - objective_function = [stage_cost_local, terminal_cost_local]( const StateTrajectory& states, - const ControlTrajectory& controls ) -> double { - return compute_trajectory_cost( states, controls, stage_cost_local, terminal_cost_local ); + objective_function = [stage_cost_local, terminal_cost_local]( const StateTrajectory& states, + const ControlTrajectory& controls ) -> Scalar { + return compute_trajectory_cost( states, controls, stage_cost_local, terminal_cost_local ); }; } best_cost = objective_function( initial_states, initial_controls ); @@ -189,7 +224,7 @@ struct OCP assert( state_dim != 0 && "No state dimension" ); assert( control_dim != 0 && "No control dimension" ); assert( horizon_steps != 0 && "No horizon dimension" ); - assert( dt != 0.0 && "dt is 0.0" ); + assert( dt != static_cast( 0 ) && "dt is 0.0" ); assert( initial_state.size() == state_dim && "Initial state size does not match state dimension" ); @@ -218,7 +253,8 @@ struct OCP assert( dynamics_output.size() == state_dim && "Dynamics output size mismatch" ); // Test objective function - double cost = objective_function( best_states, best_controls ); + Scalar cost = objective_function( best_states, best_controls ); + (void)cost; // If constraints exist, test them if( inequality_constraints ) @@ -235,4 +271,8 @@ struct OCP return true; } }; -} // namespace mas \ No newline at end of file + +using OCPd = OCP; +using OCPf = OCP; + +} // namespace mas diff --git a/include/multi_agent_solver/solution.hpp b/include/multi_agent_solver/solution.hpp index 52b454b..4533c5b 100644 --- a/include/multi_agent_solver/solution.hpp +++ b/include/multi_agent_solver/solution.hpp @@ -6,12 +6,20 @@ namespace mas { -struct Solution +template +struct SolutionT { + using StateTrajectory = StateTrajectoryT; + using ControlTrajectory = ControlTrajectoryT; + std::vector states; std::vector controls; - std::vector costs; - double total_cost = 0.0; + std::vector costs; + Scalar total_cost = static_cast( 0 ); }; +using Solution = SolutionT; +using Solutiond = SolutionT; +using Solutionf = SolutionT; + } // namespace mas diff --git a/include/multi_agent_solver/solvers/cgd.hpp b/include/multi_agent_solver/solvers/cgd.hpp index e1a2b8d..80a8d22 100644 --- a/include/multi_agent_solver/solvers/cgd.hpp +++ b/include/multi_agent_solver/solvers/cgd.hpp @@ -9,7 +9,6 @@ #include "multi_agent_solver/integrator.hpp" #include "multi_agent_solver/line_search.hpp" #include "multi_agent_solver/ocp.hpp" -#include "multi_agent_solver/solvers/solver.hpp" #include "multi_agent_solver/types.hpp" namespace mas @@ -22,9 +21,21 @@ namespace mas * All scratch buffers (multipliers, penalty parameter) live on the object, * so nothing is reallocated between calls. */ +template class CGD { public: + using ScalarType = Scalar; + using State = StateT; + using Control = ControlT; + using StateTrajectory = StateTrajectoryT; + using ControlTrajectory = ControlTrajectoryT; + using ControlGradient = ControlGradientT; + using MotionModel = MotionModelT; + using ObjectiveFunction = ObjectiveFunctionT; + using ConstraintViolations = ConstraintViolationsT; + using SolverParams = SolverParamsT; + using Problem = OCP; /// Construct from the usual parameter map. explicit CGD() {} @@ -35,8 +46,8 @@ class CGD max_iterations = static_cast( params.at( "max_iterations" ) ); tolerance = params.at( "tolerance" ); max_ms = params.at( "max_ms" ); - penalty_param = 1.0; - debug = params.count( "debug" ) && params.at( "debug" ) > 0.5; + penalty_param = static_cast( 1 ); + debug = params.count( "debug" ) && params.at( "debug" ) > static_cast( 0.5 ); } /** @@ -46,7 +57,7 @@ class CGD * @p problem.best_cost */ void - solve( OCP& problem ) + solve( Problem& problem ) { using clock = std::chrono::high_resolution_clock; const auto start_time = clock::now(); @@ -57,25 +68,28 @@ class CGD auto& state_trajectory = problem.best_states; auto& cost = problem.best_cost; - state_trajectory = integrate_horizon( problem.initial_state, controls, problem.dt, problem.dynamics, integrate_rk4 ); + state_trajectory = integrate_horizon( problem.initial_state, controls, problem.dt, problem.dynamics, + integrate_rk4 ); cost = compute_augmented_cost( problem, eq_multipliers, ineq_multipliers, penalty_param, state_trajectory, controls ); for( int iter = 0; iter < max_iterations; ++iter ) { - const double elapsed_ms = std::chrono::duration_cast( clock::now() - start_time ).count(); + const double elapsed_ms + = static_cast( std::chrono::duration_cast( clock::now() - start_time ).count() ); - if( elapsed_ms > max_ms && debug ) + if( elapsed_ms > static_cast( max_ms ) && debug ) { std::cout << "CGD solver terminated early: " << elapsed_ms << " ms > " << max_ms << " ms\n"; break; } - const ControlGradient gradients = finite_differences_gradient( problem.initial_state, controls, problem.dynamics, - problem.objective_function, problem.dt ); + const ControlGradient gradients + = finite_differences_gradient( problem.initial_state, controls, problem.dynamics, + problem.objective_function, problem.dt ); - const double step_size = armijo_line_search( problem.initial_state, controls, gradients, problem.dynamics, problem.objective_function, - problem.dt, {} ); + const Scalar step_size = armijo_line_search( problem.initial_state, controls, gradients, problem.dynamics, + problem.objective_function, problem.dt, {} ); ControlTrajectory trial_controls = controls - step_size * gradients; if( problem.input_lower_bounds && problem.input_upper_bounds ) @@ -83,13 +97,13 @@ class CGD clamp_controls( trial_controls, problem.input_lower_bounds.value(), problem.input_upper_bounds.value() ); } - const StateTrajectory trial_trajectory = integrate_horizon( problem.initial_state, trial_controls, problem.dt, problem.dynamics, - integrate_rk4 ); + const StateTrajectory trial_trajectory + = integrate_horizon( problem.initial_state, trial_controls, problem.dt, problem.dynamics, integrate_rk4 ); - const double trial_cost = compute_augmented_cost( problem, eq_multipliers, ineq_multipliers, penalty_param, trial_trajectory, - trial_controls ); + const Scalar trial_cost + = compute_augmented_cost( problem, eq_multipliers, ineq_multipliers, penalty_param, trial_trajectory, trial_controls ); - const double old_cost = cost; + const Scalar old_cost = cost; if( trial_cost < cost ) { controls = std::move( trial_controls ); @@ -101,7 +115,7 @@ class CGD increase_penalty_parameter( penalty_param, problem, state_trajectory, controls, tolerance ); - if( std::abs( old_cost - trial_cost ) < tolerance && debug ) + if( std::abs( static_cast( old_cost - trial_cost ) ) < static_cast( tolerance ) && debug ) { std::cout << "CGD solver converged in " << iter << "steps" << std::endl; break; @@ -112,12 +126,13 @@ class CGD private: void - resize_multipliers( const OCP& problem ) + resize_multipliers( const Problem& problem ) { if( problem.equality_constraints ) { - const auto m = problem.equality_constraints( problem.initial_state, {} ).size(); - eq_multipliers.setZero( m ); + Control zero_control = Control::Zero( problem.control_dim ); + const auto m = problem.equality_constraints( problem.initial_state, zero_control ).size(); + eq_multipliers.setZero( static_cast( m ) ); } else { @@ -126,8 +141,9 @@ class CGD if( problem.inequality_constraints ) { - const auto p = problem.inequality_constraints( problem.initial_state, {} ).size(); - ineq_multipliers.setZero( p ); + Control zero_control = Control::Zero( problem.control_dim ); + const auto p = problem.inequality_constraints( problem.initial_state, zero_control ).size(); + ineq_multipliers.setZero( static_cast( p ) ); } else { @@ -136,13 +152,16 @@ class CGD } int max_iterations; - double tolerance; - double max_ms; + Scalar tolerance; + Scalar max_ms; bool debug = false; ConstraintViolations eq_multipliers; ConstraintViolations ineq_multipliers; - double penalty_param; + Scalar penalty_param; }; +using CGDd = CGD; +using CGDf = CGD; + } // namespace mas diff --git a/include/multi_agent_solver/solvers/ilqr.hpp b/include/multi_agent_solver/solvers/ilqr.hpp index e09ac91..7c655a8 100644 --- a/include/multi_agent_solver/solvers/ilqr.hpp +++ b/include/multi_agent_solver/solvers/ilqr.hpp @@ -20,18 +20,34 @@ namespace mas /** * @brief Augmented-Lagrangian iLQR solver supporting path equality and inequality constraints. */ +template class iLQR { public: + using ScalarType = Scalar; + using State = StateT; + using Control = ControlT; + using StateTrajectory = StateTrajectoryT; + using ControlTrajectory = ControlTrajectoryT; + using MotionModel = MotionModelT; + using ObjectiveFunction = ObjectiveFunctionT; + using ConstraintViolations = ConstraintViolationsT; + using SolverParams = SolverParamsT; + using Problem = OCP; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using Array = Eigen::Array; + using LLT = Eigen::LLT; + iLQR() : max_iterations( 50 ) - , tolerance( 1e-6 ) - , max_ms( std::numeric_limits::infinity() ) + , tolerance( static_cast( 1e-6 ) ) + , max_ms( std::numeric_limits::infinity() ) , debug( false ) - , penalty_parameter( 10.0 ) - , penalty_increase( 5.0 ) - , constraint_tolerance( 1e-4 ) - , inequality_activation_tolerance( 1e-6 ) + , penalty_parameter( static_cast( 10 ) ) + , penalty_increase( static_cast( 5 ) ) + , constraint_tolerance( static_cast( 1e-4 ) ) + , inequality_activation_tolerance( static_cast( 1e-6 ) ) , equality_dim( 0 ) , inequality_dim( 0 ) {} @@ -42,7 +58,7 @@ class iLQR max_iterations = static_cast( params.at( "max_iterations" ) ); tolerance = params.at( "tolerance" ); max_ms = params.at( "max_ms" ); - debug = params.count( "debug" ) && params.at( "debug" ) > 0.5; + debug = params.count( "debug" ) && params.at( "debug" ) > static_cast( 0.5 ); if( auto it = params.find( "penalty" ); it != params.end() ) penalty_parameter = it->second; @@ -56,7 +72,7 @@ class iLQR //------------------------------- API ----------------------------------// void - solve( OCP& problem ) + solve( Problem& problem ) { using clock = std::chrono::high_resolution_clock; const auto start = clock::now(); @@ -66,23 +82,24 @@ class iLQR const int T = problem.horizon_steps; const int nx = problem.state_dim; const int nu = problem.control_dim; - const double dt = problem.dt; + const Scalar dt = problem.dt; StateTrajectory& x = problem.best_states; ControlTrajectory& u = problem.best_controls; - double& cost = problem.best_cost; + Scalar& cost = problem.best_cost; - x = integrate_horizon( problem.initial_state, u, dt, problem.dynamics, integrate_rk4 ); + x = integrate_horizon( problem.initial_state, u, dt, problem.dynamics, integrate_rk4 ); cost = problem.objective_function( x, u ); - double current_merit = compute_merit( problem, x, u ); + Scalar current_merit = compute_merit( problem, x, u ); if( debug ) std::cout << "iLQR initial cost=" << cost << " merit=" << current_merit << '\n'; for( int iter = 0; iter < max_iterations; ++iter ) { - const double elapsed_ms = std::chrono::duration_cast( clock::now() - start ).count(); - if( elapsed_ms > max_ms ) + const double elapsed_ms + = static_cast( std::chrono::duration_cast( clock::now() - start ).count() ); + if( elapsed_ms > static_cast( max_ms ) ) { if( debug ) std::cout << "iLQR time limit hit after " << elapsed_ms << " ms / " << max_ms << " ms\n"; @@ -99,7 +116,7 @@ class iLQR else v_xx.setZero(); - v_xx = 0.5 * ( v_xx + v_xx.transpose() ); + v_xx = static_cast( 0.5 ) * ( v_xx + v_xx.transpose() ); for( int t = T - 1; t >= 0; --t ) { @@ -122,16 +139,15 @@ class iLQR { eq_residuals[t] = problem.equality_constraints( x.col( t ), u.col( t ) ); eq_jacobian_x[t] - = problem.equality_constraints_state_jacobian ? problem.equality_constraints_state_jacobian( x.col( t ), u.col( t ) ) - : compute_constraints_state_jacobian( problem.equality_constraints, - x.col( t ), - u.col( t ) ); + = problem.equality_constraints_state_jacobian + ? problem.equality_constraints_state_jacobian( x.col( t ), u.col( t ) ) + : compute_constraints_state_jacobian( problem.equality_constraints, x.col( t ), u.col( t ) ); eq_jacobian_u[t] = problem.equality_constraints_control_jacobian ? problem.equality_constraints_control_jacobian( x.col( t ), u.col( t ) ) - : compute_constraints_control_jacobian( problem.equality_constraints, x.col( t ), u.col( t ) ); + : compute_constraints_control_jacobian( problem.equality_constraints, x.col( t ), u.col( t ) ); - const Eigen::VectorXd dual = eq_multipliers[t] + penalty_parameter * eq_residuals[t]; + const Vector dual = eq_multipliers[t] + penalty_parameter * eq_residuals[t]; q_x_step[t] += eq_jacobian_x[t].transpose() * dual; q_u_step[t] += eq_jacobian_u[t].transpose() * dual; @@ -146,23 +162,22 @@ class iLQR ineq_jacobian_x[t] = problem.inequality_constraints_state_jacobian ? problem.inequality_constraints_state_jacobian( x.col( t ), u.col( t ) ) - : compute_constraints_state_jacobian( problem.inequality_constraints, x.col( t ), u.col( t ) ); + : compute_constraints_state_jacobian( problem.inequality_constraints, x.col( t ), u.col( t ) ); ineq_jacobian_u[t] = problem.inequality_constraints_control_jacobian ? problem.inequality_constraints_control_jacobian( x.col( t ), u.col( t ) ) - : compute_constraints_control_jacobian( problem.inequality_constraints, x.col( t ), u.col( t ) ); + : compute_constraints_control_jacobian( problem.inequality_constraints, x.col( t ), u.col( t ) ); - const Eigen::VectorXd slack = ineq_residuals[t].cwiseMax( 0.0 ); - const Eigen::ArrayXd active - = ( ineq_residuals[t].array() > -inequality_activation_tolerance ).cast(); - const Eigen::VectorXd dual = ineq_multipliers[t].array() * active + penalty_parameter * slack.array() * active; + const Vector slack = ineq_residuals[t].cwiseMax( static_cast( 0 ) ); + const Array active = ( ineq_residuals[t].array() > -inequality_activation_tolerance ).template cast(); + const Vector dual = ineq_multipliers[t].array() * active + penalty_parameter * slack.array() * active; q_x_step[t] += ineq_jacobian_x[t].transpose() * dual; q_u_step[t] += ineq_jacobian_u[t].transpose() * dual; if( active.any() ) { - const Eigen::MatrixXd active_diag = active.matrix().asDiagonal(); + const Matrix active_diag = active.matrix().asDiagonal(); q_xx_step[t] += penalty_parameter * ineq_jacobian_x[t].transpose() * active_diag * ineq_jacobian_x[t]; q_ux_step[t] += penalty_parameter * ineq_jacobian_u[t].transpose() * active_diag * ineq_jacobian_x[t]; q_uu_step[t] += penalty_parameter * ineq_jacobian_u[t].transpose() * active_diag * ineq_jacobian_u[t]; @@ -171,14 +186,14 @@ class iLQR q_uu_reg_step[t] = q_uu_step[t]; auto& llt = llt_step[t]; - double reg = 1e-6; + Scalar reg = static_cast( 1e-6 ); while( true ) { llt.compute( q_uu_reg_step[t] ); if( llt.info() == Eigen::Success ) break; q_uu_reg_step[t] += reg * identity_nu; - reg *= 10.0; + reg *= static_cast( 10 ); } q_uu_inv_step[t] = llt.solve( identity_nu ); @@ -189,17 +204,17 @@ class iLQR + k_matrix[t].transpose() * q_uu_step[t] * k[t]; v_xx = q_xx_step[t] + k_matrix[t].transpose() * q_ux_step[t] + q_ux_step[t].transpose() * k_matrix[t] + k_matrix[t].transpose() * q_uu_step[t] * k_matrix[t]; - v_xx = 0.5 * ( v_xx + v_xx.transpose() ); + v_xx = static_cast( 0.5 ) * ( v_xx + v_xx.transpose() ); } x_trial.setZero(); u_trial.setZero(); x_trial.col( 0 ) = problem.initial_state; - const double amin = 1e-3; - double alpha = 1.0; + const Scalar amin = static_cast( 1e-3 ); + Scalar alpha = static_cast( 1 ); - double best_merit = current_merit; + Scalar best_merit = current_merit; StateTrajectory best_x = x; ControlTrajectory best_u = u; @@ -207,16 +222,16 @@ class iLQR { for( int t = 0; t < T; ++t ) { - const Eigen::VectorXd dx = x_trial.col( t ) - x.col( t ); + const Vector dx = x_trial.col( t ) - x.col( t ); u_trial.col( t ) = u.col( t ) + alpha * k[t] + k_matrix[t] * dx; if( problem.input_lower_bounds && problem.input_upper_bounds ) - clamp_controls( u_trial, *problem.input_lower_bounds, *problem.input_upper_bounds ); + clamp_controls( u_trial, *problem.input_lower_bounds, *problem.input_upper_bounds ); - x_trial.col( t + 1 ) = integrate_rk4( x_trial.col( t ), u_trial.col( t ), dt, problem.dynamics ); + x_trial.col( t + 1 ) = integrate_rk4( x_trial.col( t ), u_trial.col( t ), dt, problem.dynamics ); } - const double trial_merit = compute_merit( problem, x_trial, u_trial ); + const Scalar trial_merit = compute_merit( problem, x_trial, u_trial ); if( trial_merit < best_merit ) { best_merit = trial_merit; @@ -224,37 +239,39 @@ class iLQR best_u = u_trial; break; } - alpha *= 0.5; + alpha *= static_cast( 0.5 ); } - const double improvement = current_merit - best_merit; + const Scalar improvement = current_merit - best_merit; x = best_x; u = best_u; cost = problem.objective_function( x, u ); current_merit = best_merit; - double eq_violation_norm = 0.0; - double ineq_violation_norm = 0.0; + Scalar eq_violation_norm = static_cast( 0 ); + Scalar ineq_violation_norm = static_cast( 0 ); for( int t = 0; t < T; ++t ) { if( equality_dim > 0 && problem.equality_constraints ) { - const Eigen::VectorXd residual = problem.equality_constraints( x.col( t ), u.col( t ) ); + const Vector residual = problem.equality_constraints( x.col( t ), u.col( t ) ); eq_multipliers[t] += penalty_parameter * residual; eq_violation_norm += residual.squaredNorm(); } if( inequality_dim > 0 && problem.inequality_constraints ) { - const Eigen::VectorXd residual = problem.inequality_constraints( x.col( t ), u.col( t ) ); - const Eigen::VectorXd positive = residual.cwiseMax( 0.0 ); - ineq_multipliers[t] = ( ineq_multipliers[t] + penalty_parameter * positive ).cwiseMax( 0.0 ); + const Vector residual = problem.inequality_constraints( x.col( t ), u.col( t ) ); + const Vector positive = residual.cwiseMax( static_cast( 0 ) ); + ineq_multipliers[t] = ( ineq_multipliers[t] + penalty_parameter * positive ).cwiseMax( static_cast( 0 ) ); ineq_violation_norm += positive.squaredNorm(); } } - eq_violation_norm = std::sqrt( eq_violation_norm ); - ineq_violation_norm = std::sqrt( ineq_violation_norm ); + eq_violation_norm + = static_cast( std::sqrt( static_cast( eq_violation_norm ) ) ); + ineq_violation_norm + = static_cast( std::sqrt( static_cast( ineq_violation_norm ) ) ); if( eq_violation_norm > constraint_tolerance || ineq_violation_norm > constraint_tolerance ) penalty_parameter *= penalty_increase; @@ -266,8 +283,8 @@ class iLQR << '\n'; } - if( improvement < tolerance && eq_violation_norm < constraint_tolerance - && ineq_violation_norm < constraint_tolerance ) + if( static_cast( std::abs( improvement ) ) < static_cast( tolerance ) + && eq_violation_norm < constraint_tolerance && ineq_violation_norm < constraint_tolerance ) break; } } @@ -275,7 +292,7 @@ class iLQR private: //---------------- buffer management ----------------------------------// void - resize_buffers( const OCP& problem ) + resize_buffers( const Problem& problem ) { const int T = problem.horizon_steps; const int nx = problem.state_dim; @@ -289,27 +306,27 @@ class iLQR m.setZero(); }; - resize_mat_vec( k, Eigen::VectorXd::Zero( nu ) ); - resize_mat_vec( k_matrix, Eigen::MatrixXd::Zero( nu, nx ) ); - - resize_mat_vec( a_step, Eigen::MatrixXd::Zero( nx, nx ) ); - resize_mat_vec( b_step, Eigen::MatrixXd::Zero( nx, nu ) ); - resize_mat_vec( l_x_step, Eigen::VectorXd::Zero( nx ) ); - resize_mat_vec( l_u_step, Eigen::VectorXd::Zero( nu ) ); - resize_mat_vec( l_xx_step, Eigen::MatrixXd::Zero( nx, nx ) ); - resize_mat_vec( l_uu_step, Eigen::MatrixXd::Zero( nu, nu ) ); - resize_mat_vec( l_ux_step, Eigen::MatrixXd::Zero( nu, nx ) ); - - resize_mat_vec( q_x_step, Eigen::VectorXd::Zero( nx ) ); - resize_mat_vec( q_u_step, Eigen::VectorXd::Zero( nu ) ); - resize_mat_vec( q_xx_step, Eigen::MatrixXd::Zero( nx, nx ) ); - resize_mat_vec( q_ux_step, Eigen::MatrixXd::Zero( nu, nx ) ); - resize_mat_vec( q_uu_step, Eigen::MatrixXd::Zero( nu, nu ) ); - resize_mat_vec( q_uu_reg_step, Eigen::MatrixXd::Zero( nu, nu ) ); - resize_mat_vec( q_uu_inv_step, Eigen::MatrixXd::Zero( nu, nu ) ); + resize_mat_vec( k, Vector::Zero( nu ) ); + resize_mat_vec( k_matrix, Matrix::Zero( nu, nx ) ); + + resize_mat_vec( a_step, Matrix::Zero( nx, nx ) ); + resize_mat_vec( b_step, Matrix::Zero( nx, nu ) ); + resize_mat_vec( l_x_step, Vector::Zero( nx ) ); + resize_mat_vec( l_u_step, Vector::Zero( nu ) ); + resize_mat_vec( l_xx_step, Matrix::Zero( nx, nx ) ); + resize_mat_vec( l_uu_step, Matrix::Zero( nu, nu ) ); + resize_mat_vec( l_ux_step, Matrix::Zero( nu, nx ) ); + + resize_mat_vec( q_x_step, Vector::Zero( nx ) ); + resize_mat_vec( q_u_step, Vector::Zero( nu ) ); + resize_mat_vec( q_xx_step, Matrix::Zero( nx, nx ) ); + resize_mat_vec( q_ux_step, Matrix::Zero( nu, nx ) ); + resize_mat_vec( q_uu_step, Matrix::Zero( nu, nu ) ); + resize_mat_vec( q_uu_reg_step, Matrix::Zero( nu, nu ) ); + resize_mat_vec( q_uu_inv_step, Matrix::Zero( nu, nu ) ); if( static_cast( llt_step.size() ) != T ) - llt_step.assign( T, Eigen::LLT( nu ) ); + llt_step.assign( T, LLT( nu ) ); Control default_control = Control::Zero( nu ); if( problem.initial_controls.cols() == T ) @@ -324,17 +341,17 @@ class iLQR if( equality_dim > 0 ) { - resize_mat_vec( eq_residuals, Eigen::VectorXd::Zero( equality_dim ) ); - resize_mat_vec( eq_jacobian_x, Eigen::MatrixXd::Zero( equality_dim, nx ) ); - resize_mat_vec( eq_jacobian_u, Eigen::MatrixXd::Zero( equality_dim, nu ) ); + resize_mat_vec( eq_residuals, Vector::Zero( equality_dim ) ); + resize_mat_vec( eq_jacobian_x, Matrix::Zero( equality_dim, nx ) ); + resize_mat_vec( eq_jacobian_u, Matrix::Zero( equality_dim, nu ) ); if( static_cast( eq_multipliers.size() ) != T ) - eq_multipliers.assign( T, Eigen::VectorXd::Zero( equality_dim ) ); + eq_multipliers.assign( T, Vector::Zero( equality_dim ) ); else for( auto& m : eq_multipliers ) { if( m.size() != equality_dim ) - m = Eigen::VectorXd::Zero( equality_dim ); + m = Vector::Zero( equality_dim ); } } else @@ -347,17 +364,17 @@ class iLQR if( inequality_dim > 0 ) { - resize_mat_vec( ineq_residuals, Eigen::VectorXd::Zero( inequality_dim ) ); - resize_mat_vec( ineq_jacobian_x, Eigen::MatrixXd::Zero( inequality_dim, nx ) ); - resize_mat_vec( ineq_jacobian_u, Eigen::MatrixXd::Zero( inequality_dim, nu ) ); + resize_mat_vec( ineq_residuals, Vector::Zero( inequality_dim ) ); + resize_mat_vec( ineq_jacobian_x, Matrix::Zero( inequality_dim, nx ) ); + resize_mat_vec( ineq_jacobian_u, Matrix::Zero( inequality_dim, nu ) ); if( static_cast( ineq_multipliers.size() ) != T ) - ineq_multipliers.assign( T, Eigen::VectorXd::Zero( inequality_dim ) ); + ineq_multipliers.assign( T, Vector::Zero( inequality_dim ) ); else for( auto& m : ineq_multipliers ) { if( m.size() != inequality_dim ) - m = Eigen::VectorXd::Zero( inequality_dim ); + m = Vector::Zero( inequality_dim ); } } else @@ -373,33 +390,33 @@ class iLQR v_x.resize( nx ); v_xx.resize( nx, nx ); - identity_nu = Eigen::MatrixXd::Identity( nu, nu ); + identity_nu = Matrix::Identity( nu, nu ); } - double - compute_merit( const OCP& problem, const StateTrajectory& states, const ControlTrajectory& controls ) const + Scalar + compute_merit( const Problem& problem, const StateTrajectory& states, const ControlTrajectory& controls ) const { const int T = problem.horizon_steps; - double merit - = problem.objective_function ? problem.objective_function( states, controls ) : problem.best_cost; + Scalar merit = problem.objective_function ? problem.objective_function( states, controls ) : problem.best_cost; for( int t = 0; t < T; ++t ) { if( equality_dim > 0 && problem.equality_constraints ) { - const Eigen::VectorXd residual = problem.equality_constraints( states.col( t ), controls.col( t ) ); - merit += eq_multipliers[t].dot( residual ) + 0.5 * penalty_parameter * residual.squaredNorm(); + const Vector residual = problem.equality_constraints( states.col( t ), controls.col( t ) ); + merit += eq_multipliers[t].dot( residual ) + + static_cast( 0.5 ) * penalty_parameter * residual.squaredNorm(); } if( inequality_dim > 0 && problem.inequality_constraints ) { - const Eigen::VectorXd residual = problem.inequality_constraints( states.col( t ), controls.col( t ) ); - const Eigen::VectorXd slack = residual.cwiseMax( 0.0 ); - const Eigen::ArrayXd active - = ( residual.array() > -inequality_activation_tolerance ).cast(); - const Eigen::VectorXd active_slack = slack.array() * active; - const Eigen::VectorXd weighted_multiply = ineq_multipliers[t].array() * active; + const Vector residual = problem.inequality_constraints( states.col( t ), controls.col( t ) ); + const Vector slack = residual.cwiseMax( static_cast( 0 ) ); + const Array active + = ( residual.array() > -inequality_activation_tolerance ).template cast(); + const Vector active_slack = slack.array() * active; + const Vector weighted_multiply = ineq_multipliers[t].array() * active; merit += weighted_multiply.dot( active_slack ); - merit += 0.5 * penalty_parameter * active_slack.squaredNorm(); + merit += static_cast( 0.5 ) * penalty_parameter * active_slack.squaredNorm(); } } @@ -408,56 +425,59 @@ class iLQR //---------------- data members ---------------------------------------// int max_iterations; - double tolerance; - double max_ms; + Scalar tolerance; + Scalar max_ms; bool debug; - double penalty_parameter; - double penalty_increase; - double constraint_tolerance; - double inequality_activation_tolerance; + Scalar penalty_parameter; + Scalar penalty_increase; + Scalar constraint_tolerance; + Scalar inequality_activation_tolerance; int equality_dim; int inequality_dim; - std::vector k; - std::vector k_matrix; + std::vector k; + std::vector k_matrix; - std::vector a_step; - std::vector b_step; + std::vector a_step; + std::vector b_step; - std::vector l_x_step; - std::vector l_u_step; - std::vector l_xx_step; - std::vector l_uu_step; - std::vector l_ux_step; + std::vector l_x_step; + std::vector l_u_step; + std::vector l_xx_step; + std::vector l_uu_step; + std::vector l_ux_step; - std::vector q_x_step; - std::vector q_u_step; - std::vector q_xx_step; - std::vector q_ux_step; - std::vector q_uu_step; - std::vector q_uu_reg_step; - std::vector q_uu_inv_step; + std::vector q_x_step; + std::vector q_u_step; + std::vector q_xx_step; + std::vector q_ux_step; + std::vector q_uu_step; + std::vector q_uu_reg_step; + std::vector q_uu_inv_step; - std::vector> llt_step; + std::vector llt_step; - std::vector eq_residuals; - std::vector eq_jacobian_x; - std::vector eq_jacobian_u; - std::vector eq_multipliers; + std::vector eq_residuals; + std::vector eq_jacobian_x; + std::vector eq_jacobian_u; + std::vector eq_multipliers; - std::vector ineq_residuals; - std::vector ineq_jacobian_x; - std::vector ineq_jacobian_u; - std::vector ineq_multipliers; + std::vector ineq_residuals; + std::vector ineq_jacobian_x; + std::vector ineq_jacobian_u; + std::vector ineq_multipliers; StateTrajectory x_trial; ControlTrajectory u_trial; - Eigen::VectorXd v_x; - Eigen::MatrixXd v_xx; - Eigen::MatrixXd identity_nu; + Vector v_x; + Matrix v_xx; + Matrix identity_nu; }; +using iLQRd = iLQR; +using iLQRf = iLQR; + } // namespace mas diff --git a/include/multi_agent_solver/solvers/osqp.hpp b/include/multi_agent_solver/solvers/osqp.hpp index 04dc239..6fc734b 100644 --- a/include/multi_agent_solver/solvers/osqp.hpp +++ b/include/multi_agent_solver/solvers/osqp.hpp @@ -2,18 +2,22 @@ #pragma once #include +#include #include #include +#include +#include #include #include #include +#include + #include "multi_agent_solver/integrator.hpp" +#include "multi_agent_solver/line_search.hpp" #include "multi_agent_solver/ocp.hpp" -#include "multi_agent_solver/solvers/solver.hpp" #include "multi_agent_solver/types.hpp" -#include namespace mas { @@ -30,9 +34,19 @@ namespace mas * matrices instead of reallocating. * */ +template class OSQP { public: + static_assert( std::is_same_v, "OSQP backend currently supports only double precision" ); + + using ScalarType = Scalar; + using StateTrajectory = StateTrajectoryT; + using ControlTrajectory = ControlTrajectoryT; + using MotionModel = MotionModelT; + using ObjectiveFunction = ObjectiveFunctionT; + using SolverParams = SolverParamsT; + using Problem = OCP; explicit OSQP() { @@ -45,7 +59,7 @@ class OSQP max_iterations = static_cast( params.at( "max_iterations" ) ); tolerance = params.at( "tolerance" ); max_ms = params.at( "max_ms" ); - debug = params.count( "debug" ) && params.at( "debug" ) > 0.5; + debug = params.count( "debug" ) && params.at( "debug" ) > static_cast( 0.5 ); solver->settings()->setWarmStart( true ); solver->settings()->setVerbosity( false ); solver->settings()->setAdaptiveRho( true ); @@ -60,7 +74,7 @@ class OSQP * Reuses all pre-allocated memory across calls. */ void - solve( OCP& problem ) + solve( Problem& problem ) { using clock = std::chrono::high_resolution_clock; const auto start = clock::now(); @@ -76,14 +90,15 @@ class OSQP StateTrajectory& states = problem.best_states; ControlTrajectory& controls = problem.best_controls; - double& cost = problem.best_cost; + Scalar& cost = problem.best_cost; - states = integrate_horizon( problem.initial_state, controls, problem.dt, problem.dynamics, integrate_rk4 ); + states = integrate_horizon( problem.initial_state, controls, problem.dt, problem.dynamics, + integrate_rk4 ); cost = problem.objective_function( states, controls ); - double reg = 0.0; + Scalar reg = static_cast( 0 ); assemble_qp_data( problem, states, controls, reg ); @@ -113,16 +128,17 @@ class OSQP if( ls_parameters.empty() ) ls_parameters = { - { "initial_step_size", 1.0 }, - { "beta", 0.5 }, - { "c1", 1e-6 } + { "initial_step_size", static_cast( 1.0 ) }, + { "beta", static_cast( 0.5 ) }, + { "c1", static_cast( 1e-6 ) } }; //--------------------------------------------------------------------// for( int iter = 0; iter < max_iterations; ++iter ) { - const double elapsed_ms = std::chrono::duration_cast( clock::now() - start ).count(); - if( elapsed_ms > max_ms ) + const double elapsed_ms + = static_cast( std::chrono::duration_cast( clock::now() - start ).count() ); + if( elapsed_ms > static_cast( max_ms ) ) { if( debug ) { @@ -167,14 +183,15 @@ class OSQP d_u = controls - u_candidate; - const double alpha = armijo_line_search( problem.initial_state, controls, d_u, problem.dynamics, problem.objective_function, - problem.dt, ls_parameters ); + const Scalar alpha = armijo_line_search( problem.initial_state, controls, d_u, problem.dynamics, + problem.objective_function, problem.dt, ls_parameters ); - u_new = controls - alpha * d_u; - states_new = integrate_horizon( problem.initial_state, u_new, problem.dt, problem.dynamics, integrate_rk4 ); - const double cost_new = problem.objective_function( states_new, u_new ); + u_new = controls - alpha * d_u; + states_new + = integrate_horizon( problem.initial_state, u_new, problem.dt, problem.dynamics, integrate_rk4 ); + const Scalar cost_new = problem.objective_function( states_new, u_new ); - if( std::abs( cost - cost_new ) < tolerance ) + if( std::abs( static_cast( cost - cost_new ) ) < static_cast( tolerance ) ) { controls = u_new; states = states_new; @@ -227,11 +244,11 @@ class OSQP StateTrajectory states_new; - std::map ls_parameters; + std::map ls_parameters; //--------------------------------------------------------------------// void - resize_buffers( const OCP& problem ) + resize_buffers( const Problem& problem ) { const int T = problem.horizon_steps; const int nx = problem.state_dim; @@ -259,7 +276,7 @@ class OSQP //--------------------------------------------------------------------// void - assemble_qp_data( const OCP& p, const StateTrajectory& x, const ControlTrajectory& u, double reg ) + assemble_qp_data( const Problem& p, const StateTrajectory& x, const ControlTrajectory& u, Scalar reg ) { assemble_hessian( p, x, u, reg ); assemble_gradient( p, x, u ); @@ -269,7 +286,7 @@ class OSQP //--------------------------------------------------------------------// void - assemble_hessian( const OCP& p, const StateTrajectory& x, const ControlTrajectory& u, double reg ) + assemble_hessian( const Problem& p, const StateTrajectory& x, const ControlTrajectory& u, Scalar reg ) { @@ -312,7 +329,7 @@ class OSQP //--------------------------------------------------------------------// void - assemble_gradient( const OCP& p, const StateTrajectory& x, const ControlTrajectory& u ) + assemble_gradient( const Problem& p, const StateTrajectory& x, const ControlTrajectory& u ) { const int nx = p.state_dim; const int nu = p.control_dim; @@ -331,7 +348,7 @@ class OSQP //--------------------------------------------------------------------// void - assemble_constraints( const OCP& p, const StateTrajectory& x, const ControlTrajectory& u ) + assemble_constraints( const Problem& p, const StateTrajectory& x, const ControlTrajectory& u ) { const int nx = p.state_dim; const int nu = p.control_dim; @@ -377,7 +394,7 @@ class OSQP //--------------------------------------------------------------------// void - assemble_bounds( const OCP& p ) + assemble_bounds( const Problem& p ) { const int nx = p.state_dim; const int nu = p.control_dim; @@ -410,9 +427,11 @@ class OSQP //--------------------------------------------------------------------// int max_iterations; - double tolerance; - double max_ms; + Scalar tolerance; + Scalar max_ms; bool debug; }; +using OSQPd = OSQP; + } // namespace mas diff --git a/include/multi_agent_solver/solvers/osqp_collocation.hpp b/include/multi_agent_solver/solvers/osqp_collocation.hpp index 955bb74..479a91d 100644 --- a/include/multi_agent_solver/solvers/osqp_collocation.hpp +++ b/include/multi_agent_solver/solvers/osqp_collocation.hpp @@ -2,8 +2,11 @@ /* --------------- Standard / Eigen / OSQP ------------------------ */ #include #include +#include #include #include +#include +#include #include #include @@ -20,16 +23,26 @@ namespace mas /* =================================================================== */ /* Trapezoidal SQP with OSQP */ /* =================================================================== */ +template class OSQPCollocation { public: + static_assert( std::is_same_v, "OSQP collocation backend currently supports only double precision" ); + + using ScalarType = Scalar; + using StateTrajectory = StateTrajectoryT; + using ControlTrajectory = ControlTrajectoryT; + using MotionModel = MotionModelT; + using SolverParams = SolverParamsT; + using Problem = OCP; + explicit OSQPCollocation() : solver{ std::make_unique() } {} void set_params( const SolverParams& params ); - void solve( OCP& problem ); // main entry + void solve( Problem& problem ); // main entry private: @@ -47,10 +60,10 @@ class OSQPCollocation } /* ---------- phase 1: immutable sparsity pattern ---------------- */ - void prepare_structure( const OCP& ); + void prepare_structure( const Problem& ); /* ---------- phase 2: overwrite numeric values ------------------ */ - void assemble_values( const OCP& p, const StateTrajectory& X, const ControlTrajectory& U, double reg ); + void assemble_values( const Problem& p, const StateTrajectory& X, const ControlTrajectory& U, Scalar reg ); /* ---------- data ------------------------------------------------ */ struct qpt @@ -75,10 +88,10 @@ class OSQPCollocation /* settings */ int max_iter{ 20 }; - double tolerance{ 1e-4 }; + Scalar tolerance{ static_cast( 1e-4 ) }; bool debug{ false }; - double max_ms{ 1000.0 }; // max time in ms + Scalar max_ms{ static_cast( 1000.0 ) }; // max time in ms /* caching ----------------------------------------------------- */ bool use_cache{ true }; @@ -92,17 +105,20 @@ class OSQPCollocation std::vector R_cache; // size T }; +using OSQPCollocationd = OSQPCollocation; + /* =================================================================== */ /* Implementation details */ /* =================================================================== */ +template inline void -OSQPCollocation::set_params( const SolverParams& p ) +OSQPCollocation::set_params( const SolverParams& p ) { max_iter = static_cast( p.at( "max_iterations" ) ); tolerance = p.at( "tolerance" ); - debug = p.count( "debug" ) && p.at( "debug" ) > 0.5; - max_ms = p.count( "max_ms" ) ? p.at( "max_ms" ) : 1000.0; - use_cache = p.count( "cache" ) ? p.at( "cache" ) > 0.5 : true; + debug = p.count( "debug" ) && p.at( "debug" ) > static_cast( 0.5 ); + max_ms = p.count( "max_ms" ) ? p.at( "max_ms" ) : static_cast( 1000.0 ); + use_cache = p.count( "cache" ) ? p.at( "cache" ) > static_cast( 0.5 ) : true; solver->settings()->setVerbosity( false ); solver->settings()->setWarmStart( true ); solver->settings()->setAdaptiveRho( true ); @@ -117,8 +133,9 @@ OSQPCollocation::set_params( const SolverParams& p ) /* ------------------------------------------------------------------ */ /* Phase-1: build immutable sparsity pattern and initialise OSQP */ /* ------------------------------------------------------------------ */ +template inline void -OSQPCollocation::prepare_structure( const OCP& p ) +OSQPCollocation::prepare_structure( const Problem& p ) { /* ---------- basic dimensions ---------------------------------- */ T = p.horizon_steps; @@ -240,8 +257,12 @@ OSQPCollocation::prepare_structure( const OCP& p ) structure_ready = true; } +template inline void -OSQPCollocation::assemble_values( const OCP& p, const StateTrajectory& X, const ControlTrajectory& U, double reg ) +OSQPCollocation::assemble_values( const Problem& p, + const StateTrajectory& X, + const ControlTrajectory& U, + Scalar reg ) { qp.g.setZero(); for( int t = 1; t < T; ++t ) @@ -256,7 +277,7 @@ OSQPCollocation::assemble_values( const OCP& p, const StateTrajectory& X, const double* h_val = qp.H.valuePtr(); std::size_t kH = 0; - constexpr double cache_eps = 1e-9; + const Scalar cache_eps = static_cast( 1e-9 ); for( int t = 1; t < T; ++t ) { @@ -437,14 +458,16 @@ OSQPCollocation::assemble_values( const OCP& p, const StateTrajectory& X, const } /* ---------- main entry ------------------------------------------- */ +template inline void -OSQPCollocation::solve( OCP& problem ) +OSQPCollocation::solve( Problem& problem ) { using clk = std::chrono::high_resolution_clock; const auto tic = clk::now(); - const auto deadline = ( max_ms > 0.0 ) ? tic + std::chrono::milliseconds( static_cast( max_ms ) ) - : clk::time_point::max(); // β€œinfinite” if max_ms ≀ 0 + const auto deadline = ( max_ms > static_cast( 0 ) ) + ? tic + std::chrono::milliseconds( static_cast( max_ms ) ) + : clk::time_point::max(); // β€œinfinite” if max_ms ≀ 0 /* dimensions & warm-start guesses ----------------------------- */ if( !structure_ready ) @@ -462,7 +485,7 @@ OSQPCollocation::solve( OCP& problem ) U = problem.initial_controls; X.col( 0 ) = problem.initial_state; // xβ‚€ fixed - constexpr double reg = 1e-6; + const Scalar reg = static_cast( 1e-6 ); cache_valid = false; // invalidate caches for fresh solve @@ -484,15 +507,16 @@ OSQPCollocation::solve( OCP& problem ) const Eigen::VectorXd delta = solver->getSolution(); /* apply step --------------------------------------------- */ - double step_norm2 = 0.0; + Scalar step_norm2 = static_cast( 0 ); for( int t = 0; t < T; ++t ) { X.col( t + 1 ) += delta.segment( t * nx, nx ); U.col( t ) += delta.segment( T * nx + t * nu, nu ); - step_norm2 += delta.segment( t * nx, nx ).squaredNorm() + delta.segment( T * nx + t * nu, nu ).squaredNorm(); + step_norm2 + += static_cast( delta.segment( t * nx, nx ).squaredNorm() + delta.segment( T * nx + t * nu, nu ).squaredNorm() ); } - if( std::sqrt( step_norm2 ) < tolerance ) + if( static_cast( std::sqrt( static_cast( step_norm2 ) ) ) < tolerance ) { if( debug ) std::cerr << "OSQP Collocation converged in " << it + 1 << " SQP steps\n"; diff --git a/include/multi_agent_solver/solvers/solver.hpp b/include/multi_agent_solver/solvers/solver.hpp index 0851a92..f4393fe 100644 --- a/include/multi_agent_solver/solvers/solver.hpp +++ b/include/multi_agent_solver/solvers/solver.hpp @@ -1,6 +1,8 @@ #pragma once +#include +#include #include #include "multi_agent_solver/solvers/cgd.hpp" @@ -13,30 +15,77 @@ namespace mas { -// Holds any of the concrete solver objects. -#ifndef MAS_HAVE_OSQP -using Solver = std::variant; +namespace detail +{ + +template +struct SolverVariantSelector; + +template +struct SolverVariantSelector +{ + using Type = std::variant, CGD>; +}; + +#ifdef MAS_HAVE_OSQP +template +struct SolverVariantSelector +{ + using Type = std::variant, CGD, OSQP, OSQPCollocation>; +}; #endif + +template +struct SolverVariantFor +{ #ifdef MAS_HAVE_OSQP -using Solver = std::variant; + static constexpr bool kEnableOSQP = std::is_same_v; +#else + static constexpr bool kEnableOSQP = false; #endif + using Type = typename SolverVariantSelector::Type; +}; + +} // namespace detail + +// Holds any of the concrete solver objects. +template +using SolverVariant = typename detail::SolverVariantFor::Type; + +using Solverd = SolverVariant; +using Solverf = SolverVariant; +using Solver = Solverd; /** * @brief Convenience visitor to call solve() on the variant without * repeating std::visit everywhere. */ +template inline void -solve( Solver& solver, OCP& problem ) +solve( SolverVariant& solver, OCP& problem ) { std::visit( [&]( auto& s ) { s.solve( problem ); }, solver ); } inline void -set_params( Solver& solver, const SolverParams& params ) +solve( Solver& solver, OCP<>& problem ) +{ + solve( solver, problem ); +} + +template +inline void +set_params( SolverVariant& solver, const SolverParamsT& params ) { std::visit( [&]( auto& s ) { s.set_params( params ); }, solver ); } +inline void +set_params( Solver& solver, const SolverParams& params ) +{ + set_params( solver, params ); +} + template std::shared_ptr create() diff --git a/include/multi_agent_solver/strategies/centralized.hpp b/include/multi_agent_solver/strategies/centralized.hpp index de48678..041ed32 100644 --- a/include/multi_agent_solver/strategies/centralized.hpp +++ b/include/multi_agent_solver/strategies/centralized.hpp @@ -7,19 +7,25 @@ namespace mas { +template struct CentralizedStrategy { - Solver solver; + using SolverType = mas::SolverVariant; + using Problem = MultiAgentProblemT; + using OCPType = OCP; + using Solution = SolutionT; - explicit CentralizedStrategy( Solver s ) : + SolverType solver; + + explicit CentralizedStrategy( SolverType s ) : solver( std::move( s ) ) {} Solution - operator()( MultiAgentProblem& problem ) + operator()( Problem& problem ) { problem.compute_offsets(); - OCP global = problem.build_global_ocp(); + OCPType global = problem.build_global_ocp(); mas::solve( solver, global ); Solution sol; @@ -38,4 +44,7 @@ struct CentralizedStrategy } }; +using CentralizedStrategyd = CentralizedStrategy; +using CentralizedStrategyf = CentralizedStrategy; + } // namespace mas diff --git a/include/multi_agent_solver/strategies/nash.hpp b/include/multi_agent_solver/strategies/nash.hpp index d8b5c98..39651c6 100644 --- a/include/multi_agent_solver/strategies/nash.hpp +++ b/include/multi_agent_solver/strategies/nash.hpp @@ -14,17 +14,19 @@ namespace mas namespace detail { -inline Solver -make_solver_like( const Solver& proto ) +template +inline Variant +make_solver_like( const Variant& proto ) { - return std::visit( []( const auto& s ) -> Solver { return std::decay_t{}; }, proto ); + return std::visit( []( const auto& s ) -> Variant { return std::decay_t{}; }, proto ); } -inline Solution -collect_solution( MultiAgentProblem& problem ) +template +inline SolutionT +collect_solution( MultiAgentProblemT& problem ) { - Solution sol; - sol.total_cost = 0.0; + SolutionT sol; + sol.total_cost = static_cast( 0 ); for( auto& blk : problem.blocks ) { auto& ocp = *blk.agent->ocp; @@ -36,10 +38,11 @@ collect_solution( MultiAgentProblem& problem ) return sol; } -inline double -total_cost( MultiAgentProblem& problem ) +template +inline Scalar +total_cost( MultiAgentProblemT& problem ) { - double c = 0.0; + Scalar c = static_cast( 0 ); const int n = static_cast( problem.blocks.size() ); #pragma omp parallel for reduction( + : c ) schedule( static ) @@ -50,10 +53,10 @@ total_cost( MultiAgentProblem& problem ) return c; } +template inline void -sequential_solve( std::vector& solvers, MultiAgentProblem& problem ) +sequential_solve( std::vector>& solvers, MultiAgentProblemT& problem ) { - // Parallel Jacobi step: solve all -> update all const int n = static_cast( problem.blocks.size() ); #pragma omp parallel for schedule( static ) @@ -71,11 +74,13 @@ sequential_solve( std::vector& solvers, MultiAgentProblem& problem ) } } -inline Solution -run_sequential( int max_outer, const Solver& solver_proto, const SolverParams& params, MultiAgentProblem& problem ) +template +inline SolutionT +run_sequential( int max_outer, const SolverVariant& solver_proto, const SolverParamsT& params, + MultiAgentProblemT& problem ) { problem.compute_offsets(); - std::vector solvers; + std::vector> solvers; solvers.reserve( problem.blocks.size() ); for( std::size_t i = 0; i < problem.blocks.size(); ++i ) { @@ -89,11 +94,16 @@ run_sequential( int max_outer, const Solver& solver_proto, const SolverParams& p return collect_solution( problem ); } -inline Solution -run_line_search( int max_outer, const Solver& solver_proto, const SolverParams& params, MultiAgentProblem& problem ) +template +inline SolutionT +run_line_search( int max_outer, const SolverVariant& solver_proto, const SolverParamsT& params, + MultiAgentProblemT& problem ) { + using ControlTrajectory = ControlTrajectoryT; + using StateTrajectory = StateTrajectoryT; + problem.compute_offsets(); - std::vector solvers; + std::vector> solvers; solvers.reserve( problem.blocks.size() ); for( std::size_t i = 0; i < problem.blocks.size(); ++i ) { @@ -101,14 +111,14 @@ run_line_search( int max_outer, const Solver& solver_proto, const SolverParams& set_params( solvers.back(), params ); } - double base_cost = total_cost( problem ); + Scalar base_cost = total_cost( problem ); for( int outer = 0; outer < max_outer; ++outer ) { const int n = static_cast( problem.blocks.size() ); - std::vector old_controls( n ); - std::vector old_states( n ); - for( std::size_t i = 0; i < n; ++i ) + std::vector old_controls( static_cast( n ) ); + std::vector old_states( static_cast( n ) ); + for( std::size_t i = 0; i < static_cast( n ); ++i ) { auto& ocp = *problem.blocks[i].agent->ocp; old_controls[i] = ocp.best_controls; @@ -116,33 +126,34 @@ run_line_search( int max_outer, const Solver& solver_proto, const SolverParams& } sequential_solve( solvers, problem ); - double new_cost = total_cost( problem ); + Scalar new_cost = total_cost( problem ); if( new_cost >= base_cost ) { - std::vector cand_controls( n ); - for( std::size_t i = 0; i < n; ++i ) + std::vector cand_controls( static_cast( n ) ); + for( std::size_t i = 0; i < static_cast( n ); ++i ) cand_controls[i] = problem.blocks[i].agent->ocp->best_controls; - double alpha = 0.5; + Scalar alpha = static_cast( 0.5 ); bool accepted = false; - while( alpha > 1e-3 && !accepted ) + while( alpha > static_cast( 1e-3 ) && !accepted ) { - std::vector trial_controls( n ); - std::vector trial_states( n ); - double trial_cost = 0.0; + std::vector trial_controls( static_cast( n ) ); + std::vector trial_states( static_cast( n ) ); + Scalar trial_cost = static_cast( 0 ); #pragma omp parallel for reduction( + : trial_cost ) schedule( static ) for( int i = 0; i < n; ++i ) { auto idx = static_cast( i ); auto& ocp = *problem.blocks[idx].agent->ocp; trial_controls[idx] = old_controls[idx] + alpha * ( cand_controls[idx] - old_controls[idx] ); - trial_states[idx] = integrate_horizon( ocp.initial_state, trial_controls[idx], ocp.dt, ocp.dynamics, integrate_rk4 ); + trial_states[idx] = integrate_horizon( ocp.initial_state, trial_controls[idx], ocp.dt, ocp.dynamics, + integrate_rk4 ); trial_cost += ocp.objective_function( trial_states[idx], trial_controls[idx] ); } if( trial_cost < base_cost ) { - for( std::size_t i = 0; i < n; ++i ) + for( std::size_t i = 0; i < static_cast( n ); ++i ) { auto& ocp = *problem.blocks[i].agent->ocp; ocp.best_controls = trial_controls[i]; @@ -155,12 +166,12 @@ run_line_search( int max_outer, const Solver& solver_proto, const SolverParams& } else { - alpha *= 0.5; + alpha *= static_cast( 0.5 ); } } if( !accepted ) { - for( std::size_t i = 0; i < n; ++i ) + for( std::size_t i = 0; i < static_cast( n ); ++i ) { auto& ocp = *problem.blocks[i].agent->ocp; ocp.best_controls = old_controls[i]; @@ -179,11 +190,16 @@ run_line_search( int max_outer, const Solver& solver_proto, const SolverParams& return collect_solution( problem ); } -inline Solution -run_trust_region( int max_outer, const Solver& solver_proto, const SolverParams& params, MultiAgentProblem& problem ) +template +inline SolutionT +run_trust_region( int max_outer, const SolverVariant& solver_proto, const SolverParamsT& params, + MultiAgentProblemT& problem ) { + using ControlTrajectory = ControlTrajectoryT; + using StateTrajectory = StateTrajectoryT; + problem.compute_offsets(); - std::vector solvers; + std::vector> solvers; solvers.reserve( problem.blocks.size() ); for( std::size_t i = 0; i < problem.blocks.size(); ++i ) { @@ -191,7 +207,7 @@ run_trust_region( int max_outer, const Solver& solver_proto, const SolverParams& set_params( solvers.back(), params ); } - std::vector radii( problem.blocks.size(), 1.0 ); + std::vector radii( problem.blocks.size(), static_cast( 1 ) ); for( int outer = 0; outer < max_outer; ++outer ) { @@ -207,21 +223,21 @@ run_trust_region( int max_outer, const Solver& solver_proto, const SolverParams& ControlTrajectory old_u = ocp.best_controls; StateTrajectory old_x = ocp.best_states; - double old_cost = ocp.best_cost; + Scalar old_cost = ocp.best_cost; mas::solve( solvers[idx], ocp ); ControlTrajectory cand_u = ocp.best_controls; StateTrajectory cand_x = ocp.best_states; - double cand_cost = ocp.best_cost; + Scalar cand_cost = ocp.best_cost; ControlTrajectory delta = cand_u - old_u; - double norm = delta.norm(); + Scalar norm = delta.norm(); if( norm > radii[idx] ) { - double scale = radii[idx] / norm; + Scalar scale = radii[idx] / norm; cand_u = old_u + scale * delta; - cand_x = integrate_horizon( ocp.initial_state, cand_u, ocp.dt, ocp.dynamics, integrate_rk4 ); + cand_x = integrate_horizon( ocp.initial_state, cand_u, ocp.dt, ocp.dynamics, integrate_rk4 ); cand_cost = ocp.objective_function( cand_x, cand_u ); } @@ -231,7 +247,7 @@ run_trust_region( int max_outer, const Solver& solver_proto, const SolverParams& ocp.best_states = cand_x; ocp.best_cost = cand_cost; ocp.update_initial_with_best(); - radii[idx] *= 1.5; + radii[idx] *= static_cast( 1.5 ); } else { @@ -239,7 +255,7 @@ run_trust_region( int max_outer, const Solver& solver_proto, const SolverParams& ocp.best_states = old_x; ocp.best_cost = old_cost; ocp.update_initial_with_best(); - radii[idx] *= 0.5; + radii[idx] *= static_cast( 0.5 ); } } } @@ -249,61 +265,86 @@ run_trust_region( int max_outer, const Solver& solver_proto, const SolverParams& } // namespace detail +template struct SequentialNashStrategy { - int max_outer; - Solver solver_proto; - SolverParams params; + using SolverType = SolverVariant; + using ParamsType = SolverParamsT; + using ProblemType = MultiAgentProblemT; + using Solution = SolutionT; + + int max_outer; + SolverType solver_proto; + ParamsType params; - SequentialNashStrategy( int outer, Solver s, SolverParams p ) : + SequentialNashStrategy( int outer, SolverType s, ParamsType p ) : max_outer( outer ), solver_proto( std::move( s ) ), params( std::move( p ) ) {} Solution - operator()( MultiAgentProblem& problem ) + operator()( ProblemType& problem ) { - return detail::run_sequential( max_outer, solver_proto, params, problem ); + return detail::run_sequential( max_outer, solver_proto, params, problem ); } }; +template struct LineSearchNashStrategy { - int max_outer; - Solver solver_proto; - SolverParams params; + using SolverType = SolverVariant; + using ParamsType = SolverParamsT; + using ProblemType = MultiAgentProblemT; + using Solution = SolutionT; - LineSearchNashStrategy( int outer, Solver s, SolverParams p ) : + int max_outer; + SolverType solver_proto; + ParamsType params; + + LineSearchNashStrategy( int outer, SolverType s, ParamsType p ) : max_outer( outer ), solver_proto( std::move( s ) ), params( std::move( p ) ) {} Solution - operator()( MultiAgentProblem& problem ) + operator()( ProblemType& problem ) { - return detail::run_line_search( max_outer, solver_proto, params, problem ); + return detail::run_line_search( max_outer, solver_proto, params, problem ); } }; +template struct TrustRegionNashStrategy { - int max_outer; - Solver solver_proto; - SolverParams params; + using SolverType = SolverVariant; + using ParamsType = SolverParamsT; + using ProblemType = MultiAgentProblemT; + using Solution = SolutionT; + + int max_outer; + SolverType solver_proto; + ParamsType params; - TrustRegionNashStrategy( int outer, Solver s, SolverParams p ) : + TrustRegionNashStrategy( int outer, SolverType s, ParamsType p ) : max_outer( outer ), solver_proto( std::move( s ) ), params( std::move( p ) ) {} Solution - operator()( MultiAgentProblem& problem ) + operator()( ProblemType& problem ) { - return detail::run_trust_region( max_outer, solver_proto, params, problem ); + return detail::run_trust_region( max_outer, solver_proto, params, problem ); } }; +using SequentialNashStrategyd = SequentialNashStrategy; +using SequentialNashStrategyf = SequentialNashStrategy; +using LineSearchNashStrategyd = LineSearchNashStrategy; +using LineSearchNashStrategyf = LineSearchNashStrategy; +using TrustRegionNashStrategyd = TrustRegionNashStrategy; +using TrustRegionNashStrategyf = TrustRegionNashStrategy; + } // namespace mas diff --git a/include/multi_agent_solver/strategies/strategy.hpp b/include/multi_agent_solver/strategies/strategy.hpp index 240b1c5..948256a 100644 --- a/include/multi_agent_solver/strategies/strategy.hpp +++ b/include/multi_agent_solver/strategies/strategy.hpp @@ -10,12 +10,26 @@ namespace mas { -using Strategy = std::variant; +template +using StrategyT + = std::variant, SequentialNashStrategy, LineSearchNashStrategy, + TrustRegionNashStrategy>; + +using Strategy = StrategyT; +using Strategyd = StrategyT; +using Strategyf = StrategyT; + +template +inline SolutionT +solve( StrategyT& strategy, MultiAgentProblemT& problem ) +{ + return std::visit( [&]( auto& s ) { return s( problem ); }, strategy ); +} inline Solution solve( Strategy& strategy, MultiAgentProblem& problem ) { - return std::visit( [&]( auto& s ) { return s( problem ); }, strategy ); + return solve( strategy, problem ); } } // namespace mas diff --git a/include/multi_agent_solver/types.hpp b/include/multi_agent_solver/types.hpp index ac6c53e..9db927b 100644 --- a/include/multi_agent_solver/types.hpp +++ b/include/multi_agent_solver/types.hpp @@ -1,4 +1,5 @@ #pragma once + #include #include // For std::setw #include @@ -11,49 +12,112 @@ namespace mas { // Types defined for clarity in other functions -using State = Eigen::VectorXd; -using StateDerivative = Eigen::VectorXd; -using Control = Eigen::VectorXd; -using ControlTrajectory = Eigen::MatrixXd; -using StateTrajectory = Eigen::MatrixXd; +template +using StateT = Eigen::Matrix; +template +using StateDerivativeT = Eigen::Matrix; +template +using ControlT = Eigen::Matrix; +template +using ControlTrajectoryT = Eigen::Matrix; +template +using StateTrajectoryT = Eigen::Matrix; + +using State = StateT; +using StateDerivative = StateDerivativeT; +using Control = ControlT; +using ControlTrajectory = ControlTrajectoryT; +using StateTrajectory = StateTrajectoryT; // Dynamics -using MotionModel = std::function; +template +using MotionModelT = std::function( const StateT&, const ControlT& )>; +using MotionModel = MotionModelT; // Cost Function -using ObjectiveFunction = std::function; +template +using ObjectiveFunctionT = std::function&, const ControlTrajectoryT& )>; +using ObjectiveFunction = ObjectiveFunctionT; // A stage cost is evaluated on a single (state, control) pair. -using StageCostFunction = std::function; +template +using StageCostFunctionT = std::function&, const ControlT&, size_t time_idx )>; +using StageCostFunction = StageCostFunctionT; // A terminal cost is evaluated on the final state. -using TerminalCostFunction = std::function; - +template +using TerminalCostFunctionT = std::function& )>; +using TerminalCostFunction = TerminalCostFunctionT; // Constraints -using ConstraintViolations = Eigen::VectorXd; -using ConstraintsFunction = std::function; -using ConstraintsJacobian = Eigen::MatrixXd; -using ConstraintsJacobianFunction = std::function; +template +using ConstraintViolationsT = Eigen::Matrix; +template +using ConstraintsFunctionT = std::function( const StateT&, const ControlT& )>; +template +using ConstraintsJacobianT = Eigen::Matrix; +template +using ConstraintsJacobianFunctionT = std::function( const StateT&, const ControlT& )>; + +using ConstraintViolations = ConstraintViolationsT; +using ConstraintsFunction = ConstraintsFunctionT; +using ConstraintsJacobian = ConstraintsJacobianT; +using ConstraintsJacobianFunction = ConstraintsJacobianFunctionT; // Derivative interfaces -using DynamicsStateJacobian = std::function; -using DynamicsControlJacobian = std::function; -using CostStateGradient = std::function; -using CostControlGradient = std::function; -using CostStateHessian = std::function; -using CostControlHessian = std::function; -using CostCrossTerm = std::function; -using TerminalCostGradient = std::function; -using TerminalCostHessian = std::function; -using ControlGradient = Eigen::MatrixXd; +template +using DynamicsStateJacobianT = std::function( + const MotionModelT& dynamics, const StateT&, const ControlT& )>; +template +using DynamicsControlJacobianT = std::function( + const MotionModelT& dynamics, const StateT&, const ControlT& )>; +template +using CostStateGradientT = std::function( const StageCostFunctionT&, const StateT&, + const ControlT&, size_t )>; +template +using CostControlGradientT = std::function< + Eigen::Matrix( const StageCostFunctionT&, const StateT&, const ControlT&, size_t )>; +template +using CostStateHessianT = std::function( + const StageCostFunctionT&, const StateT&, const ControlT&, size_t )>; +template +using CostControlHessianT = std::function( + const StageCostFunctionT&, const StateT&, const ControlT&, size_t )>; +template +using CostCrossTermT = std::function( + const StageCostFunctionT&, const StateT&, const ControlT&, size_t )>; +template +using TerminalCostGradientT + = std::function( const TerminalCostFunctionT&, const StateT& )>; +template +using TerminalCostHessianT + = std::function( const TerminalCostFunctionT&, const StateT& )>; +template +using ControlGradientT = Eigen::Matrix; + +using DynamicsStateJacobian = DynamicsStateJacobianT; +using DynamicsControlJacobian = DynamicsControlJacobianT; +using CostStateGradient = CostStateGradientT; +using CostControlGradient = CostControlGradientT; +using CostStateHessian = CostStateHessianT; +using CostControlHessian = CostControlHessianT; +using CostCrossTerm = CostCrossTermT; +using TerminalCostGradient = TerminalCostGradientT; +using TerminalCostHessian = TerminalCostHessianT; +using ControlGradient = ControlGradientT; // GradientComputer interface -using GradientComputer - = std::function; -using SolverParams = std::unordered_map; +template +using GradientComputerT = std::function< + ControlGradientT( const StateT& initial_state, const ControlTrajectoryT& controls, + const MotionModelT& dynamics, const ObjectiveFunctionT& objective_function, Scalar timestep )>; +using GradientComputer = GradientComputerT; + +template +using SolverParamsT = std::unordered_map; +using SolverParams = SolverParamsT; +using SolverParamsf = SolverParamsT; // ANSI Escape Codes for Colors namespace print_color diff --git a/scripts/compare_solvers.py b/scripts/compare_solvers.py index f846600..cd800e0 100755 --- a/scripts/compare_solvers.py +++ b/scripts/compare_solvers.py @@ -66,6 +66,12 @@ def parse_arguments(argv: Optional[Iterable[str]] = None) -> argparse.Namespace: default=["ilqr", "cgd", "osqp", "osqpcollocation"], help="Solvers to test. Names are passed directly to the executables.", ) + parser.add_argument( + "--scalars", + nargs="+", + default=["float", "double"], + help="Scalar precisions to benchmark. Names are passed directly to the executables.", + ) parser.add_argument( "--strategies", nargs="+", @@ -171,17 +177,18 @@ def fmt_float(value: str, precision: int = 2) -> str: except ValueError: return value - headers = ["solver"] + headers = ["scalar", "solver"] if include_strategy: headers.append("strategy") headers.extend(["cost", "time_ms"]) display_rows: List[List[str]] = [] for row in rows: + scalar = row.get("scalar", "") solver = row.get("solver", "") cost = fmt_float(row.get("cost", "")) time_ms = fmt_float(row.get("time_ms", "")) - values = [solver] + values = [scalar, solver] if include_strategy: values.append(row.get("strategy", "")) values.extend([cost, time_ms]) @@ -230,19 +237,58 @@ def main(argv: Optional[Iterable[str]] = None) -> int: continue if example in MULTI_AGENT_EXAMPLES: - for solver in args.solvers: - for strategy in args.strategies: + for scalar in args.scalars: + for solver in args.solvers: + for strategy in args.strategies: + cmd = [ + str(executable), + f"--solver={solver}", + f"--strategy={strategy}", + f"--agents={args.agents}", + f"--max-outer={args.max_outer}", + f"--scalar={scalar}", + ] + result = run_command(cmd, args.timeout, args.verbose) + if result.returncode != 0: + sys.stderr.write( + f"Error: '{example}' with solver={solver} strategy={strategy} scalar={scalar} exited with code {result.returncode}.\n" + ) + if result.stderr: + sys.stderr.write(result.stderr + "\n") + if args.fail_fast: + return result.returncode or 1 + continue + line = find_result_line(result.stdout) + if not line: + if "unsupported" in result.stdout: + if args.verbose: + print( + f"Skipping {example} solver={solver} strategy={strategy} scalar={scalar}: unsupported" + ) + continue + sys.stderr.write( + f"Error: could not parse output from '{example}' with solver={solver} strategy={strategy} scalar={scalar}.\n" + ) + if args.fail_fast: + return 1 + continue + data = parse_result_line(line) + data.setdefault("scalar", scalar) + data.setdefault("solver", solver) + data.setdefault("strategy", strategy) + results[example].append(data) + else: + for scalar in args.scalars: + for solver in args.solvers: cmd = [ str(executable), f"--solver={solver}", - f"--strategy={strategy}", - f"--agents={args.agents}", - f"--max-outer={args.max_outer}", + f"--scalar={scalar}", ] result = run_command(cmd, args.timeout, args.verbose) if result.returncode != 0: sys.stderr.write( - f"Error: '{example}' with solver={solver} strategy={strategy} exited with code {result.returncode}.\n" + f"Error: '{example}' with solver={solver} scalar={scalar} exited with code {result.returncode}.\n" ) if result.stderr: sys.stderr.write(result.stderr + "\n") @@ -251,40 +297,22 @@ def main(argv: Optional[Iterable[str]] = None) -> int: continue line = find_result_line(result.stdout) if not line: + if "unsupported" in result.stdout: + if args.verbose: + print( + f"Skipping {example} solver={solver} scalar={scalar}: unsupported" + ) + continue sys.stderr.write( - f"Error: could not parse output from '{example}' with solver={solver} strategy={strategy}.\n" + f"Error: could not parse output from '{example}' with solver={solver} scalar={scalar}.\n" ) if args.fail_fast: return 1 continue data = parse_result_line(line) + data.setdefault("scalar", scalar) data.setdefault("solver", solver) - data.setdefault("strategy", strategy) results[example].append(data) - else: - for solver in args.solvers: - cmd = [str(executable), f"--solver={solver}"] - result = run_command(cmd, args.timeout, args.verbose) - if result.returncode != 0: - sys.stderr.write( - f"Error: '{example}' with solver={solver} exited with code {result.returncode}.\n" - ) - if result.stderr: - sys.stderr.write(result.stderr + "\n") - if args.fail_fast: - return result.returncode or 1 - continue - line = find_result_line(result.stdout) - if not line: - sys.stderr.write( - f"Error: could not parse output from '{example}' with solver={solver}.\n" - ) - if args.fail_fast: - return 1 - continue - data = parse_result_line(line) - data.setdefault("solver", solver) - results[example].append(data) for example in args.examples: rows = results.get(example, [])