fsb_compute_kinematics.h

 
#ifndef FSB_COMPUTE_KINEMATICS_H
#define FSB_COMPUTE_KINEMATICS_H
 
#include "fsb_body.h"
#include "fsb_body_tree.h"
#include "fsb_joint.h"
#include "fsb_kinematics.h"
#include "fsb_jacobian.h"
#include "fsb_motion.h"
 
namespace fsb
{
 
enum class ComputeKinematicsError : uint8_t
{
    SUCCESS = 0,
    INVALID_BODY_TREE = 1
};
 
class ComputeKinematics
{
public:
    ComputeKinematics() = default;
 
    ComputeKinematicsError initialize(const BodyTree& tree);
 
    void compute_forward_kinematics_pose(
        const JointSpacePosition& joint_position, BodyCartesianPva& cartesian) const;
 
    void compute_forward_kinematics(
        ForwardKinematicsOption opt, const JointPva& joint, BodyCartesianPva& cartesian) const;
 
    void compute_forward_kinematics_with_base(
        ForwardKinematicsOption opt, const JointPva& joint, const CartesianPva& base,
        BodyCartesianPva& cartesian) const;
 
    JacobianError compute_jacobian(
        size_t body_index, const BodyCartesianPva& cartesian, Jacobian& jacobian) const;
 
    [[nodiscard]] size_t get_num_bodies() const
    {
        return m_body_tree.get_num_bodies();
    }
 
    [[nodiscard]] size_t get_num_coordinates() const
    {
        return m_body_tree.get_num_coordinates();
    }
 
private:
    BodyTree m_body_tree;
};
 
inline ComputeKinematicsError ComputeKinematics::initialize(const BodyTree& tree)
{
    m_body_tree = tree;
    return ComputeKinematicsError::SUCCESS;
}
 
inline void ComputeKinematics::compute_forward_kinematics_pose(
    const JointSpacePosition& joint_position, BodyCartesianPva& cartesian) const
{
    const JointPva joint = {joint_position, {}, {}};
    compute_forward_kinematics(ForwardKinematicsOption::POSE, joint, cartesian);
}
 
inline void ComputeKinematics::compute_forward_kinematics(
    const ForwardKinematicsOption opt, const JointPva& joint, BodyCartesianPva& cartesian) const
{
    forward_kinematics(m_body_tree, joint, {}, opt, cartesian);
}
 
inline void ComputeKinematics::compute_forward_kinematics_with_base(
    const ForwardKinematicsOption opt, const JointPva& joint, const CartesianPva& base,
    BodyCartesianPva& cartesian) const
{
    forward_kinematics(m_body_tree, joint, base, opt, cartesian);
}
 
inline JacobianError ComputeKinematics::compute_jacobian(
    const size_t body_index, const BodyCartesianPva& cartesian, Jacobian& jacobian) const
{
    return calculate_jacobian(body_index, m_body_tree, cartesian, jacobian);
}
 
} // namespace fsb
 
#endif