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"
#include "fsb_linalg.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());
 
FsbLinalgErrorType inverse_velocity_kinematics(
    const Jacobian& jacobian, const MotionVector& cart_velocity, size_t dofs,
    JointSpace& joint_velocity);
 
FsbLinalgErrorType inverse_acceleration_kinematics(
    const Jacobian& jacobian, const Jacobian& jacobian_derivative,
    const MotionVector& cart_acceleration, const JointSpace& joint_velocity, size_t dofs,
    JointSpace& joint_acceleration);
 
} // namespace fsb
 
#endif // FSB_INVERSE_KINEMATICS_H