Template examples for scalar-aware solver comparisons

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
 

examples/example_utils.hpp

@@ -5,13 +5,15 @@
 #include <ostream>
 #include <stdexcept>
 #include <string>
+#include <type_traits>
 #include <utility>
 #include <vector>
 
 #include "Eigen/Dense"
 
 #include "multi_agent_solver/solvers/solver.hpp"
 #include "multi_agent_solver/strategies/strategy.hpp"
+#include "multi_agent_solver/types.hpp"
 
 namespace examples
 {
@@ -63,65 +65,139 @@ 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<typename Scalar>
+inline std::string
+scalar_label()
+{
+  if constexpr( std::is_same_v<Scalar, float> )
+    return "float";
+  if constexpr( std::is_same_v<Scalar, double> )
+    return "double";
+  return "unknown";
+}
+
+template<typename Scalar>
 inline std::vector<std::string>
 available_solver_names()
 {
   std::vector<std::string> names{ "ilqr", "cgd" };
 #ifdef MAS_HAVE_OSQP
-  names.push_back( "osqp" );
-  names.push_back( "osqp_collocation" );
+  if constexpr( std::is_same_v<Scalar, double> )
+  {
+    names.push_back( "osqp" );
+    names.push_back( "osqp_collocation" );
+  }
 #endif
   return names;
 }
 
-inline mas::Solver
+inline std::vector<std::string>
+available_solver_names()
+{
+  return available_solver_names<double>();
+}
+
+template<typename Scalar>
+inline bool
+solver_supported_for_scalar( const std::string& canonical )
+{
+#ifdef MAS_HAVE_OSQP
+  if constexpr( !std::is_same_v<Scalar, double> )
+  {
+    if( canonical == "osqp" || canonical == "osqp_collocation" )
+      return false;
+  }
+#else
+  if( canonical == "osqp" || canonical == "osqp_collocation" )
+    return false;
+#endif
+  return true;
+}
+
+template<typename Scalar>
+inline mas::SolverVariant<Scalar>
 make_solver( const std::string& name )
 {
   const std::string canonical = canonical_solver_name( name );
   if( canonical == "ilqr" )
-    return mas::Solver{ std::in_place_type<mas::iLQR> };
+    return mas::SolverVariant<Scalar>{ std::in_place_type<mas::iLQR<Scalar>> };
   if( canonical == "cgd" )
-    return mas::Solver{ std::in_place_type<mas::CGD> };
+    return mas::SolverVariant<Scalar>{ std::in_place_type<mas::CGD<Scalar>> };
 #ifdef MAS_HAVE_OSQP
   if( canonical == "osqp" )
-    return mas::Solver{ std::in_place_type<mas::OSQP> };
+  {
+    if constexpr( std::is_same_v<Scalar, double> )
+      return mas::SolverVariant<Scalar>{ std::in_place_type<mas::OSQP<Scalar>> };
+  }
   if( canonical == "osqp_collocation" )
-    return mas::Solver{ std::in_place_type<mas::OSQPCollocation> };
+  {
+    if constexpr( std::is_same_v<Scalar, double> )
+      return mas::SolverVariant<Scalar>{ std::in_place_type<mas::OSQPCollocation<Scalar>> };
+  }
 #endif
+  if( !solver_supported_for_scalar<Scalar>( canonical ) )
+    throw std::invalid_argument( "Solver '" + name + "' is not available for scalar type '" + scalar_label<Scalar>() + "'." );
   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<double>( name );
+}
+
+template<typename Scalar>
+inline mas::StrategyT<Scalar>
+make_strategy( const std::string& name, mas::SolverVariant<Scalar> solver, const mas::SolverParamsT<Scalar>& 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<Scalar>{ mas::CentralizedStrategy<Scalar>{ std::move( solver ) } };
   }
   if( canonical == "sequential" )
-    return mas::Strategy{ mas::SequentialNashStrategy{ max_outer, std::move( solver ), params } };
+    return mas::StrategyT<Scalar>{ mas::SequentialNashStrategy<Scalar>{ max_outer, std::move( solver ), params } };
   if( canonical == "linesearch" )
-    return mas::Strategy{ mas::LineSearchNashStrategy{ max_outer, std::move( solver ), params } };
+    return mas::StrategyT<Scalar>{ mas::LineSearchNashStrategy<Scalar>{ max_outer, std::move( solver ), params } };
   if( canonical == "trustregion" )
-    return mas::Strategy{ mas::TrustRegionNashStrategy{ max_outer, std::move( solver ), params } };
+    return mas::StrategyT<Scalar>{ mas::TrustRegionNashStrategy<Scalar>{ 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<double>( name, std::move( solver ), params, max_outer );
+}
+
 inline void
 print_available( std::ostream& os )
 {
   const auto solvers = available_solver_names();
   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<typename Scalar>
 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<Scalar>& 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<double>( col ) * dt : static_cast<double>( col );
+    const double time_value
+      = dt > static_cast<Scalar>( 0 ) ? static_cast<double>( col ) * static_cast<double>( dt ) : static_cast<double>( col );
     os << time_value;
     for( int row = 0; row < states.rows(); ++row )
-      os << ',' << states( row, col );
+      os << ',' << static_cast<double>( states( row, col ) );
     os << '\n';
   }
   os << '\n';
 }
 
+template<typename Scalar>
 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<Scalar>& 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<double>( col ) * dt : static_cast<double>( col );
+    const double time_value
+      = dt > static_cast<Scalar>( 0 ) ? static_cast<double>( col ) * static_cast<double>( dt ) : static_cast<double>( col );
     os << time_value;
     for( int row = 0; row < controls.rows(); ++row )
-      os << ',' << controls( row, col );
+      os << ',' << static_cast<double>( 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<double>( 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<double>( os, controls, dt, label );
+}
+
 } // namespace examples

examples/models/pendulum_model.hpp

@@ -1,40 +1,66 @@
 #pragma once
+
+#include <cmath>
+
 #include <Eigen/Dense>
 
 #include "multi_agent_solver/types.hpp"
 
 namespace mas
 {
-inline StateDerivative
-pendulum_dynamics( const State& x, const Control& u )
+
+template<typename Scalar>
+inline StateDerivativeT<Scalar>
+pendulum_dynamics( const StateT<Scalar>& x, const ControlT<Scalar>& 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<Scalar>( 9.81 ); // [m/s^2]
+  const Scalar          l    = static_cast<Scalar>( 1.0 );  // [m]
+  const Scalar          m    = static_cast<Scalar>( 1.0 );  // [kg]
+  StateDerivativeT<Scalar> 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<double>( x, u );
+}
+
+template<typename Scalar>
+inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
+pendulum_state_jacobian( const StateT<Scalar>& x, const ControlT<Scalar>& )
+{
+  const Scalar g = static_cast<Scalar>( 9.81 );
+  const Scalar l = static_cast<Scalar>( 1.0 );
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> A = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero( 2, 2 );
+  A( 0, 1 ) = static_cast<Scalar>( 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<double>( x, u );
+}
+
+template<typename Scalar>
+inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
+pendulum_control_jacobian( const StateT<Scalar>&, const ControlT<Scalar>& )
+{
+  const Scalar m = static_cast<Scalar>( 1.0 );
+  const Scalar l = static_cast<Scalar>( 1.0 );
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> B = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero( 2, 1 );
+  B( 1, 0 ) = static_cast<Scalar>( 1.0 ) / ( m * l * l );
   return B;
 }
+
+inline Eigen::MatrixXd
+pendulum_control_jacobian( const State& x, const Control& u )
+{
+  return pendulum_control_jacobian<double>( x, u );
+}
+
 } // namespace mas