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_;

  }


  void setJointRotorInertia(int index, double Ir)

  {

    joints_[static_cast<std::size_t>(index)].setRotorInertia(Ir);

  }


  void setJointGearRatio(int index, double gr)

  {

    joints_[static_cast<std::size_t>(index)].setGearRatio(gr);

  }


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

Body.h

Joint.h

rbd::Body
Definition Body.h:21

rbd::Joint
Definition Joint.h:36

rbd::MultiBody
Definition MultiBody.h:30

rbd::MultiBody::sSuccessor
int sSuccessor(int num) const
Definition MultiBody.h:290

rbd::MultiBody::successors
const std::vector< int > & successors() const
Definition MultiBody.h:110

rbd::MultiBody::Xt_
std::vector< sva::PTransformd > Xt_
Transformation from the body base to joint i.
Definition MultiBody.h:375

rbd::MultiBody::MultiBody
MultiBody()

rbd::MultiBody::joint
const Joint & joint(int num) const
Definition MultiBody.h:92

rbd::MultiBody::sTransform
const sva::PTransformd & sTransform(int num) const
Definition MultiBody.h:320

rbd::MultiBody::nrParams
int nrParams() const
Definition MultiBody.h:206

rbd::MultiBody::jointPosInDof
int jointPosInDof(int i) const
Definition MultiBody.h:188

rbd::MultiBody::predecessor
int predecessor(int num) const
Definition MultiBody.h:104

rbd::MultiBody::setJointGearRatio
void setJointGearRatio(int index, double gr)
Set Joint Gear Ratio.
Definition MultiBody.h:234

rbd::MultiBody::sPredecessor
int sPredecessor(int num) const
Definition MultiBody.h:282

rbd::MultiBody::bodies
void bodies(std::vector< Body > b)
Set bodies of the multibody system.
Definition MultiBody.h:68

rbd::MultiBody::nrDof_
int nrDof_
Definition MultiBody.h:386

rbd::MultiBody::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)

rbd::MultiBody::bodyIndexByName
const std::unordered_map< std::string, int > & bodyIndexByName() const
Definition MultiBody.h:170

rbd::MultiBody::parents
const std::vector< int > & parents() const
Definition MultiBody.h:122

rbd::MultiBody::parent
int parent(int num) const
Definition MultiBody.h:128

rbd::MultiBody::bodies
const std::vector< Body > & bodies() const
Definition MultiBody.h:62

rbd::MultiBody::parent_
std::vector< int > parent_
Definition MultiBody.h:373

rbd::MultiBody::transform
void transform(int num, const sva::PTransformd &Xt)
Set the transformation from the body base to joint num.
Definition MultiBody.h:152

rbd::MultiBody::sJoint
const Joint & sJoint(int num) const
Definition MultiBody.h:274

rbd::MultiBody::sJointPosInDof
int sJointPosInDof(int i) const
Definition MultiBody.h:344

rbd::MultiBody::body
const Body & body(int num) const
Definition MultiBody.h:74

rbd::MultiBody::jointsPosInDof
const std::vector< int > & jointsPosInDof() const
Definition MultiBody.h:200

rbd::MultiBody::pred_
std::vector< int > pred_
Definition MultiBody.h:371

rbd::MultiBody::sBody
void sBody(int num, const Body &b)
Definition MultiBody.h:266

rbd::MultiBody::jointPosInParam
int jointPosInParam(int i) const
Definition MultiBody.h:182

rbd::MultiBody::sTransforms
void sTransforms(std::vector< sva::PTransformd > Xt)
Definition MultiBody.h:306

rbd::MultiBody::succ_
std::vector< int > succ_
Definition MultiBody.h:372

rbd::MultiBody::successor
int successor(int num) const
Definition MultiBody.h:116

rbd::MultiBody::jointIndexByName
int jointIndexByName(const std::string &name) const
Definition MultiBody.h:164

rbd::MultiBody::body
void body(int num, const Body &b)
Set the body num in bodies list.
Definition MultiBody.h:80

rbd::MultiBody::jointIndexByName
const std::unordered_map< std::string, int > & jointIndexByName() const
Definition MultiBody.h:176

rbd::MultiBody::jointsPosInParam
const std::vector< int > & jointsPosInParam() const
Definition MultiBody.h:194

rbd::MultiBody::joints
const std::vector< Joint > & joints() const
Definition MultiBody.h:86

rbd::MultiBody::sBodies
void sBodies(std::vector< Body > b)
Definition MultiBody.h:244

rbd::MultiBody::nrParams_
int nrParams_
Definition MultiBody.h:385

rbd::MultiBody::nrDof
int nrDof() const
Definition MultiBody.h:212

rbd::MultiBody::transforms
const std::vector< sva::PTransformd > & transforms() const
Definition MultiBody.h:134

rbd::MultiBody::predecessors
const std::vector< int > & predecessors() const
Definition MultiBody.h:98

rbd::MultiBody::sBody
const Body & sBody(int num) const
Definition MultiBody.h:258

rbd::MultiBody::nrBodies
int nrBodies() const
Definition MultiBody.h:50

rbd::MultiBody::sJointPosInParam
int sJointPosInParam(int i) const
Definition MultiBody.h:336

rbd::MultiBody::bodies_
std::vector< Body > bodies_
Definition MultiBody.h:368

rbd::MultiBody::transforms
void transforms(std::vector< sva::PTransformd > Xt)
Definition MultiBody.h:140

rbd::MultiBody::setJointRotorInertia
void setJointRotorInertia(int index, double Ir)
Set Joint Rotor Inertia.
Definition MultiBody.h:223

rbd::MultiBody::transform
const sva::PTransformd & transform(int num) const
Definition MultiBody.h:146

rbd::MultiBody::jointNameToInd_
std::unordered_map< std::string, int > jointNameToInd_
Definition MultiBody.h:378

rbd::MultiBody::bodyIndexByName
int bodyIndexByName(const std::string &name) const
Definition MultiBody.h:158

rbd::MultiBody::jointPosInDof_
std::vector< int > jointPosInDof_
Position of joint i in dof vector (velocity, acceleration...).
Definition MultiBody.h:383

rbd::MultiBody::joints_
std::vector< Joint > joints_
Definition MultiBody.h:369

rbd::MultiBody::sBodyIndexByName
int sBodyIndexByName(const std::string &name) const
Definition MultiBody.h:353

rbd::MultiBody::sParent
int sParent(int num) const
Definition MultiBody.h:298

rbd::MultiBody::sJointIndexByName
int sJointIndexByName(const std::string &name) const
Definition MultiBody.h:362

rbd::MultiBody::nrJoints
int nrJoints() const
Definition MultiBody.h:56

rbd::MultiBody::jointPosInParam_
std::vector< int > jointPosInParam_
Position of joint i in parameter vector.
Definition MultiBody.h:381

rbd::MultiBody::bodyNameToInd_
std::unordered_map< std::string, int > bodyNameToInd_
Definition MultiBody.h:377

rbd::MultiBody::sTransform
void sTransform(int num, const sva::PTransformd &Xt)
Definition MultiBody.h:328

rbd
Definition common.h:21