Model.hpp

Go to the documentation of this file.

#ifndef TR_MODEL_HPP
#define TR_MODEL_HPP
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <map>
#include <memory>
#include <numeric>
#include <sstream>
 
#include "Data.hpp"
#include "Joint.hpp"
#include "Link.hpp"
 
namespace tinyrobotics {
 
    template <typename Scalar, int nq>
    struct Model {
        std::string name = "";
 
        int n_links = 0;
 
        int n_joints = 0;
 
        int n_q = 0;
 
        std::vector<Link<Scalar>> links = {};
 
        std::vector<Joint<Scalar>> joints = {};
 
        int base_link_idx = -1;
 
        std::vector<int> q_idx = {};
 
        std::vector<int> parent = {};
 
        Eigen::Matrix<Scalar, 3, 1> gravity = {0, 0, -9.81};
 
        Scalar mass = 0;
 
        Data<Scalar, nq> data;
 
        Link<Scalar> get_link(const std::string& name) const {
            for (auto link : links) {
                if (link.name == name) {
                    return link;
                }
            }
            // No link was found
            return Link<Scalar>();
        }
 
        Link<Scalar> get_parent_link(const std::string& name) const {
            for (auto link : links) {
                if (link.name == name) {
                    return links[link.parent];
                }
            }
            // No link was found
            throw std::runtime_error("Error! Parent of Link [" + name + "] not found!");
            return Link<Scalar>();
        }
 
        Joint<Scalar> get_joint(const std::string& name) const {
            for (auto link : links) {
                if (link.joint.name == name) {
                    return link.joint;
                }
            }
            // No joint was found, return an empty joint
            return Joint<Scalar>();
        }
 
        void show_details() {
            const int spacing = 25;
            const std::string separator(150, '*');
            std::cout << separator << std::endl;
            std::cout << "Model name             : " << name << std::endl;
            std::cout << "No. of links           : " << links.size() << std::endl;
            std::cout << "No. of joints          : " << joints.size() << std::endl;
            std::cout << "No. of actuated joints : " << n_q << std::endl;
            std::cout << "Base link name         : " << links[base_link_idx].name << std::endl << std::endl;
            std::cout << "Model Mass             : " << mass << std::endl;
            std::cout << std::left << std::setw(spacing) << "Index" << std::setw(spacing) << "Link Name"
                      << std::setw(spacing) << "Joint Name [idx]" << std::setw(spacing) << "Joint Type"
                      << std::setw(spacing) << "Parent Name [idx]" << std::setw(spacing) << "Children Names"
                      << std::endl;
            for (const auto& link : links) {
                std::string children_names;
                for (const auto& child_link_idx : link.child_links) {
                    children_names += links[child_link_idx].name + " ";
                }
                Link<Scalar> parent_link;
                std::string parent_name = "";
                int parent_idx          = -1;
                if (link.parent != -1) {
                    parent_link = links[link.parent];
                    parent_name = parent_link.name;
                    parent_idx  = link.parent;
                }
                std::cout << std::left << std::setw(spacing) << link.idx << std::setw(spacing) << link.name
                          << std::setw(spacing) << link.joint.name + "[" + std::to_string(link.joint.idx) + "]"
                          << std::setw(spacing) << link.joint.get_type() << std::setw(spacing)
                          << parent_name + " [" + std::to_string(parent_idx) + "]" << std::setw(spacing)
                          << children_names << std::endl;
            }
            std::cout << separator << std::endl;
        }
 
        Eigen::Matrix<Scalar, nq, 1> home_configuration() const {
            assert(n_q > 0);
            Eigen::Matrix<Scalar, nq, 1> q(n_q);
            q.setZero();
            return q;
        }
 
        Eigen::Matrix<Scalar, nq, 1> random_configuration() const {
            assert(n_q > 0);
            Eigen::Matrix<Scalar, nq, 1> q(n_q);
            q.setRandom();
            q = M_PI * q;
            return q;
        }
 
        template <typename NewScalar>
        Model<NewScalar, nq> cast() {
            Model<NewScalar, nq> new_model = Model<NewScalar, nq>();
            new_model.name                 = name;
            new_model.n_links              = n_links;
            new_model.n_joints             = n_joints;
            new_model.n_q                  = n_q;
            new_model.base_link_idx        = base_link_idx;
            new_model.gravity              = gravity.template cast<NewScalar>();
            for (auto& joint : joints) {
                new_model.joints.push_back(joint.template cast<NewScalar>());
            }
            for (auto& link : links) {
                new_model.links.push_back(link.template cast<NewScalar>());
            }
            new_model.data = data.template cast<NewScalar>();
            return new_model;
        }
    };
}  // namespace tinyrobotics
 
#endif