ReferenceFrame.hpp

Go to the documentation of this file.

/*
 * RDL - Robot Dynamics Library
 * Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#ifndef __RDL_REFERENCE_FRAME_HPP__
#define __RDL_REFERENCE_FRAME_HPP__

#include <climits>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "rdl_dynamics/FrameExceptions.hpp"
#include "rdl_dynamics/SpatialAlgebraOperators.h"

namespace RobotDynamics
{
class ReferenceFrame;
class FixedReferenceFrame;
typedef std::shared_ptr<ReferenceFrame> ReferenceFramePtr;
typedef std::shared_ptr<FixedReferenceFrame> FixedReferenceFramePtr;
typedef std::vector<ReferenceFramePtr> ReferenceFramePtrV;
typedef std::vector<FixedReferenceFramePtr> FixedReferenceFramePtrV;
class ReferenceFrame
{
  public:
    ReferenceFrame(const ReferenceFrame& referenceFrameToCopy)
      : frameName(referenceFrameToCopy.frameName)
      , parentFrame(referenceFrameToCopy.parentFrame)
      , transformFromParent(referenceFrameToCopy.transformFromParent)
      , transformToRoot(referenceFrameToCopy.transformToRoot)
      , inverseTransformToRoot(referenceFrameToCopy.inverseTransformToRoot)
      , rootFrame(referenceFrameToCopy.rootFrame)
      , isWorldFrame(referenceFrameToCopy.isWorldFrame)
      , isBodyFrame(referenceFrameToCopy.isBodyFrame)
      , movableBodyId(referenceFrameToCopy.movableBodyId)
    {
    }

    // cppcheck-suppress passedByValue
    ReferenceFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const RobotDynamics::Math::SpatialTransform& transformFromParent, bool isBodyFrame,
                   unsigned int movableBodyId)
      : frameName(frameName)
      , parentFrame(parentFrame)
      , transformFromParent(transformFromParent)
      , isWorldFrame(false)
      , isBodyFrame(isBodyFrame)
      , movableBodyId(movableBodyId)
    {
        if (parentFrame == nullptr)
        {
            throw ReferenceFrameException("You are not allowed to create a frame with parentFrame=nullptr. Only a root frame and the world frame may have "
                                          "parentFrame=nullptr");
        }

        ReferenceFramePtr root = parentFrame;

        while (root->getParentFrame() != nullptr)
        {
            root = root->getParentFrame();
        }

        rootFrame = root.get();

        update();
    }

    ReferenceFrame()
    {
    }

    ReferenceFrame* getRootFrame()
    {
        return rootFrame;
    }

    virtual ~ReferenceFrame()
    {
    }

    void update();

    // cppcheck-suppress passedByValue
    inline void getTransformToDesiredFrame(RobotDynamics::Math::SpatialTransform& transformToPack, ReferenceFramePtr desiredFrame)
    {
        transformToPack = getTransformToDesiredFrame(desiredFrame);
    }

    virtual RobotDynamics::Math::SpatialTransform getTransformToDesiredFrame(ReferenceFramePtr desiredFrame);

    void verifyFramesHaveSameRoot(ReferenceFramePtr frame);

    inline void setTransformFromParent(const RobotDynamics::Math::SpatialTransform& transformFromParent)
    {
        this->transformFromParent = transformFromParent;
    }

    void checkReferenceFramesMatch(ReferenceFramePtr referenceFrame) const;

    void checkReferenceFramesMatch(ReferenceFrame* referenceFrame) const;

    virtual RobotDynamics::Math::SpatialTransform getTransformToRoot()
    {
        return this->transformToRoot;
    }

    virtual RobotDynamics::Math::SpatialTransform getInverseTransformToRoot()
    {
        return this->inverseTransformToRoot;
    }

    inline ReferenceFramePtr getParentFrame()
    {
        return this->parentFrame;
    }

    inline std::string getName() const
    {
        return this->frameName;
    }

    static ReferenceFramePtr createARootFrame(const std::string& frameName)
    {
        return ReferenceFramePtr(new ReferenceFrame(frameName, false, 0, true));
    }

    static ReferenceFramePtr getWorldFrame()
    {
        return worldFrame;
    }

    inline RobotDynamics::Math::SpatialTransform getTransformFromParent()
    {
        return this->transformFromParent;
    }

    inline RobotDynamics::Math::SpatialTransform getTransformToParent()
    {
        return this->transformFromParent.inverse();
    }

    inline bool getIsWorldFrame() const
    {
        return this->isWorldFrame;
    }

    inline unsigned int getMovableBodyId() const
    {
        return this->movableBodyId;
    }

    inline bool getIsBodyFrame() const
    {
        return this->isBodyFrame;
    }

    ReferenceFrame& operator=(const ReferenceFrame& other)
    {
        worldFrame = other.worldFrame;
        frameName = other.frameName;
        parentFrame = other.parentFrame;
        transformFromParent = other.transformFromParent;
        transformToRoot = other.transformToRoot;
        inverseTransformToRoot = other.inverseTransformToRoot;
        isWorldFrame = other.isWorldFrame;
        isBodyFrame = other.isBodyFrame;
        movableBodyId = other.movableBodyId;
        rootFrame = other.rootFrame;

        return *this;
    }

  protected:
    ReferenceFrame(const std::string& frameName, bool isWorldFrame, unsigned int movableBodyId, bool isBodyFrame)
      : frameName(frameName), parentFrame(nullptr), isWorldFrame(isWorldFrame), isBodyFrame(isBodyFrame), movableBodyId(movableBodyId)
    {
        rootFrame = this;
        update();
    }

    static ReferenceFramePtr createAWorldFrame(const std::string& frameName = "world")
    {
        return ReferenceFramePtr(new ReferenceFrame(frameName, true, 0, true));
    }

    static ReferenceFramePtr worldFrame;                          
    std::string frameName;                                        
    ReferenceFramePtr parentFrame;                                
    RobotDynamics::Math::SpatialTransform transformFromParent;    
    RobotDynamics::Math::SpatialTransform transformToRoot;        
    RobotDynamics::Math::SpatialTransform inverseTransformToRoot; 
    ReferenceFrame* rootFrame;
    bool isWorldFrame;          
    bool isBodyFrame;           
    unsigned int movableBodyId; 
};

class FixedReferenceFrame : public ReferenceFrame
{
    FixedReferenceFrame(const FixedReferenceFrame&) = delete;
    void operator=(const FixedReferenceFrame&) = delete;

  public:
    // cppcheck-suppress passedByValue
    FixedReferenceFrame(const std::string& frameName, ReferenceFramePtr parentFrame, const RobotDynamics::Math::SpatialTransform& transformFromParent,
                        unsigned int movableBodyId)
      : ReferenceFrame(frameName, parentFrame, transformFromParent, false, movableBodyId)
    {
    }

    virtual ~FixedReferenceFrame()
    {
    }

    RobotDynamics::Math::SpatialTransform getTransformToRoot() override
    {
        transformToRoot = parentFrame->getTransformToRoot() * transformFromParent.inverse();
        return transformToRoot;
    }

    RobotDynamics::Math::SpatialTransform getInverseTransformToRoot() override
    {
        inverseTransformToRoot = transformFromParent * parentFrame->getInverseTransformToRoot();
        return inverseTransformToRoot;
    }
};
}  // namespace RobotDynamics

#endif  // ifndef __RDL_REFERENCE_FRAME_HPP__