doxygen/HEAD/MultiBody_8h_source.html

/*

 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL

 */


#pragma once


// includes

// std

#include <stdexcept>

#include <unordered_map>

#include <vector>


// SpaceVecAlg

#include <SpaceVecAlg/SpaceVecAlg>


// RBDyn

#include <rbdyn/config.hh>


#include "Body.h"

#include "Joint.h"


namespace rbd

{


class RBDYN_DLLAPI MultiBody

{

public:

  MultiBody();


  MultiBody(std::vector<Body> bodies,

            std::vector<Joint> joints,

            std::vector<int> pred,

            std::vector<int> succ,

            std::vector<int> parent,

            std::vector<sva::PTransformd> Xt);


  int nrBodies() const

  {

    return static_cast<int>(bodies_.size());

  }


  int nrJoints() const

  {

    return static_cast<int>(joints_.size());

  }


  const std::vector<Body> & bodies() const

  {

    return bodies_;

  }


  void bodies(std::vector<Body> b)

  {

    bodies_ = std::move(b);

  }


  const Body & body(int num) const

  {

    return bodies_[static_cast<std::size_t>(num)];

  }


  void body(int num, const Body & b)

  {

    bodies_[static_cast<std::size_t>(num)] = b;

  }


  const std::vector<Joint> & joints() const

  {

    return joints_;

  }


  const Joint & joint(int num) const

  {

    return joints_[static_cast<std::size_t>(num)];

  }


  const std::vector<int> & predecessors() const

  {

    return pred_;

  }


  int predecessor(int num) const

  {

    return pred_[static_cast<std::size_t>(num)];

  }


  const std::vector<int> & successors() const

  {

    return succ_;

  }


  int successor(int num) const

  {

    return succ_[static_cast<std::size_t>(num)];

  }


  const std::vector<int> & parents() const

  {

    return parent_;

  }


  int parent(int num) const

  {

    return parent_[static_cast<std::size_t>(num)];

  }


  const std::vector<sva::PTransformd> & transforms() const

  {

    return Xt_;

  }


  void transforms(std::vector<sva::PTransformd> Xt)

  {

    Xt_ = std::move(Xt);

  }


  const sva::PTransformd & transform(int num) const

  {

    return Xt_[static_cast<std::size_t>(num)];

  }


  void transform(int num, const sva::PTransformd & Xt)

  {

    Xt_[static_cast<std::size_t>(num)] = Xt;

  }


  int bodyIndexByName(const std::string & name) const

  {

    return bodyNameToInd_.find(name)->second;

  }


  int jointIndexByName(const std::string & name) const

  {

    return jointNameToInd_.find(name)->second;

  }


  const std::unordered_map<std::string, int> & bodyIndexByName() const

  {

    return bodyNameToInd_;

  }


  const std::unordered_map<std::string, int> & jointIndexByName() const

  {

    return jointNameToInd_;

  }


  int jointPosInParam(int i) const

  {

    return jointPosInParam_[static_cast<std::size_t>(i)];

  }


  int jointPosInDof(int i) const

  {

    return jointPosInDof_[static_cast<std::size_t>(i)];

  }


  const std::vector<int> & jointsPosInParam() const

  {

    return jointPosInParam_;

  }


  const std::vector<int> & jointsPosInDof() const

  {

    return jointPosInDof_;

  }


  int nrParams() const

  {

    return nrParams_;

  }


  int nrDof() const

  {

    return nrDof_;

  }


  // safe accessors version for python binding


  void sBodies(std::vector<Body> b)

  {

    if(b.size() != bodies_.size())

    {

      std::ostringstream str;

      str << "bodies vector size mismatch: expected size " << bodies_.size() << " gived " << b.size();

      throw std::runtime_error(str.str());

    }

    bodies(b);

  }


  const Body & sBody(int num) const

  {

    return bodies_.at(static_cast<std::size_t>(num));

  }


  void sBody(int num, const Body & b)

  {

    bodies_.at(static_cast<std::size_t>(num)) = b;

  }


  const Joint & sJoint(int num) const

  {

    return joints_.at(static_cast<std::size_t>(num));

  }


  int sPredecessor(int num) const

  {

    return pred_.at(static_cast<std::size_t>(num));

  }


  int sSuccessor(int num) const

  {

    return succ_.at(static_cast<std::size_t>(num));

  }


  int sParent(int num) const

  {

    return parent_.at(static_cast<std::size_t>(num));

  }


  void sTransforms(std::vector<sva::PTransformd> Xt)

  {

    if(Xt.size() != Xt_.size())

    {

      std::ostringstream str;

      str << "transform vector size mismatch: expected size " << Xt_.size() << " gived " << Xt.size();

      throw std::runtime_error(str.str());

    }

    transforms(Xt);

  }


  const sva::PTransformd & sTransform(int num) const

  {

    return Xt_.at(static_cast<std::size_t>(num));

  }


  void sTransform(int num, const sva::PTransformd & Xt)

  {

    Xt_.at(static_cast<std::size_t>(num)) = Xt;

  }


  int sJointPosInParam(int i) const

  {

    return jointPosInParam_.at(static_cast<std::size_t>(i));

  }


  int sJointPosInDof(int i) const

  {

    return jointPosInDof_.at(static_cast<std::size_t>(i));

  }


  int sBodyIndexByName(const std::string & name) const

  {

    return bodyNameToInd_.at(name);

  }


  int sJointIndexByName(const std::string & name) const

  {

    return jointNameToInd_.at(name);

  }


protected:

  std::vector<Body> bodies_;

  std::vector<Joint> joints_;


  std::vector<int> pred_;

  std::vector<int> succ_;

  std::vector<int> parent_;

  std::vector<sva::PTransformd> Xt_;


  std::unordered_map<std::string, int> bodyNameToInd_;

  std::unordered_map<std::string, int> jointNameToInd_;


  std::vector<int> jointPosInParam_;

  std::vector<int> jointPosInDof_;


  int nrParams_;

  int nrDof_;

};


} // namespace rbd