Model.h

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.
 */

#ifndef RDL_MODEL_H
#define RDL_MODEL_H

#include <assert.h>
#include <boost/asio/io_service.hpp>
#include <boost/function.hpp>
#include <boost/thread/thread.hpp>
#include <cstring>
#include <iostream>
#include <list>
#include <limits>
#include <map>
#include <type_traits>

#include "rdl_dynamics/Body.h"
#include "rdl_dynamics/Joint.h"
#include "rdl_dynamics/Momentum.hpp"
#include "rdl_dynamics/Quaternion.h"
#include "rdl_dynamics/RdlExceptions.hpp"
#include "rdl_dynamics/ReferenceFrame.hpp"
#include "rdl_dynamics/SpatialAcceleration.hpp"
#include "rdl_dynamics/SpatialAlgebraOperators.h"
#include "rdl_dynamics/SpatialForce.hpp"
#include "rdl_dynamics/SpatialMotion.hpp"
#include "rdl_dynamics/SpatialRigidBodyInertia.hpp"

// std::vectors containing any objects that have Eigen matrices or vectors
// as members need to have a special allocater. This can be achieved with
// the following macro.

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RobotDynamics::Joint);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RobotDynamics::Body);
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RobotDynamics::FixedBody);

namespace RobotDynamics
{
struct Model
{
    explicit Model(unsigned int* n_threads = nullptr);

    ~Model()
    {
        work.reset();
        thread_pool.join_all();
    }

    Model(const Model&) = delete;
    void operator=(const Model&) = delete;

    // Structural information

    ReferenceFramePtr worldFrame; 
    /* clang-format off */

    std::vector<ReferenceFramePtr> bodyFrames; 
    // clang for some reason continuously reformats the comment on the above line no matter
    // how many times you run it.
    /* clang-format on */

    std::vector<ReferenceFramePtr> bodyCenteredFrames;

    std::vector<ReferenceFramePtr> fixedBodyFrames;

    std::vector<unsigned int> lambda;                    
    std::vector<std::vector<unsigned int>> lambda_chain; 
    std::vector<unsigned int> lambda_q; 
    std::vector<std::vector<unsigned int>> mu; 
    unsigned int dof_count;

    unsigned int q_size;

    unsigned int qdot_size;

    double mass;

    unsigned int previously_added_body_id;

    Math::MotionVector gravity;

    // State information
    Math::SpatialAccelerationV a; 
    Math::SpatialMotionV v; 
    // Joints


    std::vector<Joint> mJoints;

    Math::MotionVectorV S;   
    Math::MotionVectorV S_o; 
    // Joint state variables
    std::vector<Math::SpatialTransform> X_J;

    std::vector<Math::SpatialVector> c_J; 
    Math::SpatialMotionV v_J; 
    std::vector<unsigned int> mJointUpdateOrder;

    // It is expressed in the coordinate frame of the parent.
    std::vector<Math::SpatialTransform> X_T;

    std::vector<unsigned int> mFixedJointCount;

    // Special variables for joints with 3 degrees of freedom

    std::vector<Math::Matrix63> multdof3_S;
    std::vector<Math::Matrix63> multdof3_S_o;
    std::vector<Math::Matrix63> multdof3_U;
    std::vector<Math::Matrix3d> multdof3_Dinv;
    std::vector<Math::Vector3d> multdof3_u;
    std::vector<unsigned int> multdof3_w_index;

    std::vector<CustomJoint*> mCustomJoints;

    // Dynamics variables

    std::vector<Math::SpatialVector> c;

    std::vector<Math::SpatialMatrix> IA;

    Math::SpatialForceV pA;

    std::vector<Math::SpatialVector> U;

    Math::VectorNd d;

    Math::VectorNd u;

    Math::SpatialForceV f;
    Math::SpatialForceV f_b;

    Math::SpatialInertiaV I;    
    Math::RigidBodyInertiaV Ic; 
    Math::SpatialInertiaV Ib_c; 
    std::vector<Math::Momentum> hc;

    Math::MatrixNd ndof0_mat;    
    Math::MatrixNd three_x_qd0;  
    Math::VectorNd q0_vec;       
    Math::VectorNd ndof0_vec;    
    Math::VectorNd nbodies0_vec; 
    // Bodies

    std::vector<Math::SpatialTransform> X_lambda;

    std::vector<FixedBody> mFixedBodies;

    unsigned int fixed_body_discriminator;

    std::vector<Body> mBodies;

    std::map<std::string, unsigned int> mBodyNameMap;

    std::map<std::string, ReferenceFramePtr> referenceFrameMap;
    unsigned int num_branch_ends;

    ReferenceFramePtr comFrame;

    boost::asio::io_service io_service;
    std::unique_ptr<boost::asio::io_service::work> work;
    std::vector<boost::thread*> threads;
    boost::thread_group thread_pool;

    bool setThreadParameters(int policy, struct sched_param param)
    {
        if (threads.size() > 0)
        {
            for (unsigned int i = 0; i < threads.size(); ++i)
            {
                int retcode;
                if ((retcode = pthread_setschedparam(threads[i]->native_handle(), policy, &param)) != 0)
                {
                    if (retcode == EPERM)
                    {
                        std::cerr << "Model.setThreadParameters: Failed setting thread parameters. Invalid permissions" << std::endl;
                    }
                    else if (retcode == ESRCH)
                    {
                        std::cerr << "Model.setThreadParameters: Failed setting thread parameters. No thread with the ID could be found" << std::endl;
                    }
                    else if (retcode == EINVAL)
                    {
                        std::cerr << "Model.setThreadParameters: Failed setting thread parameters. policy is not a recognized policy, or param does not make sense for "
                                     "the policy."
                                  << std::endl;
                    }
                    else
                    {
                        std::cerr << "Model.setThreadParameters: Failed setting thread parameters. pthread_setschedparam returned with unknown error code " << retcode
                                  << std::endl;
                    }

                    return false;
                }
            }
        }

        return true;
    }

    unsigned int addBody(const unsigned int parent_id, const Math::SpatialTransform& joint_frame, const Joint& joint, const Body& body, std::string body_name = "");

    unsigned int addBodySphericalJoint(const unsigned int parent_id, const Math::SpatialTransform& joint_frame, const Joint& joint, const Body& body,
                                       std::string body_name = "");

    unsigned int appendBody(const Math::SpatialTransform& joint_frame, const Joint& joint, const Body& body, std::string body_name = "");

    unsigned int addBodyCustomJoint(const unsigned int parent_id, const Math::SpatialTransform& joint_frame, CustomJoint* custom_joint, const Body& body,
                                    std::string body_name = "");

    unsigned int GetBodyId(const char* body_name) const
    {
        if (mBodyNameMap.count(body_name) == 0)
        {
            return std::numeric_limits<unsigned int>::max();
        }

        return mBodyNameMap.find(body_name)->second;
    }

    ReferenceFramePtr getReferenceFrame(const std::string& frameName) const
    {
        ReferenceFramePtr frame;
        try
        {
            frame = referenceFrameMap.at(frameName);
        }
        catch (const std::out_of_range& e)
        {
            frame = nullptr;
        }

        return frame;
    }

    std::string GetBodyName(unsigned int body_id) const
    {
        std::map<std::string, unsigned int>::const_iterator iter = mBodyNameMap.begin();

        while (iter != mBodyNameMap.end())
        {
            if (iter->second == body_id)
            {
                return iter->first;
            }

            ++iter;
        }

        return "";
    }

    bool IsFixedBodyId(unsigned int body_id) const
    {
        if ((body_id >= fixed_body_discriminator) && (body_id < std::numeric_limits<unsigned int>::max()) && (body_id - fixed_body_discriminator < mFixedBodies.size()))
        {
            return true;
        }
        return false;
    }

    bool IsBodyId(unsigned int id) const
    {
        if ((id > 0) && (id < mBodies.size()))
        {
            return true;
        }

        if ((id >= fixed_body_discriminator) && (id < std::numeric_limits<unsigned int>::max()))
        {
            if (id - fixed_body_discriminator < mFixedBodies.size())
            {
                return true;
            }
        }
        return false;
    }

    unsigned int GetParentBodyId(unsigned int id)
    {
        if (id >= fixed_body_discriminator)
        {
            return mFixedBodies[id - fixed_body_discriminator].mMovableParent;
        }

        unsigned int parent_id = lambda[id];

        while (mBodies[parent_id].mIsVirtual)
        {
            parent_id = lambda[parent_id];
        }

        return parent_id;
    }

    Math::SpatialTransform GetJointFrame(unsigned int id)
    {
        if (id >= fixed_body_discriminator)
        {
            return mFixedBodies[id - fixed_body_discriminator].mParentTransform;
        }

        unsigned int child_id = id;
        unsigned int parent_id = lambda[id];

        if (mBodies[parent_id].mIsVirtual)
        {
            while (mBodies[parent_id].mIsVirtual)
            {
                child_id = parent_id;
                parent_id = lambda[child_id];
            }
            return X_T[child_id];
        }
        else
        {
            return X_T[id];
        }
    }

    void SetJointFrame(unsigned int id, const Math::SpatialTransform& transform)
    {
        if (id >= fixed_body_discriminator)
        {
            throw RdlException("Error: setting of parent transform not supported for fixed bodies!");
        }

        unsigned int child_id = id;
        unsigned int parent_id = lambda[id];

        if (mBodies[parent_id].mIsVirtual)
        {
            while (mBodies[parent_id].mIsVirtual)
            {
                child_id = parent_id;
                parent_id = lambda[child_id];
            }
            X_T[child_id] = transform;
        }
        else if (id > 0)
        {
            X_T[id] = transform;
        }
    }

    Math::Quaternion GetQuaternion(unsigned int i, const Math::VectorNd& Q) const
    {
        assert(mJoints[i].mJointType == JointTypeSpherical);
        unsigned int q_index = mJoints[i].q_index;
        return Math::Quaternion(Q[q_index], Q[q_index + 1], Q[q_index + 2], Q[multdof3_w_index[i]]);
    }

    void SetQuaternion(unsigned int i, const Math::Quaternion& quat, Math::VectorNd& Q) const
    {
        assert(mJoints[i].mJointType == JointTypeSpherical);
        unsigned int q_index = mJoints[i].q_index;

        Q[q_index] = quat[0];
        Q[q_index + 1] = quat[1];
        Q[q_index + 2] = quat[2];
        Q[multdof3_w_index[i]] = quat[3];
    }

    unsigned int getCommonMovableParentId(unsigned int id_1, unsigned int id_2) const
    {
        if (IsFixedBodyId(id_1))
        {
            id_1 = mFixedBodies[id_1 - fixed_body_discriminator].mMovableParent;
        }

        if (IsFixedBodyId(id_2))
        {
            id_2 = mFixedBodies[id_2 - fixed_body_discriminator].mMovableParent;
        }

        if (id_1 == id_2)
        {
            return id_1;
        }

        if (id_1 == 0 || id_2 == 0)
        {
            return 0;
        }

        unsigned int chain_1_size = lambda_chain[id_1].size();
        unsigned int chain_2_size = lambda_chain[id_2].size();

        if (chain_1_size <= chain_2_size)
        {
            // Can start at 1 bc every list should start with world body id which is zero
            for (unsigned int i = 1; i < chain_1_size; i++)
            {
                if (lambda_chain[id_1][i] != lambda_chain[id_2][i])
                {
                    return lambda_chain[id_1][i - 1];
                }
            }

            return lambda_chain[id_1][chain_1_size - 1];
        }
        else
        {
            // Can start at 1 bc every list should start with world body id which is zero
            for (unsigned int i = 1; i < chain_2_size; i++)
            {
                if (lambda_chain[id_1][i] != lambda_chain[id_2][i])
                {
                    return lambda_chain[id_2][i - 1];
                }
            }

            return lambda_chain[id_2][chain_2_size - 1];
        }
    }

    void crawlChainKinematics(unsigned int b_id, std::atomic<unsigned int>* branch_ends, const Math::VectorNd* Q, const Math::VectorNd* QDot,
                              const Math::VectorNd* QDDot);

  private:
    bool validateRigidBodyInertia(const Body& body)
    {
        if (body.mIsVirtual)
        {
            // don't care about virtual bodies really
            return true;
        }

        RobotDynamics::Math::Matrix3d rbi = body.mInertia;
        if (rbi(0, 0) <= 0 || rbi(1, 1) <= 0 || rbi(2, 2) <= 0)
        {
            std::cerr << "\033[1;31m Invalid inertia: Each element of the trace must be > 0 \033[0m" << std::endl;
            return false;
        }

        if (rbi(0, 0) >= (rbi(1, 1) + rbi(2, 2)))
        {
            std::cerr << "\033[1;31m Invalid inertia: Triangle inequality not satisfied \033[0m\n" << std::endl;
            return false;
        }

        if (rbi(1, 1) >= (rbi(2, 2) + rbi(0, 0)))
        {
            std::cerr << "\033[1;31m Invalid inertia: Triangle inequality not satisfied \033[0m\n" << std::endl;
            return false;
        }

        if (rbi(2, 2) >= (rbi(1, 1) + rbi(0, 0)))
        {
            std::cerr << "\033[1;31m Invalid inertia: Triangle inequality not satisfied \033[0m\n" << std::endl;
            return false;
        }

        if (!(rbi - rbi.transpose()).isZero(1.e-8))
        {
            std::cerr << "\033[1;31m Invalid inertia: Inertia matrix is not symmetric \033[0m\n" << std::endl;
            return false;
        }

        Eigen::EigenSolver<Eigen::Matrix3d> solver(rbi);
        Eigen::EigenSolver<Eigen::Matrix3d>::EigenvalueType eivals = solver.eigenvalues();

        for (unsigned int i = 0; i < eivals.rows(); i++)
        {
            if (eivals[i].real() <= 0)
            {
                std::cerr << "\033[1;31m Invalid inertia: Inertia matrix is not positive definite \033[0m\n" << std::endl;
                return false;
            }
        }

        return true;
    }
};
typedef std::shared_ptr<Model> ModelPtr;
}  // namespace RobotDynamics

/* _MODEL_H */
#endif  // ifndef RDL_MODEL_H