fsb_inverse_kinematics.h

//
// Created by Kyle Chisholm on 2025-05-16.
//
 
#ifndef FSB_INVERSE_KINEMATICS_H
#define FSB_INVERSE_KINEMATICS_H
 
#include <array>
#include <cstddef>
#include <cstdint>
#include "fsb_body.h"
#include "fsb_body_tree.h"
#include "fsb_configuration.h"
#include "fsb_motion.h"
#include "fsb_joint.h"
#include "fsb_types.h"
#include "fsb_jacobian.h"
 
namespace fsb
{
 
struct OptimParameters
{
    size_t max_iterations; 
    real_t objective_tol; 
    real_t state_tol; 
    real_t damping_factor; 
    MotionVector objective_weights; 
};
 
enum class InverseKinematicsInfo : uint8_t
{
    INVALID_INPUT = 0,
    SUCCESS = 1, 
    WITHIN_FTOL = 2, 
    WITHIN_XTOL = 3, 
    WITHIN_FTOL_XTOL = 4, 
    MAXIMUM_EVALUATIONS_REACHED = 5, 
    SINGULAR_UPDATE_MATRIX = 6 
};
 
struct InverseKinematicsResult
{
    InverseKinematicsInfo info; 
    JointSpacePosition joint_position; 
    BodyCartesianPva body_poses; 
    Jacobian jacobian; 
    Transform computed_pose; 
    MotionVector error_pose; 
    size_t iterations; 
};
 
inline OptimParameters default_optim_parameters()
{
    return {
        100U, // max iterations
        1.0e-12, // objective tolerance
        1.0e-12, // f tolerance
        0.001,
        {
            {1.0, 1.0, 1.0},
            {1.0, 1.0, 1.0}
        }
    };
}
 
InverseKinematicsResult compute_inverse_kinematics(
    const BodyTree& body_tree, const OptimParameters& params, const JointSpacePosition& initial_config, size_t body_index,
    const Transform& target_pose, const Transform& base_pose = transform_identity());
 
}
 
#endif //FSB_INVERSE_KINEMATICS_H