robot.hpp

Go to the documentation of this file.

#pragma once
 
#include <array>
#include <cstddef>
#include <functional>
#include <memory>
#include <utility>
#include <vector>
 
#include "copp/core.hpp"
#include "copp/interpolation.hpp"
#include "copp/path.hpp"
 
namespace copp
{
 
    class Robot;
    class ConstraintsRef;

    using InverseDynamics = std::function<void(
        Span<const double> q,
        Span<const double> dq,
        Span<const double> ddq,
        Span<double> tau)>;
 
    namespace detail
    {
        COPP_API const void *robot_handle(const Robot &robot) noexcept;

        COPP_API void *robot_handle_mut(Robot &robot) noexcept;

        COPP_API const void *constraints_handle(ConstraintsRef constraints) noexcept;

        COPP_API void *constraints_handle_mut(ConstraintsRef constraints) noexcept;
    } // namespace detail

    class COPP_API ConstraintsRef
    {
    public:
        ConstraintsRef() noexcept = default;

        bool valid() const noexcept;

        std::size_t dim() const;

        std::size_t len() const;

        std::size_t size() const;

        std::size_t capacity() const;

        bool is_empty() const;

        std::pair<std::size_t, std::size_t> idx_s_range() const;

        ConstraintsRef &append_s(Span<const double> s);

        std::vector<double> s_values(std::size_t idx_s_from, std::size_t idx_s_to) const;

        std::vector<double> amax_values(std::size_t idx_s_from, std::size_t idx_s_to) const;

        double s_value(std::size_t idx_s) const;

        double amax_value(std::size_t idx_s) const;

        ConstraintsRef &amax_substitute(Span<const double> amax, std::size_t idx_s);

        ConstraintsRef &clear(bool keep_idx_s = false);

        ConstraintsRef &pop_front_n(std::size_t n_cols);

        ConstraintsRef &pop_back_n(std::size_t n_cols);

        ConstraintsRef &pop_front_until(std::size_t idx_s_cut);

        ConstraintsRef &pop_back_until(std::size_t idx_s_cut);

        ConstraintsRef &add_constraint_1st(Span<const double> amax, std::size_t idx_s);

        ConstraintsRef &add_constraint_1st(MatrixView amax, std::size_t idx_s);

        ConstraintsRef &add_constraint_2nd(
            MatrixView acc_a,
            MatrixView acc_b,
            MatrixView acc_max,
            std::size_t idx_s,
            bool is_negative = false);

        ConstraintsRef &add_constraint_3rd(
            MatrixView jerk_a,
            MatrixView jerk_b,
            MatrixView jerk_c,
            MatrixView jerk_d,
            MatrixView jerk_max,
            std::size_t idx_s,
            bool is_negative = false);

        std::array<double, 2> exceed_topp2(Span<const double> a, std::size_t idx_s_start = 0) const;

        std::array<double, 3> exceed_topp3(
            Span<const double> a,
            Span<const double> b,
            std::size_t num_stationary_start,
            std::size_t num_stationary_end,
            std::size_t idx_s_start = 0) const;

        std::array<double, 3> exceed_topp3(const Profile3rd &profile, std::size_t idx_s_start = 0) const;
 
    private:
        explicit ConstraintsRef(void *handle) noexcept;
 
        void *handle_ = nullptr;
 
        friend class Robot;
        friend class Constraints;
        friend const void *detail::constraints_handle(ConstraintsRef constraints) noexcept;
        friend void *detail::constraints_handle_mut(ConstraintsRef constraints) noexcept;
    };

    class COPP_API Constraints
    {
    public:
        explicit Constraints(std::size_t dim, std::size_t capacity = 0);
 
        Constraints(Constraints &&) noexcept;
        Constraints &operator=(Constraints &&) noexcept;
        ~Constraints();
 
        Constraints(const Constraints &) = delete;
        Constraints &operator=(const Constraints &) = delete;

        ConstraintsRef ref() noexcept;
 
        std::size_t dim() const;
        std::size_t len() const;
        std::size_t size() const;
        std::size_t capacity() const;
        bool is_empty() const;
        std::pair<std::size_t, std::size_t> idx_s_range() const;
 
        Constraints &append_s(Span<const double> s);
        std::vector<double> s_values(std::size_t idx_s_from, std::size_t idx_s_to) const;
        std::vector<double> amax_values(std::size_t idx_s_from, std::size_t idx_s_to) const;
        double s_value(std::size_t idx_s) const;
        double amax_value(std::size_t idx_s) const;
        Constraints &amax_substitute(Span<const double> amax, std::size_t idx_s);
        Constraints &clear(bool keep_idx_s = false);
        Constraints &pop_front_n(std::size_t n_cols);
        Constraints &pop_back_n(std::size_t n_cols);
        Constraints &pop_front_until(std::size_t idx_s_cut);
        Constraints &pop_back_until(std::size_t idx_s_cut);
        Constraints &add_constraint_1st(Span<const double> amax, std::size_t idx_s);
        Constraints &add_constraint_1st(MatrixView amax, std::size_t idx_s);
        Constraints &add_constraint_2nd(
            MatrixView acc_a,
            MatrixView acc_b,
            MatrixView acc_max,
            std::size_t idx_s,
            bool is_negative = false);
        Constraints &add_constraint_3rd(
            MatrixView jerk_a,
            MatrixView jerk_b,
            MatrixView jerk_c,
            MatrixView jerk_d,
            MatrixView jerk_max,
            std::size_t idx_s,
            bool is_negative = false);

        std::array<double, 2> exceed_topp2(Span<const double> a, std::size_t idx_s_start = 0) const;
        std::array<double, 3> exceed_topp3(
            Span<const double> a,
            Span<const double> b,
            std::size_t num_stationary_start,
            std::size_t num_stationary_end,
            std::size_t idx_s_start = 0) const;
        std::array<double, 3> exceed_topp3(const Profile3rd &profile, std::size_t idx_s_start = 0) const;
 
    private:
        struct Impl;
 
        std::unique_ptr<Impl> impl_;
    };

    class COPP_API Robot
    {
    public:
        explicit Robot(std::size_t dim, std::size_t capacity = 0);
        Robot(std::size_t dim, InverseDynamics inverse_dynamics, std::size_t capacity = 0);
 
        Robot(Robot &&) noexcept;
        Robot &operator=(Robot &&) noexcept;
        ~Robot();
 
        Robot(const Robot &) = delete;
        Robot &operator=(const Robot &) = delete;

        ConstraintsRef constraints() noexcept;
 
        std::size_t dim() const;
        std::size_t len() const;
        std::size_t size() const;
        std::size_t capacity() const;
        bool is_empty() const;
        std::pair<std::size_t, std::size_t> idx_s_range() const;

        bool has_inverse_dynamics() const;

        Robot &set_inverse_dynamics(InverseDynamics inverse_dynamics);

        Robot &clear_inverse_dynamics();

        Robot &append_s(Span<const double> s);

        Robot &set_q_2nd(
            MatrixView q,
            MatrixView dq,
            MatrixView ddq,
            std::size_t idx_s);

        Robot &set_q_3rd(
            MatrixView q,
            MatrixView dq,
            MatrixView ddq,
            MatrixView dddq,
            std::size_t idx_s);

        Robot &set_q_from_path_2nd(const Path &path, std::size_t idx_s_from, std::size_t idx_s_to);

        Robot &set_q_from_path_3rd(const Path &path, std::size_t idx_s_from, std::size_t idx_s_to);

        Robot &add_velocity_limits(
            Span<const double> upper,
            Span<const double> lower,
            std::size_t start_idx_s,
            std::size_t length = 0);

        Robot &add_velocity_limits(MatrixView upper, MatrixView lower, std::size_t start_idx_s);

        Robot &add_acceleration_limits(
            Span<const double> upper,
            Span<const double> lower,
            std::size_t start_idx_s,
            std::size_t length = 0);

        Robot &add_acceleration_limits(MatrixView upper, MatrixView lower, std::size_t start_idx_s);

        Robot &add_jerk_limits(
            Span<const double> upper,
            Span<const double> lower,
            std::size_t start_idx_s,
            std::size_t length = 0);

        Robot &add_jerk_limits(MatrixView upper, MatrixView lower, std::size_t start_idx_s);

        Robot &add_torque_limits(
            Span<const double> upper,
            Span<const double> lower,
            std::size_t start_idx_s,
            std::size_t length = 0);

        Robot &add_torque_limits(MatrixView upper, MatrixView lower, std::size_t start_idx_s);
 
    private:
        struct Impl;
 
        std::unique_ptr<Impl> impl_;
 
        friend const void *detail::robot_handle(const Robot &robot) noexcept;
        friend void *detail::robot_handle_mut(Robot &robot) noexcept;
    };
 
} // namespace copp