link.hpp

Go to the documentation of this file.

#ifndef TR_LINK_HPP
#define TR_LINK_HPP
 
#include "joint.hpp"
 
namespace tinyrobotics {
 
    template <typename Scalar>
    struct Link {
 
        std::string name = "";
 
        int idx = -1;
 
        int parent = -1;
 
        std::vector<int> child_links = {};
 
        Eigen::Transform<Scalar, 3, Eigen::Isometry> center_of_mass =
            Eigen::Transform<Scalar, 3, Eigen::Isometry>::Identity();
 
        Eigen::Matrix<Scalar, 3, 3> inertia = Eigen::Matrix<Scalar, 3, 3>::Zero();
 
        Scalar mass = 0;
 
        Joint<Scalar> joint;
 
        // @brief Spatial inertia matrix of the link.
        Eigen::Matrix<Scalar, 6, 6> I = {};
 
        void add_child_link_idx(const int child_link_idx) {
            child_links.push_back(child_link_idx);
        }
 
        template <typename NewScalar>
        Link<NewScalar> cast() {
            Link<NewScalar> new_link = Link<NewScalar>();
            new_link.name            = name;
            new_link.idx             = idx;
            new_link.parent          = parent;
            new_link.child_links     = child_links;
            new_link.center_of_mass  = center_of_mass.template cast<NewScalar>();
            new_link.inertia         = inertia.template cast<NewScalar>();
            new_link.mass            = NewScalar(mass);
            new_link.joint           = joint.template cast<NewScalar>();
            new_link.I               = I.template cast<NewScalar>();
            return new_link;
        }
    };
}  // namespace tinyrobotics
#endif