inverse_kinematics.h

Go to the documentation of this file.

#pragma once
 
#include <libopynsim/model_state.h>
#include <libopynsim/symbol.h>
 
#include <liboscar/maths/quaternion.h>
#include <liboscar/maths/vector.h>
#include <liboscar/utilities/assertions.h>
 
#include <concepts>
#include <cstddef>
#include <utility>
#include <vector>
 
namespace opyn
{
    class IKResult final {
    public:
        size_t num_states() const;
 
        const ModelState& model_state(size_t i) const;
 
    private:
        std::vector<ModelState> model_states_;
    };
 
    template<typename T>
    class IKMeasurements final {
    public:
 
        template<typename U>
        requires std::constructible_from<T, U>
        explicit IKMeasurements(
            const Symbol& model_component,
            U&& value,
            double time = 0.0,
            double weight = 1.0) :
 
            model_component_{model_component},
            times_{time},
            values_{std::forward<U>(value)},
            weights_{weight}
        {}
 
        explicit IKMeasurements(
            const Symbol& model_component,
            std::vector<double> times,
            std::vector<T> values) :
 
            model_component_{model_component},
            times_{std::move(times)},
            values_{std::move(values)}
        {
            OSC_ASSERT_ALWAYS(times_.size() == values_.size() && "The number of times/weights/values in an IK measurement sequence must be equal");
            weights_.resize(times_.size(), 1.0);
        }
 
        explicit IKMeasurements(
            const Symbol& model_component,
            std::vector<double> times,
            std::vector<T> values,
            std::vector<double> weights) :
 
            model_component_{model_component},
            times_{std::move(times)},
            values_{std::move(values)},
            weights_{std::move(weights)}
        {
            OSC_ASSERT_ALWAYS(times_.size() == values_.size() && values_.size() == weights_.size() && "The number of times/weights/values in an IK measurement sequence must be equal");
        }
 
        const Symbol& model_component() const { return model_component_; }
 
        size_t size() const { return times_.size(); }
 
        double time(size_t i) const { return times_.at(i); }
 
        const T& value(size_t i) const { return values_.at(i); }
 
        double weight(size_t i) const { return weights_.at(i); }
 
    private:
        Symbol model_component_;
        std::vector<double> times_;
        std::vector<T> values_;
        std::vector<double> weights_;
    };
 
    using IKStationMeasurements = IKMeasurements<osc::Vector3d>;
 
    using IKOrientationMeasurements = IKMeasurements<osc::Quaterniond>;
 
    using IKCoordinateMeasurements = IKMeasurements<double>;
 
    IKResult inverse_kinematics_solve(
        const Model& model,
        const std::optional<IKStationMeasurements>& station_measurements = {},
        const std::optional<IKOrientationMeasurements>& orientation_measurements = {},
        const std::optional<IKCoordinateMeasurements>& coordinate_measurements = {}
    );
}