fsb_inverse_kinematics.h

//
// Created by Kyle Chisholm on 2025-05-16.
//
 
#ifndef FSB_INVERSE_KINEMATICS_H
#define FSB_INVERSE_KINEMATICS_H
 
#include <cstddef>
#include <cstdint>
#include "fsb_body.h"
#include "fsb_body_tree.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 = 100U; 
    Real         objective_tol = 1.0e-12; 
    Real         state_tol = 1.0e-12; 
    Real         damping_factor = 0.001; 
    MotionVector objective_weights = {
        {1.0, 1.0, 1.0},
        {1.0, 1.0, 1.0}
    }; 
};
 
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 = InverseKinematicsInfo::INVALID_INPUT; 
    JointSpacePosition    joint_position = {}; 
    BodyCartesianPva      body_poses = {}; 
    Jacobian              jacobian = {}; 
    Transform             computed_pose = {}; 
    MotionVector          error_pose = {}; 
    size_t                iterations = 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