data.hpp

Go to the documentation of this file.

#ifndef TR_DATA_HPP
#define TR_DATA_HPP
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
namespace tinyrobotics {
 
    template <typename Scalar, int nq>
    struct Data {
 
        Eigen::Matrix<Scalar, nq, 1> q;
 
        Eigen::Matrix<Scalar, nq, 1> dq;
 
        Eigen::Matrix<Scalar, nq, 1> ddq;
 
        Eigen::Matrix<Scalar, nq, 1> tau;
 
        Eigen::Matrix<Scalar, nq, nq> mass_matrix = Eigen::Matrix<Scalar, nq, nq>::Zero();
 
        Eigen::Matrix<Scalar, nq, nq> inv_mass_matrix = Eigen::Matrix<Scalar, nq, nq>::Zero();
 
        Eigen::Matrix<Scalar, nq, nq> coriolis = Eigen::Matrix<Scalar, nq, nq>::Zero();
 
        Eigen::Matrix<Scalar, nq, 1> gravity;
 
        Eigen::Matrix<Scalar, nq, nq> input_mapping = Eigen::Matrix<Scalar, nq, nq>::Identity();
 
        Eigen::Matrix<Scalar, nq, nq> damping = Eigen::Matrix<Scalar, nq, nq>::Zero();
 
        Scalar kinetic_energy = 0;
 
        Scalar potential_energy = 0;
 
        Scalar total_energy = 0;
 
        std::vector<Eigen::Transform<Scalar, 3, Eigen::Isometry>> forward_kinematics = {};
 
        std::vector<Eigen::Transform<Scalar, 3, Eigen::Isometry>> forward_kinematics_com = {};
 
        std::vector<Eigen::Matrix<Scalar, 6, 6>> Xup =
            std::vector<Eigen::Matrix<Scalar, 6, 6>>(nq, Eigen::Matrix<Scalar, 6, 6>::Zero());
 
        std::vector<Eigen::Matrix<Scalar, 6, 1>> S =
            std::vector<Eigen::Matrix<Scalar, 6, 1>>(nq, Eigen::Matrix<Scalar, 6, 1>::Zero());
 
        std::vector<Eigen::Matrix<Scalar, 6, 1>> v =
            std::vector<Eigen::Matrix<Scalar, 6, 1>>(nq, Eigen::Matrix<Scalar, 6, 1>::Zero());
 
        std::vector<Eigen::Matrix<Scalar, 6, 1>> c =
            std::vector<Eigen::Matrix<Scalar, 6, 1>>(nq, Eigen::Matrix<Scalar, 6, 1>::Zero());
 
        std::vector<Eigen::Matrix<Scalar, 6, 6>> IA =
            std::vector<Eigen::Matrix<Scalar, 6, 6>>(nq, Eigen::Matrix<Scalar, 6, 6>::Zero());
 
        std::vector<Eigen::Matrix<Scalar, 6, 1>> pA =
            std::vector<Eigen::Matrix<Scalar, 6, 1>>(nq, Eigen::Matrix<Scalar, 6, 1>::Zero());
 
        std::vector<Eigen::Matrix<Scalar, 6, 1>> U =
            std::vector<Eigen::Matrix<Scalar, 6, 1>>(nq, Eigen::Matrix<Scalar, 6, 1>::Zero());
 
        std::vector<Scalar> d = std::vector<Scalar>(nq, 0);
 
        std::vector<Scalar> u = std::vector<Scalar>(nq, 0);
 
        std::vector<Eigen::Matrix<Scalar, 6, 1>> a =
            std::vector<Eigen::Matrix<Scalar, 6, 1>>(nq, Eigen::Matrix<Scalar, 6, 1>::Zero());
 
        Eigen::Transform<Scalar, 3, Eigen::Isometry> T = Eigen::Transform<Scalar, 3, Eigen::Isometry>::Identity();
 
        std::vector<Eigen::Matrix<Scalar, 6, 1>> avp =
            std::vector<Eigen::Matrix<Scalar, 6, 1>>(nq, Eigen::Matrix<Scalar, 6, 1>::Zero());
 
        std::vector<Eigen::Matrix<Scalar, 6, 1>> fvp =
            std::vector<Eigen::Matrix<Scalar, 6, 1>>(nq, Eigen::Matrix<Scalar, 6, 1>::Zero());
 
        Eigen::Matrix<Scalar, nq, 1> C = Eigen::Matrix<Scalar, nq, 1>::Zero();
 
        Eigen::Matrix<Scalar, 6, 1> fh = Eigen::Matrix<Scalar, 6, 1>::Zero();
 
        std::vector<Eigen::Matrix<Scalar, 6, 6>> IC =
            std::vector<Eigen::Matrix<Scalar, 6, 6>>(nq, Eigen::Matrix<Scalar, 6, 6>::Zero());
 
        Eigen::Matrix<Scalar, 6, 1> spatial_gravity = Eigen::Matrix<Scalar, 6, 1>::Zero();
 
        Eigen::Matrix<Scalar, 6, nq> J = Eigen::Matrix<Scalar, 6, nq>::Zero();
 
        template <typename NewScalar>
        Data<NewScalar, nq> cast() {
            Data<NewScalar, nq> new_res;
            new_res.q                = q.template cast<NewScalar>();
            new_res.dq               = dq.template cast<NewScalar>();
            new_res.ddq              = ddq.template cast<NewScalar>();
            new_res.tau              = tau.template cast<NewScalar>();
            new_res.mass_matrix      = mass_matrix.template cast<NewScalar>();
            new_res.inv_mass_matrix  = inv_mass_matrix.template cast<NewScalar>();
            new_res.coriolis         = coriolis.template cast<NewScalar>();
            new_res.gravity          = gravity.template cast<NewScalar>();
            new_res.input_mapping    = input_mapping.template cast<NewScalar>();
            new_res.damping          = damping.template cast<NewScalar>();
            new_res.kinetic_energy   = NewScalar(kinetic_energy);
            new_res.potential_energy = NewScalar(potential_energy);
            new_res.total_energy     = NewScalar(total_energy);
            new_res.C                = C.template cast<NewScalar>();
            new_res.fh               = fh.template cast<NewScalar>();
            new_res.spatial_gravity  = spatial_gravity.template cast<NewScalar>();
            new_res.J                = J.template cast<NewScalar>();
            new_res.forward_kinematics.resize(forward_kinematics.size());
            for (int i = 0; i < forward_kinematics.size(); i++) {
 
                new_res.forward_kinematics[i] = forward_kinematics[i].template cast<NewScalar>();
            }
            for (int i = 0; i < Xup.size(); i++) {
                new_res.Xup[i] = Xup[i].template cast<NewScalar>();
                new_res.S[i]   = S[i].template cast<NewScalar>();
                new_res.v[i]   = v[i].template cast<NewScalar>();
                new_res.c[i]   = c[i].template cast<NewScalar>();
                new_res.IA[i]  = IA[i].template cast<NewScalar>();
                new_res.pA[i]  = pA[i].template cast<NewScalar>();
                new_res.U[i]   = U[i].template cast<NewScalar>();
                new_res.d[i]   = NewScalar(d[i]);
                new_res.u[i]   = NewScalar(u[i]);
                new_res.a[i]   = a[i].template cast<NewScalar>();
                new_res.avp[i] = avp[i].template cast<NewScalar>();
                new_res.fvp[i] = fvp[i].template cast<NewScalar>();
                new_res.IC[i]  = IC[i].template cast<NewScalar>();
            }
            return new_res;
        }
    };
}  // namespace tinyrobotics
 
#endif