joint.hpp

Go to the documentation of this file.

#ifndef TR_JOINT_HPP
#define TR_JOINT_HPP
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
 
namespace tinyrobotics {
 
    enum class JointType {
        UNKNOWN,     
        REVOLUTE,    
        CONTINUOUS,  
        PRISMATIC,   
        FLOATING,    
        PLANAR,      
        FIXED        
    };
 
    template <typename Scalar>
    struct Joint {
 
        int joint_id = -1;
 
        int idx = -1;
 
        std::string name = "";
 
        JointType type = JointType::UNKNOWN;
 
        Eigen::Matrix<Scalar, 3, 1> axis = Eigen::Matrix<Scalar, 3, 1>::Zero();
 
        Eigen::Transform<Scalar, 3, Eigen::Isometry> parent_transform =
            Eigen::Transform<Scalar, 3, Eigen::Isometry>::Identity();
 
        Eigen::Transform<Scalar, 3, Eigen::Isometry> child_transform =
            Eigen::Transform<Scalar, 3, Eigen::Isometry>::Identity();
 
        std::string parent_link_name = "";
 
        std::string child_link_name = "";
 
        Eigen::Matrix<Scalar, 6, 6> X = Eigen::Matrix<Scalar, 6, 6>::Identity();
 
        Eigen::Matrix<Scalar, 6, 1> S = Eigen::Matrix<Scalar, 6, 1>::Zero();
 
        std::string get_type() const {
            switch (type) {
                case JointType::UNKNOWN: return "Unknown";
                case JointType::REVOLUTE: return "Revolute";
                case JointType::CONTINUOUS: return "Continuous";
                case JointType::PRISMATIC: return "Prismatic";
                case JointType::FLOATING: return "Floating";
                case JointType::PLANAR: return "Planar";
                case JointType::FIXED: return "Fixed";
                default: return "Invalid";
            }
        }
 
        Eigen::Transform<Scalar, 3, Eigen::Isometry> get_joint_transform(const Scalar& q) const {
            Eigen::Transform<Scalar, 3, Eigen::Isometry> T = Eigen::Transform<Scalar, 3, Eigen::Isometry>::Identity();
            switch (type) {
                case JointType::REVOLUTE: T.linear() = Eigen::AngleAxis<Scalar>(q, axis).toRotationMatrix(); break;
                case JointType::PRISMATIC: T.translation() = q * axis; break;
                case JointType::FIXED: break;
                default: throw std::runtime_error("Joint type not supported.");
            }
            return T;
        }
 
        Eigen::Transform<Scalar, 3, Eigen::Isometry> get_parent_to_child_transform(const Scalar& q) {
            return parent_transform * get_joint_transform(q) * child_transform;
        }
 
        template <typename NewScalar>
        Joint<NewScalar> cast() const {
            Joint<NewScalar> new_joint = Joint<NewScalar>();
            new_joint.joint_id         = joint_id;
            new_joint.idx              = idx;
            new_joint.name             = name;
            new_joint.type             = type;
            new_joint.axis             = axis.template cast<NewScalar>();
            new_joint.parent_transform = parent_transform.template cast<NewScalar>();
            new_joint.child_transform  = child_transform.template cast<NewScalar>();
            new_joint.parent_link_name = parent_link_name;
            new_joint.child_link_name  = child_link_name;
            new_joint.X                = X.template cast<NewScalar>();
            new_joint.S                = S.template cast<NewScalar>();
            return new_joint;
        }
    };
 
 
}  // namespace tinyrobotics
#endif