fsb_body_tree.h

 
#ifndef FSB_BODY_TREE_H
#define FSB_BODY_TREE_H
 
#include <array>
#include <cstdint>
#include "fsb_configuration.h"
#include "fsb_body.h"
#include "fsb_joint.h"
#include "fsb_motion.h"
#include "fsb_types.h"
 
namespace fsb
{
 
enum class BodyTreeError : uint8_t
{
    SUCCESS = 0,
    BODY_NOT_IN_TREE = 1,
    JOINT_NOT_IN_TREE = 2,
    MAX_BODIES_REACHED = 3,
    PARENT_BODY_NONEXISTENT = 4,
    MAX_JOINTS_REACHED = 5,
    MAX_JOINT_COORDINATES_REACHED = 6,
    MAX_JOINT_DOFS_REACHED = 7,
    MASS_ZERO = 8,
    INERTIA_ZERO = 9,
    MASS_ZERO_WITH_INERTIA = 10,
    INERTIA_NOT_POS_DEF = 11,
    UNSUPPORTED_JOINT_TYPE = 12
};
 
struct JointLimits
{
    std::array<bool, MaxSize::kDofs> set; 
    std::array<Real, MaxSize::kDofs> lower_position; 
    std::array<Real, MaxSize::kDofs> upper_position; 
    std::array<Real, MaxSize::kDofs> max_velocity; 
};
 
class BodyTree
{
public:
    BodyTree() = default;
 
    static constexpr size_t kBaseIndex = 0U;
 
    size_t add_body(
        size_t parent_body_index, JointType joint_type, const Transform& parent_joint_transform,
        const Body& body, BodyTreeError& err);
 
    size_t add_massless_body(
        size_t parent_body_index, JointType joint_type, const Transform& parent_joint_transform,
        const MotionVector& origin_offset, BodyTreeError& err);
 
    BodyTreeError
    set_joint_position_limit(size_t joint_index, Real lower_position, Real upper_position);
 
    BodyTreeError unset_joint_position_limit(size_t joint_index);
 
    BodyTreeError get_joint_position_limit(
        size_t joint_index, bool& is_set, Real& lower_position, Real& upper_position) const;
 
    BodyTreeError set_joint_velocity_limit(size_t joint_index, Real max_velocity);
 
    BodyTreeError get_joint_velocity_limit(size_t joint_index, Real& max_velocity) const;
 
    BodyTreeError set_joint_reversed(size_t joint_index, bool reversed);
 
    void set_gravity(const Vec3& gravity)
    {
        m_gravity = gravity;
    }
 
    Joint get_joint(size_t joint_index, BodyTreeError& err) const;
 
    Body get_body(size_t body_index, BodyTreeError& err) const;
 
    BodyTreeError set_body_mass_props(size_t body_index, const MassProps& mass_props);
 
    BodyTreeError set_body_origin_offset(size_t body_index, const MotionVector& origin_offset);
 
    BodyTreeError
    set_parent_joint_transform(size_t body_index, const Transform& parent_joint_transform);
 
    size_t get_body_dofs(size_t body_index, BodyTreeError& err) const;
 
    [[nodiscard]] size_t get_num_bodies() const
    {
        return m_num_bodies;
    }
 
    [[nodiscard]] size_t get_num_dofs() const
    {
        return m_num_dofs;
    }
 
    [[nodiscard]] size_t get_num_coordinates() const
    {
        return m_num_coordinates;
    }
 
    [[nodiscard]] Vec3 get_gravity() const
    {
        return m_gravity;
    }
 
    [[nodiscard]] std::array<size_t, MaxSize::kBodies> get_leaves(size_t& num_leaves) const
    {
        std::array<size_t, MaxSize::kBodies> result = {};
        num_leaves = 0U;
        for (size_t index = 0; index < m_num_bodies; ++index)
        {
            if (m_bodies[index].is_leaf)
            {
                result[num_leaves] = index;
                num_leaves++;
            }
        }
        return result;
    }
 
private:
    [[nodiscard]] BodyTreeError validate_massless_body_input(size_t parent_body_index) const;
    [[nodiscard]] BodyTreeError
    validate_add_body_input(size_t parent_body_index, JointType joint_type, const Body& body) const;
 
    std::array<Body, MaxSize::kBodies>  m_bodies = {};
    std::array<Joint, MaxSize::kJoints> m_joints = {};
    JointLimits                         m_limits = {};
    Vec3                                m_gravity = {};
 
    size_t m_num_coordinates = 0U;
    size_t m_num_dofs = 0U;
    size_t m_num_bodies = 1U;
    size_t m_num_joints = 0U;
};
 
// inline methods
 
inline Joint BodyTree::get_joint(const size_t joint_index, BodyTreeError& err) const
{
    Joint joint = {};
    if (joint_index < m_num_joints)
    {
        joint = m_joints[joint_index];
        err = BodyTreeError::SUCCESS;
    }
    else
    {
        err = BodyTreeError::JOINT_NOT_IN_TREE;
    }
    return joint;
}
 
inline Body BodyTree::get_body(const size_t body_index, BodyTreeError& err) const
{
    Body body = {};
    if (body_index < m_num_bodies)
    {
        body = m_bodies[body_index];
        err = BodyTreeError::SUCCESS;
    }
    else
    {
        err = BodyTreeError::BODY_NOT_IN_TREE;
    }
    return body;
}
 
inline BodyTreeError
BodyTree::set_body_mass_props(const size_t body_index, const MassProps& mass_props)
{
    auto err = BodyTreeError::SUCCESS;
    if (body_index < m_num_bodies)
    {
        m_bodies[body_index].mass_props = mass_props;
        err = BodyTreeError::SUCCESS;
    }
    else
    {
        err = BodyTreeError::BODY_NOT_IN_TREE;
    }
    return err;
}
 
inline BodyTreeError
BodyTree::set_body_origin_offset(const size_t body_index, const MotionVector& origin_offset)
{
    auto err = BodyTreeError::SUCCESS;
    if (body_index < m_num_bodies)
    {
        m_bodies[body_index].origin_offset = origin_offset;
        err = BodyTreeError::SUCCESS;
    }
    else
    {
        err = BodyTreeError::BODY_NOT_IN_TREE;
    }
    return err;
}
 
inline BodyTreeError BodyTree::set_parent_joint_transform(
    const size_t body_index, const Transform& parent_joint_transform)
{
    auto err = BodyTreeError::SUCCESS;
    if (body_index < m_num_bodies)
    {
        const size_t joint_index = m_bodies[body_index].joint_index;
        m_joints[joint_index].parent_joint_transform = parent_joint_transform;
        err = BodyTreeError::SUCCESS;
    }
    else
    {
        err = BodyTreeError::BODY_NOT_IN_TREE;
    }
    return err;
}
 
} // namespace fsb
 
#endif