fsb_jacobian.h

 
#ifndef FSB_JACOBIAN_H
#define FSB_JACOBIAN_H
 
#include <array>
#include <cstddef>
#include <cstdint>
#include "fsb_body.h"
#include "fsb_body_tree.h"
#include "fsb_configuration.h"
#include "fsb_joint.h"
#include "fsb_motion.h"
#include "fsb_types.h"
 
namespace fsb
{
 
enum class JacobianError : uint8_t
{
    SUCCESS,
    BODY_NOT_IN_TREE
};
 
struct Jacobian
{
    std::array<real_t, 6U * MaxSize::dofs> j;
};
 
struct JointMatrix
{
    std::array<real_t, MaxSize::dofs * MaxSize::dofs> j;
};
 
struct Hessian
{
    std::array<Jacobian, MaxSize::dofs> h;
};
 
struct SingularityEllipsoid
{
    Vec3 eig_values;
    Mat3 eig_vectors;
};
 
struct Mat3Singularity
{
    bool is_singular = false;
    real_t condition_number = 0.0;
    SingularityEllipsoid ellipsoid = {};
};
 
struct JacobianMetrics
{
    Mat3Singularity angular;
    Mat3Singularity linear;
};
 
inline size_t jacobian_index(const size_t row, const size_t col)
{
    return 6U * col + row;
}
 
inline size_t joint_matrix_index(const size_t row, const size_t col, const size_t dofs)
{
    return dofs * col + row;
}
 
MotionVector jacobian_multiply(
    const Jacobian& jacobian, const JointSpace& joint_motion, size_t dofs = MaxSize::dofs);
 
JointSpace jacobian_transpose_multiply(
    const Jacobian& jacobian, const MotionVector& cartesian_motion, size_t dofs = MaxSize::dofs);
 
JointMatrix jacobian_transpose_multiply_jacobian(const Jacobian& jacobian, const MotionVector& cartesian_weights, size_t dofs = MaxSize::dofs);
 
Jacobian jacobian_derivative(
    const Hessian& hessian, const JointSpace& joint_velocity, size_t dofs = MaxSize::dofs);
 
JacobianError calculate_jacobian(
    size_t body_index, const BodyTree& body_tree, const BodyCartesianPva& cartesian_pva,
    Jacobian& jacobian);
 
JacobianMetrics calculate_jacobian_metrics(const Jacobian& jacobian, size_t dofs= MaxSize::dofs);
 
} // namespace fsb
 
#endif // FSB_JACOBIAN_H