topp3.hpp

Go to the documentation of this file.

#pragma once
 
#include <cstddef>
#include <cstdint>
#include <optional>
#include <utility>
#include <vector>
 
#include "copp/clarabel.hpp"
#include "copp/core.hpp"
#include "copp/interpolation.hpp"
#include "copp/robot.hpp"
 
namespace copp::solver::topp3
{

    struct StationaryBounds
    {
        std::size_t start = 1;
        std::size_t end = 1;
 
        static constexpr StationaryBounds both(std::size_t value) noexcept
        {
            return StationaryBounds{value, value};
        }
    };

    class COPP_API Problem
    {
    public:
        Problem(
            ConstraintsRef constraints,
            Span<const double> a_linearization,
            std::size_t idx_s_start = 0,
            Boundary3 boundary = {},
            StationaryBounds num_stationary_max = {},
            double a_linearization_floor = 1.0e-10,
            Span<const double> b_linearization = {});
 
        Problem(
            ConstraintsRef constraints,
            Span<const double> a_linearization,
            std::size_t idx_s_start,
            Boundary3 boundary,
            std::size_t symmetric_num_stationary_max,
            double a_linearization_floor = 1.0e-10,
            Span<const double> b_linearization = {});
 
        ConstraintsRef constraints() const noexcept { return constraints_; }
        const std::vector<double> &a_linearization() const noexcept { return a_linearization_; }
        const std::vector<double> &b_linearization() const noexcept { return b_linearization_; }
        std::size_t idx_s_start() const noexcept { return idx_s_start_; }
        std::size_t idx_s_final() const noexcept { return idx_s_final_; }
        Boundary3 boundary() const noexcept { return boundary_; }
        StationaryBounds num_stationary_max() const noexcept { return num_stationary_max_; }
        StationaryBounds num_stationary() const noexcept { return num_stationary_; }
        double a_linearization_floor() const noexcept { return a_linearization_floor_; }
        std::size_t s_len() const noexcept { return a_linearization_.size(); }
 
    private:
        ConstraintsRef constraints_;
        std::vector<double> a_linearization_;
        std::vector<double> b_linearization_;
        std::size_t idx_s_start_ = 0;
        std::size_t idx_s_final_ = 0;
        Boundary3 boundary_;
        StationaryBounds num_stationary_max_;
        StationaryBounds num_stationary_;
        double a_linearization_floor_ = 1.0e-10;
    };

    struct Result
    {
        std::optional<Profile3rd> profile;
        std::vector<double> x;
        std::vector<double> z;
        std::vector<double> s;
        clarabel::SolverStatus solver_status = clarabel::SolverStatus::Unsolved;
        double obj_val = 0.0;
        double obj_val_dual = 0.0;
        double solve_time = 0.0;
        std::uint32_t iterations = 0;
        double r_prim = 0.0;
        double r_dual = 0.0;
        clarabel::LinearSolverInfo linsolver;
        std::optional<double> objective_value;
        std::vector<double> objective_terms;
 
        bool has_profile() const noexcept { return profile.has_value(); }
    };
 
} // namespace copp::solver::topp3