Modeling of KUKA LBR

#include <rbdl/rbdl.h>
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
struct KUKA_LBR
{
    KUKA_LBR() : model(), mass(1), Q(), QDot(), QDDot(), Tau(), body_base_id(), body_l1_id(), body_l2_id(), body_l3_id(), body_l4_id(), body_l5_id(), body_l6_id(), body_l7_id()
    {

        model.gravity = Vector3d(0., 0., -9.81);
        Body body_base, body_l1, body_l2, body_l3, body_l4, body_l5, body_l6, body_l7;
        Joint Joint_RevZ, Joint_Fixed;
        Joint_Fixed = Joint(JointTypeFixed);
        Joint_RevZ = Joint(JointTypeRevoluteZ);

        // body base
        body_base = Body(mass, Vector3d(0., 0., 0.), Vector3d(0., 0., 0.)); /* mass, com, inertia*/
        Matrix3_t world_base_rot;
        /* **note** the rotation matrix is a 3*3 concise representation of a 6*6 spacial matrix.
          The rotation matrix should be inserted as a transpose of the rotation matrix.*/
        world_base_rot << 1., 0., 0.,
            0., 1., 0.,
            0., 0., 1.;
        Vector3d world_base_trans(0., 0., 0.);
        SpatialTransform world_base_tf(world_base_rot, world_base_trans);
        body_base_id = model.AddBody(0, world_base_tf, Joint_Fixed, body_base);
        // body l1
        body_l1 = Body(mass, Vector3d(0.0, -0.017, 0.134), Vector3d(0.00815814, 0.007363868, 0.003293455)); /* mass, com, inertia*/
        Matrix3_t base_l1_rot;
        base_l1_rot << 1., 0., 0.,
            0., 1., 0.,
            0., 0., 1.;
        Vector3d base_l1_trans(0., 0., 0.103);
        SpatialTransform base_l1_tf(base_l1_rot, base_l1_trans);
        body_l1_id = model.AddBody(body_base_id, base_l1_tf, Joint_RevZ, body_l1);
        // body l2
        body_l2 = Body(mass, Vector3d(0.0, -0.074, 0.009), Vector3d(0.00812252, 0.00329668, 0.00733904)); /* mass, com, inertia*/
        Matrix3_t l1_l2_rot;
        l1_l2_rot << 1., 0., 0.,
            0., 0., -1.,
            0., 1., 0.;
        Vector3d l1_l2_trans(0.0, 0.013, 0.209);
        SpatialTransform l1_l2_tf(l1_l2_rot, l1_l2_trans);
        body_l2_id = model.AddBody(body_l1_id, l1_l2_tf, Joint_RevZ, body_l2);
        // body l3
        body_l3 = Body(mass, Vector3d(0.0, 0.017, 0.134), Vector3d(0.008159, 0.007421, 0.00330)); /* mass, com, inertia*/
        Matrix3_t l2_l3_rot;
        l2_l3_rot << 1., 0., 0.,
            0., 0., 1.,
            0., -1., 0.;
        Vector3d l2_l3_trans(0.0, -0.194, -0.009);
        SpatialTransform l2_l3_tf(l2_l3_rot, l2_l3_trans);
        body_l3_id = model.AddBody(body_l2_id, l2_l3_tf, Joint_RevZ, body_l3);
        // body l4
        body_l4 = Body(mass, Vector3d(-0.001, 0.081, 0.008), Vector3d(0.0081471, 0.003297, 0.0073715)); /* mass, com, inertia*/
        Matrix3_t l3_l4_rot;
        l3_l4_rot << 1., 0., 0.,
            0., 0., 1.,
            0., -1., 0.;
        Vector3d l3_l4_trans(0.0, -0.013, 0.202);
        SpatialTransform l3_l4_tf(l3_l4_rot, l3_l4_trans);
        body_l4_id = model.AddBody(body_l3_id, l3_l4_tf, Joint_RevZ, body_l4);
        // body l5
        body_l5 = Body(mass, Vector3d(0.0, -0.017, 0.129), Vector3d(0.0077265, 0.006950, 0.00329)); /* mass, com, inertia*/
        Matrix3_t l4_l5_rot;
        l4_l5_rot << 1., 0., 0.,
            0., 0., -1.,
            0., 1., 0.;
        Vector3d l4_l5_trans(-0.002, 0.202, -0.008);
        SpatialTransform l4_l5_tf(l4_l5_rot, l4_l5_trans);
        body_l5_id = model.AddBody(body_l4_id, l4_l5_tf, Joint_RevZ, body_l5);
        // body l6
        body_l6 = Body(mass, Vector3d(0.0, 0.007, 0.068), Vector3d(0.002983, 0.003299, 0.003146)); /* mass, com, inertia*/
        Matrix3_t l5_l6_rot;
        l5_l6_rot << 1., 0., 0.,
            0., 0., -1.,
            0., 1., 0.;
        Vector3d l5_l6_trans(0.002, -0.052, 0.204);
        SpatialTransform l5_l6_tf(l5_l6_rot, l5_l6_trans);
        body_l6_id = model.AddBody(body_l5_id, l5_l6_tf, Joint_RevZ, body_l6);
        // body l7
        body_l7 = Body(mass, Vector3d(0.006, 0.0, 0.015), Vector3d(0.000651, 0.0006512, 0.00112)); /* mass, com, inertia*/
        Matrix3_t l6_l7_rot;
        l6_l7_rot << 1., 0., 0.,
            0., 0., 1.,
            0., -1., 0.;
        Vector3d l6_l7_trans(-0.003, -0.05, 0.053);
        SpatialTransform l6_l7_tf(l6_l7_rot, l6_l7_trans);
        body_l7_id = model.AddBody(body_l6_id, l6_l7_tf, Joint_RevZ, body_l7);
        Q = VectorNd::Zero(model.dof_count);
        QDot = VectorNd::Zero(model.dof_count);
        Tau = VectorNd::Zero(model.dof_count);
        QDDot = VectorNd::Zero(model.dof_count);
    }

    Model model;
    VectorNd Q;
    VectorNd QDot;
    VectorNd QDDot;
    VectorNd Tau;
    double mass;
    unsigned int body_l1_id, body_l2_id, body_l3_id, body_l4_id, body_l5_id, body_l6_id, body_l7_id, body_base_id;
};

The robot description file format in the AMBF simulator is written in a YAML file format, where all the information regarding the joints and bodies are stored. By accessing the description file, information regarding the mass, Center Of Mass (COM), inertia, the translational offset between the parent and child bodies, etc,. for each joint and body are obtained. Body constructor is used to define the physical properties of each link, and Joint constructor is used for defining the type of the connection between the parent and child body. Spatial transformation matrices containing spacial rotation matrices and translation vectors are used to define the orientation and position of the joint origin and the child body with respect to the parent body.