Model.hpp

Go to the documentation of this file.

// Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
// Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
// RDL - Robot Dynamics Library
// 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 <limits>
#include <list>
#include <map>
#include <rclcpp/rclcpp.hpp>
#include <type_traits>
 
#include <rdl_dynamics/Body.hpp>
#include <rdl_dynamics/Joint.hpp>
#include <rdl_dynamics/Momentum.hpp>
#include <rdl_dynamics/RdlExceptions.hpp>
#include <rdl_dynamics/ReferenceFrame.hpp>
#include <rdl_dynamics/SpatialAcceleration.hpp>
#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.
 
namespace RobotDynamics
{
struct Model
{
    using SharedPtr = std::shared_ptr<Model>;
    explicit Model(const rclcpp::Node::SharedPtr& n = nullptr, unsigned int* n_threads = nullptr);
 
    ~Model()
    {
        work.reset();
        thread_pool.join_all();
    }
 
    Model(const Model&) = delete;
    void operator=(const Model&) = delete;
 
    void setupRosParameters();
 
    // 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> bodyCenterOfBuoyancyFrames; 
    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 = 0;
 
    unsigned int q_size = 0;
 
    unsigned int qdot_size = 0;
 
    double mass = 0.;
    double volume = 0.;
 
    unsigned int previously_added_body_id = 0;
 
    Math::MotionVector gravity;
    double fluidDensity = 1000.; 
    Math::MotionVector a_f;      
    Math::MotionVector v_f;      
    // 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;             
    std::vector<Math::SpatialMatrix> I_add; 
    std::vector<Math::SpatialMatrix> I_H;   
    std::vector<Math::SpatialMatrix> Ic_H;  
    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::numeric_limits<unsigned int>::max() / 2;
 
    std::vector<Body> mBodies;
 
    std::map<std::string, unsigned int> mBodyNameMap;
 
    std::map<std::string, ReferenceFramePtr> referenceFrameMap;
    unsigned int num_branch_ends = 0;
 
    ReferenceFramePtr comFrame;
 
    ReferenceFramePtr cobFrame;
 
    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;
 
    rclcpp::Node::SharedPtr node;
    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handle;
 
    rcl_interfaces::msg::SetParametersResult paramCb(const std::vector<rclcpp::Parameter>& parameters);
 
    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
 
#endif