Model.cc

Go to the documentation of this file.

/*
 * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 *
 *
 * RDL - Robot Dynamics Library
 * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#include <assert.h>
#include <iostream>
#include <limits>
#include <thread>

#include "rdl_dynamics/rdl_mathutils.h"
#include "rdl_dynamics/Model.h"

using namespace RobotDynamics;
using namespace RobotDynamics::Math;

Model::Model(unsigned int* n_threads) : work(new boost::asio::io_service::work(io_service))
{
    worldFrame = ReferenceFrame::getWorldFrame();
    referenceFrameMap[worldFrame->getName()] = worldFrame;
    comFrame.reset(new FixedReferenceFrame("robot_com", worldFrame, Math::SpatialTransform(), 0));

    Body root_body;
    Joint root_joint;

    mass = 0.;

    // structural information
    lambda.push_back(0);
    lambda_q.push_back(0);
    mu.push_back(std::vector<unsigned int>());
    dof_count = 0;
    q_size = 0;
    qdot_size = 0;
    previously_added_body_id = 0;

    std::vector<unsigned int> id_chain;
    id_chain.push_back(0);
    lambda_chain.push_back(id_chain);

    gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);

    // state information
    SpatialMotion rootBodySpatialVelocity(worldFrame, worldFrame, worldFrame, SpatialVectorZero);
    SpatialAcceleration rootBodySpatialAcceleration(worldFrame, worldFrame, worldFrame, SpatialVectorZero);

    v.push_back(rootBodySpatialVelocity);
    a.push_back(rootBodySpatialAcceleration);

    // Joints
    mJoints.push_back(root_joint);
    S.push_back(MotionVector(SpatialVectorZero));
    S_o.push_back(MotionVector(SpatialVectorZero));
    X_T.push_back(SpatialTransform());

    X_J.push_back(SpatialTransform());
    v_J.push_back(SpatialMotion(worldFrame, worldFrame, worldFrame, SpatialVectorZero));
    c_J.push_back(SpatialVectorZero);

    // Spherical joints
    multdof3_S.push_back(Matrix63::Zero());
    multdof3_S_o.push_back(Matrix63::Zero());
    multdof3_U.push_back(Matrix63::Zero());
    multdof3_Dinv.push_back(Matrix3d::Zero());
    multdof3_u.push_back(Vector3d::Zero());
    multdof3_w_index.push_back(0);

    // Dynamic variables
    c.push_back(SpatialVectorZero);
    IA.push_back(SpatialMatrixIdentity);
    pA.push_back(RobotDynamics::Math::SpatialForce(worldFrame, 0., 0., 0., 0., 0., 0.));
    U.push_back(SpatialVectorZero);

    u = VectorNd::Zero(1);
    d = VectorNd::Zero(1);

    f.push_back(RobotDynamics::Math::SpatialForce(worldFrame, 0., 0., 0., 0., 0., 0.));
    f_b.push_back(RobotDynamics::Math::SpatialForce(worldFrame, 0., 0., 0., 0., 0., 0.));
    hc.push_back(RobotDynamics::Math::Momentum());

    // Bodies
    X_lambda.push_back(SpatialTransform());

    bodyFrames.push_back(worldFrame);          // 0th body is world
    bodyCenteredFrames.push_back(worldFrame);  // 0th body centered frame is world
    I.push_back(SpatialInertia(worldFrame));
    Ib_c.push_back(SpatialInertia(worldFrame));
    Ic.push_back(RigidBodyInertia());

    mBodies.push_back(root_body);
    mBodyNameMap["ROOT"] = 0;

    fixed_body_discriminator = std::numeric_limits<unsigned int>::max() / 2;

    unsigned int num_threads = n_threads == nullptr ? std::thread::hardware_concurrency() : *n_threads;
    for(unsigned int i = 0; i < num_threads; ++i)
    {   
        threads.push_back(thread_pool.create_thread(boost::bind(&boost::asio::io_service::run, &io_service)));
    }
}

unsigned int addBodyFixedJoint(Model& model, const unsigned int parent_id, const SpatialTransform& joint_frame, const Joint& joint, const Body& body,
                               std::string body_name)
{
    FixedBody fbody = FixedBody::CreateFromBody(body);
    model.mass += fbody.mMass;
    fbody.mMovableParent = parent_id;
    fbody.mParentTransform = joint_frame;

    if (model.IsFixedBodyId(parent_id))
    {
        FixedBody fixed_parent = model.mFixedBodies.at(parent_id - model.fixed_body_discriminator);

        fbody.mMovableParent = fixed_parent.mMovableParent;
        fbody.mParentTransform = joint_frame * fixed_parent.mParentTransform;
    }

    // merge the two bodies
    Body parent_body = model.mBodies.at(fbody.mMovableParent);
    parent_body.join(fbody.mParentTransform, body);
    model.mBodies.at(fbody.mMovableParent) = parent_body;

    model.I.at(fbody.mMovableParent).set(createFromMassComInertiaC(parent_body.mMass, parent_body.mCenterOfMass, parent_body.mInertia));
    model.Ib_c.at(fbody.mMovableParent).set(createFromMassComInertiaC(parent_body.mMass, Vector3dZero, parent_body.mInertia));

    // Since the bodies have been combined and there is a new COM, need to update the body's COM frame xform to parent.
    model.bodyCenteredFrames.at(fbody.mMovableParent)->setTransformFromParent(Xtrans(parent_body.mCenterOfMass));

    model.mFixedBodies.push_back(fbody);

    if (model.mFixedBodies.size() > std::numeric_limits<unsigned int>::max() - model.fixed_body_discriminator)
    {
        std::string msg = "Error: cannot add more than " + std::to_string(std::numeric_limits<unsigned int>::max() - model.mFixedBodies.size()) +
                          " fixed bodies. You need to modify Model::fixed_body_discriminator for this.";
        throw RdlException(msg);
    }

    ReferenceFramePtr fixedBodyFrame;
    fixedBodyFrame.reset(new FixedReferenceFrame(body_name, model.bodyFrames.at(fbody.mMovableParent), fbody.mParentTransform, fbody.mMovableParent));
    model.fixedBodyFrames.push_back(fixedBodyFrame);

    if (body_name.size() != 0)
    {
        if (model.mBodyNameMap.find(body_name) != model.mBodyNameMap.end())
        {
            std::string msg = "Error: Fixed body with name '" + body_name + "' already exists!";
            throw RdlException(msg);
        }
        model.mBodyNameMap[body_name] = model.mFixedBodies.size() + model.fixed_body_discriminator - 1;
        model.referenceFrameMap[body_name] = fixedBodyFrame;
    }

    return model.mFixedBodies.size() + model.fixed_body_discriminator - 1;
}

unsigned int addBodyMultiDofJoint(Model& model, const unsigned int parent_id, const SpatialTransform& joint_frame, const Joint& joint, const Body& body,
                                  std::string body_name)
{
    // Here we emulate multi DoF joints by simply adding nullbodies. This
    // allows us to use fixed size elements for S,v,a, etc. which is very
    // fast in Eigen.
    unsigned int joint_count = 0;
    if (joint.mJointType == JointType1DoF)
    {
        joint_count = 1;
    }
    else if (joint.mJointType == JointType2DoF)
    {
        joint_count = 2;
    }
    else if (joint.mJointType == JointType3DoF)
    {
        joint_count = 3;
    }
    else if (joint.mJointType == JointType4DoF)
    {
        joint_count = 4;
    }
    else if (joint.mJointType == JointType5DoF)
    {
        joint_count = 5;
    }
    else if (joint.mJointType == JointType6DoF)
    {
        joint_count = 6;
    }
    else if (joint.mJointType == JointTypeFloatingBase)
    // no action required
    {
    }
    else
    {
        std::cerr << "Error: Invalid joint type: " << joint.mJointType << std::endl;

        assert(0 && !"Invalid joint type!");
    }

    Body null_body(0., Vector3d(0., 0., 0.), Vector3d(0., 0., 0.));
    null_body.mIsVirtual = true;

    unsigned int null_parent = parent_id;
    SpatialTransform joint_frame_transform;

    if (joint.mJointType == JointTypeFloatingBase)
    {
        null_parent = model.addBody(parent_id, joint_frame, Joint(JointTypeTranslationXYZ), null_body);

        return model.addBody(null_parent, SpatialTransform(), Joint(JointTypeSpherical), body, body_name);
    }

    Joint single_dof_joint;
    unsigned int j;

    // Here we add multiple virtual bodies that have no mass or inertia for
    // which each is attached to the model with a single degree of freedom
    // joint.
    for (j = 0; j < joint_count; j++)
    {
        single_dof_joint = Joint(joint.mJointAxes[j]);
        if (single_dof_joint.mJointType == JointType1DoF)
        {
            Vector3d rotation(joint.mJointAxes[j][0], joint.mJointAxes[j][1], joint.mJointAxes[j][2]);
            Vector3d translation(joint.mJointAxes[j][3], joint.mJointAxes[j][4], joint.mJointAxes[j][5]);

            if (rotation == Vector3d(0., 0., 0.))
            {
                single_dof_joint = Joint(JointTypePrismatic, translation);
            }
            else if (translation == Vector3d(0., 0., 0.))
            {
                single_dof_joint = Joint(JointTypeRevolute, rotation);
            }
            else
            {
                std::cerr << "Invalid joint axis: " << joint.mJointAxes[0].transpose() << ". Helical joints not (yet) supported." << std::endl;
                abort();
            }
        }

        // the first joint has to be transformed by joint_frame, all the
        // others must have a null transformation
        if (j == 0)
        {
            joint_frame_transform = joint_frame;
        }
        else
        {
            joint_frame_transform = SpatialTransform();
        }

        if (j == joint_count - 1)
        {
            // if we are at the last we must add the real body
            break;
        }
        else
        {
            // otherwise we just add an intermediate body
            null_parent = model.addBody(null_parent, joint_frame_transform, single_dof_joint, null_body);
        }
    }

    return model.addBody(null_parent, joint_frame_transform, single_dof_joint, body, body_name);
}

unsigned int Model::addBody(const unsigned int parent_id, const SpatialTransform& joint_frame, const Joint& joint, const Body& body, std::string body_name)
{
    assert(lambda.size() > 0);
    assert(joint.mJointType != JointTypeUndefined);

    if (joint.mJointType == JointTypeFixed)
    {
        previously_added_body_id = addBodyFixedJoint(*this, parent_id, joint_frame, joint, body, body_name);

        return previously_added_body_id;
    }
    else if ((joint.mJointType == JointTypeSpherical) || (joint.mJointType == JointTypeEulerZYX) || (joint.mJointType == JointTypeEulerXYZ) ||
             (joint.mJointType == JointTypeEulerYXZ) || (joint.mJointType == JointTypeTranslationXYZ) || (joint.mJointType == JointTypeCustom))
    {
        // no action required
    }
    else if (joint.mJointType != JointTypePrismatic && joint.mJointType != JointTypeRevolute && joint.mJointType != JointTypeRevoluteX &&
             joint.mJointType != JointTypeRevoluteY && joint.mJointType != JointTypeRevoluteZ)
    {
        previously_added_body_id = addBodyMultiDofJoint(*this, parent_id, joint_frame, joint, body, body_name);
        return previously_added_body_id;
    }

    // If we add the body to a fixed body we have to make sure that we
    // actually add it to its movable parent.
    unsigned int movable_parent_id = parent_id;
    SpatialTransform movable_parent_transform;

    if (IsFixedBodyId(parent_id))
    {
        unsigned int fbody_id = parent_id - fixed_body_discriminator;
        movable_parent_id = mFixedBodies.at(fbody_id).mMovableParent;
        movable_parent_transform = mFixedBodies.at(fbody_id).mParentTransform;
    }

    // structural information
    lambda.push_back(movable_parent_id);
    unsigned int lambda_q_last = mJoints.back().q_index;

    if (mJoints[mJoints.size() - 1].mDoFCount > 0 && mJoints[mJoints.size() - 1].mJointType != JointTypeCustom)
    {
        lambda_q_last = lambda_q_last + mJoints.back().mDoFCount;
    }
    else if (mJoints[mJoints.size() - 1].mJointType == JointTypeCustom)
    {
        lambda_q_last = lambda_q_last + mCustomJoints.back()->mDoFCount;
    }

    for (unsigned int i = 0; i < joint.mDoFCount; i++)
    {
        lambda_q.push_back(lambda_q_last + i);
    }
    mu.push_back(std::vector<unsigned int>());
    mu.at(movable_parent_id).push_back(mBodies.size());

    num_branch_ends = 0;
    for(unsigned int i = 0; i < mu.size(); ++i)
    {
        if(mu[i].size() == 0)
        {
            num_branch_ends++;
        }
    }

    // Bodies
    X_lambda.push_back(joint_frame);

    ReferenceFramePtr bodyFrame;
    ReferenceFramePtr parentBodyFrame;
    if (!movable_parent_id)
    {
        parentBodyFrame = ReferenceFrame::getWorldFrame();
        // movable_parent_id = 0 so this is the root body, and it's parent frame is world frame
        bodyFrame.reset(new ReferenceFrame(body_name, parentBodyFrame, X_lambda.back(), true, mBodies.size()));
    }
    else
    {
        parentBodyFrame = bodyFrames.at(movable_parent_id);
        bodyFrame.reset(new ReferenceFrame(body_name, parentBodyFrame, joint_frame * movable_parent_transform, true, mBodies.size()));
    }

    bodyFrames.push_back(bodyFrame);
    referenceFrameMap[body_name] = bodyFrame;

    mBodies.push_back(body);
    nbodies0_vec.conservativeResize(mBodies.size());
    nbodies0_vec.setZero();

    if (!body_name.empty())
    {
        if (mBodyNameMap.find(body_name) != mBodyNameMap.end())
        {
            std::string msg = "Error: Body with name '" + body_name + "' already exists!";
            throw RdlException(msg);
        }
        mBodyNameMap[body_name] = mBodies.size() - 1;
    }

    // state information
    // Body spatial velocity. Expressed in body frame
    v.push_back(SpatialMotion(bodyFrame, ReferenceFrame::getWorldFrame(), bodyFrame, SpatialVectorZero));
    a.push_back(SpatialAcceleration(bodyFrame, ReferenceFrame::getWorldFrame(), bodyFrame, SpatialVectorZero));

    // Joints
    unsigned int prev_joint_index = mJoints.size() - 1;
    mJoints.push_back(joint);

    mJoints.back().q_index = mJoints.at(prev_joint_index).q_index + mJoints.at(prev_joint_index).mDoFCount;

    S.push_back(MotionVector(joint.mJointAxes[0]));
    S_o.push_back(MotionVector(SpatialVectorZero));
    // Joint state variables
    X_J.push_back(SpatialTransform());
    c_J.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));

    v_J.push_back(SpatialMotion(bodyFrame, parentBodyFrame, bodyFrame, joint.mJointAxes[0]));

    // workspace for joints with 3 dof
    multdof3_S.push_back(Matrix63::Zero(6, 3));
    multdof3_S_o.push_back(Matrix63::Zero(6, 3));
    multdof3_U.push_back(Matrix63::Zero());
    multdof3_Dinv.push_back(Matrix3d::Zero());
    multdof3_u.push_back(Vector3d::Zero());
    multdof3_w_index.push_back(0);

    dof_count = dof_count + joint.mDoFCount;

    ndof0_mat.conservativeResize(dof_count, dof_count);
    ndof0_mat.setZero();

    ndof0_vec.conservativeResize(dof_count);
    ndof0_vec.setZero();

    // update the w components of the Quaternions. They are stored at the end
    // of the q vector
    int multdof3_joint_counter = 0;
    for (unsigned int i = 1; i < mJoints.size(); i++)
    {
        if (mJoints.at(i).mJointType == JointTypeSpherical)
        {
            multdof3_w_index.at(i) = dof_count + multdof3_joint_counter;
            multdof3_joint_counter++;
        }
    }

    q_size = dof_count + multdof3_joint_counter;
    q0_vec.conservativeResize(q_size);
    q0_vec.setZero();

    qdot_size = qdot_size + joint.mDoFCount;

    three_x_qd0.conservativeResize(3, qdot_size);
    three_x_qd0.setZero();

    // we have to invert the transformation as it is later always used from the
    // child bodies perspective.
    X_T.push_back(joint_frame * movable_parent_transform);

    // Dynamic variables
    c.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
    IA.push_back(SpatialMatrix::Zero(6, 6));
    pA.push_back(RobotDynamics::Math::SpatialForce(bodyFrame, 0., 0., 0., 0., 0., 0.));
    U.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));

    d = VectorNd::Zero(mBodies.size());
    u = VectorNd::Zero(mBodies.size());

    f.push_back(RobotDynamics::Math::SpatialForce(bodyFrame, 0., 0., 0., 0., 0., 0.));
    f_b.push_back(RobotDynamics::Math::SpatialForce(bodyFrame, 0., 0., 0., 0., 0., 0.));

    ReferenceFramePtr centerOfMassFrame;
    centerOfMassFrame.reset(new FixedReferenceFrame(body_name + "_com", bodyFrames.back(), Xtrans(body.mCenterOfMass), bodyFrame->getMovableBodyId()));

    bodyCenteredFrames.push_back(centerOfMassFrame);

    if (!validateRigidBodyInertia(body))
    {
        std::cerr << "\033[1;31m Body " << body_name << " failed inertia validation\033[0m" << std::endl;
    }

    SpatialInertia bodyInertia(bodyFrames.back(), createFromMassComInertiaC(body.mMass, body.mCenterOfMass, body.mInertia));
    SpatialInertia bodyCenteredInertia(centerOfMassFrame, createFromMassComInertiaC(body.mMass, Vector3dZero, body.mInertia));

    I.push_back(bodyInertia);
    Ib_c.push_back(bodyCenteredInertia);
    Ic.push_back(bodyInertia);

    hc.push_back(RobotDynamics::Math::Momentum());

    if (mBodies.size() == fixed_body_discriminator)
    {
        std::cerr << "Error: cannot add more than " << fixed_body_discriminator
                  << " movable bodies. You need to modify "
                     "Model::fixed_body_discriminator for this."
                  << std::endl;
        assert(0);
        abort();
    }

    previously_added_body_id = mBodies.size() - 1;

    mJointUpdateOrder.clear();

    // update the joint order computation
    std::vector<std::pair<JointType, unsigned int>> joint_types;
    for (unsigned int i = 0; i < mJoints.size(); i++)
    {
        joint_types.push_back(std::pair<JointType, unsigned int>(mJoints[i].mJointType, i));
        mJointUpdateOrder.push_back(mJoints[i].mJointType);
    }

    mJointUpdateOrder.clear();
    JointType current_joint_type = JointTypeUndefined;
    while (joint_types.size() != 0)
    {
        current_joint_type = joint_types[0].first;

        std::vector<std::pair<JointType, unsigned int>>::iterator type_iter = joint_types.begin();

        while (type_iter != joint_types.end())
        {
            if (type_iter->first == current_joint_type)
            {
                mJointUpdateOrder.push_back(type_iter->second);
                type_iter = joint_types.erase(type_iter);
            }
            else
            {
                ++type_iter;
            }
        }
    }

    std::vector<unsigned int> parent_chain(lambda_chain.at(movable_parent_id).size() + 1);
    for (unsigned int idx = 0; idx < lambda_chain.at(movable_parent_id).size(); idx++)
    {
        parent_chain[idx] = lambda_chain.at(movable_parent_id).at(idx);
    }

    parent_chain.back() = previously_added_body_id;  // add this joint
    lambda_chain.push_back(parent_chain);

    mass += body.mMass;
    return previously_added_body_id;
}

unsigned int Model::appendBody(const Math::SpatialTransform& joint_frame, const Joint& joint, const Body& body, std::string body_name)
{
    return Model::addBody(previously_added_body_id, joint_frame, joint, body, body_name);
}

unsigned int Model::addBodyCustomJoint(const unsigned int parent_id, const Math::SpatialTransform& joint_frame, CustomJoint* custom_joint, const Body& body,
                                       std::string body_name)
{
    custom_joint->ndof0_vec = VectorNd::Zero(custom_joint->mDoFCount);
    Joint proxy_joint(JointTypeCustom, custom_joint->mDoFCount);
    proxy_joint.custom_joint_index = mCustomJoints.size();

    // proxy_joint.mDoFCount = custom_joint->mDoFCount; //MM added. Otherwise
    // model.q_size = 0, which is not good.

    mCustomJoints.push_back(custom_joint);

    unsigned int body_id = addBody(parent_id, joint_frame, proxy_joint, body, body_name);

    return body_id;
}

void Model::crawlChainKinematics(unsigned int b_id, std::atomic<unsigned int>* branch_ends, const Math::VectorNd *Q, const Math::VectorNd *QDot, const Math::VectorNd *QDDot)
{
    if(b_id > 0)
    {
        if(Q && !QDot && !QDDot)
        {
            jcalc(*this, b_id, (*Q), q0_vec);
        }
        else if(Q && QDot && !QDDot)
        {
            jcalc(*this, b_id, *Q, *QDot);

            ReferenceFramePtr bodyFrame = bodyFrames[b_id];
            unsigned int lambda_id = lambda[b_id];

            if(lambda_id != 0)
            {
                v[b_id].set(v[lambda_id].transform_copy(bodyFrame->getTransformFromParent()) + v_J[b_id]);
                c[b_id] = c_J[b_id] + v[b_id] % v_J[b_id];
            }
            else
            {
                v[b_id].set(v_J[b_id]);
                c[b_id] = c_J[b_id] + v[b_id] % v_J[b_id];
            }
        }
        else if(Q && QDot && QDDot)
        {
            jcalc(*this, b_id, *Q, *QDot);

            ReferenceFramePtr bodyFrame = bodyFrames[b_id];
            unsigned int lambda_id = lambda[b_id];

            if(lambda_id != 0)
            {
                v[b_id].set(v[lambda_id].transform_copy(bodyFrame->getTransformFromParent()) + v_J[b_id]);
                c[b_id] = c_J[b_id] + v[b_id] % v_J[b_id];
            }
            else
            {
                v[b_id].set(v_J[b_id]);
                c[b_id] = c_J[b_id] + v[b_id] % v_J[b_id];
            }

            unsigned int q_index = mJoints[b_id].q_index;

            if(lambda_id != 0)
            {
                a[b_id].set(a[lambda_id].transform_copy(bodyFrame->getTransformFromParent()) + c[b_id]);
            }
            else
            {
                a[b_id].set(c[b_id]);
            }

            if(mJoints[b_id].mJointType != JointTypeCustom)
            {
                if(mJoints[b_id].mDoFCount == 1)
                {
                    a[b_id].set(a[b_id] + S[b_id] * (*QDDot)[q_index]);
                }
                else if(mJoints[b_id].mDoFCount == 3)
                {
                    Vector3d omegadot_temp((*QDDot)[q_index], (*QDDot)[q_index + 1], (*QDDot)[q_index + 2]);
                    a[b_id].set(a[b_id] + multdof3_S[b_id] * omegadot_temp);
                }
            }
            else
            {
                unsigned int custom_index = mJoints[b_id].custom_joint_index;
                const CustomJoint *custom_joint = mCustomJoints[custom_index];
                unsigned int joint_dof_count = custom_joint->mDoFCount;

                a[b_id].set(a[b_id] + (mCustomJoints[custom_index]->S * (QDDot->block(q_index, 0, joint_dof_count, 1))));
            }
        }
    }

    if(mu[b_id].size() > 1)
    {
        unsigned int mu_size = mu[b_id].size();
        for(unsigned int i = 0; i < mu_size - 1; ++i)
        {
            // spawn threads here
            io_service.post(boost::bind(&Model::crawlChainKinematics, this, mu[b_id][i], branch_ends, Q, QDot, QDDot));
        }

        // we have a current thread that can process this one, so no need to spawn yet another thread
        crawlChainKinematics(mu[b_id][mu_size - 1], branch_ends, Q, QDot, QDDot);

        return;
    }
    else if(mu[b_id].size() == 1)
    {
        crawlChainKinematics(mu[b_id][0], branch_ends, Q, QDot, QDDot);
        return;
    }

    (*branch_ends)++;
}