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,
    NOT_REVOLUTE_OR_PRISMATIC
};
 
struct Jacobian
{
    std::array<Real, 6U * MaxSize::kDofs> j;
};
 
struct JointMatrix
{
    std::array<Real, MaxSize::kDofs * MaxSize::kDofs> j;
};
 
struct Hessian
{
    std::array<Jacobian, MaxSize::kDofs> h;
};
 
struct SingularityEllipsoid
{
    Vec3 eig_values;
    Mat3 eig_vectors;
};
 
struct Mat3Singularity
{
    bool                 is_singular = false;
    Real                 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::kDofs);
 
JointSpace jacobian_transpose_multiply(
    const Jacobian& jacobian, const MotionVector& cartesian_motion, size_t dofs = MaxSize::kDofs);
 
JointMatrix jacobian_transpose_multiply_jacobian(
    const Jacobian& jacobian, const MotionVector& cartesian_weights, size_t dofs = MaxSize::kDofs);
 
JacobianError calculate_jacobian(
    size_t body_index, const BodyTree& body_tree, const BodyCartesianPva& cartesian_pva,
    Jacobian& jacobian);
 
JacobianError jacobian_derivative(
    size_t body_index, const BodyTree& body_tree, const Jacobian& jacobian,
    const JointSpace& joint_velocity, Jacobian& jacobian_deriv);
 
Jacobian jacobian_derivative_from_hessian(
    const Hessian& hessian, const JointSpace& joint_velocity, size_t dofs);
 
JacobianError calculate_hessian(
    size_t body_index, const BodyTree& body_tree, const Jacobian& jacobian, Hessian& hessian);
 
Jacobian hessian_multiply(const Hessian& hessian, const JointSpace& joint_motion, size_t dofs);
 
JacobianMetrics calculate_jacobian_metrics(const Jacobian& jacobian, size_t dofs = MaxSize::kDofs);
 
} // namespace fsb
 
#endif // FSB_JACOBIAN_H