Frame.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rbdyn/api.h>
#include <mc_rbdyn/fwd.h>
#include <mc_rbdyn/hat.h>
 
#include <mc_tvm/fwd.h>
 
#include <mc_rtc/shared.h>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace mc_rbdyn
{
 
struct MC_RBDYN_DLLAPI Frame : public mc_rtc::shared<Frame>
{
protected:
  struct NewFrameToken
  {
  };
 
public:
  Frame(NewFrameToken, const std::string & name) noexcept;
 
  Frame(NewFrameToken, const std::string & name, Frame & parent, sva::PTransformd X_p_f, bool bake) noexcept;
 
  virtual ~Frame();
 
  /* Prevent copy and move */
  Frame(const Frame &) = delete;
  Frame(Frame &&) = delete;
  Frame & operator=(const Frame &) = delete;
  Frame & operator=(Frame &&) = delete;
 
  inline static FramePtr make(const std::string & name) noexcept
  {
    return std::make_shared<Frame>(NewFrameToken{}, name);
  }
 
  static FramePtr make(const std::string & name, Frame & parent, sva::PTransformd X_p_f, bool baked);
 
  inline const std::string & name() const noexcept { return name_; }
 
  virtual inline sva::PTransformd position() const noexcept
  {
    return parent_ ? position_ * parent_->position() : position_;
  }
 
  virtual inline sva::MotionVecd velocity() const noexcept
  {
    if(!parent_) { return velocity_; }
    auto vel = parent_->velocity();
    auto X_0_p = parent_->position();
    vel.linear() += -hat(X_0_p.rotation().transpose() * position_.translation()) * vel.angular();
    return vel;
  }
 
  inline Frame & position(sva::PTransformd pos) noexcept
  {
    position_ = pos;
    return *this;
  }
 
  inline Frame & velocity(sva::MotionVecd velocity) noexcept
  {
    velocity_ = velocity;
    return *this;
  }
 
  inline const FramePtr & parent() const noexcept { return parent_; }
 
  inline mc_tvm::Frame & tvm_frame() const
  {
    if(!tvm_frame_) { init_tvm_frame(); }
    return *tvm_frame_;
  }
 
protected:
  std::string name_;
  FramePtr parent_ = nullptr;
  sva::PTransformd position_ = sva::PTransformd::Identity();
  sva::MotionVecd velocity_ = sva::MotionVecd::Zero();
  mutable mc_tvm::FramePtr tvm_frame_;
  virtual void init_tvm_frame() const;
};
 
} // namespace mc_rbdyn