MultiBodyConfig.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// includes
// std
#include <stdexcept>
#include <vector>
 
// sva
#include <SpaceVecAlg/SpaceVecAlg>
 
// RBDyn
#include <rbdyn/config.hh>
 
#include "MultiBody.h"
 
namespace rbd
{
 
struct RBDYN_DLLAPI MultiBodyConfig
{
  MultiBodyConfig() {}
  MultiBodyConfig(const MultiBody & mb);
 
  void zero(const MultiBody & mb);
 
  std::vector<std::vector<double>> q;
 
  std::vector<std::vector<double>> alpha;
 
  std::vector<std::vector<double>> alphaD;
 
  std::vector<sva::ForceVecd> force;
 
  std::vector<sva::PTransformd> jointConfig;
 
  std::vector<sva::MotionVecd> jointVelocity;
 
  std::vector<std::vector<double>> jointTorque;
 
  std::vector<Eigen::Matrix<double, 6, Eigen::Dynamic>> motionSubspace;
 
  std::vector<sva::PTransformd> bodyPosW;
 
  std::vector<sva::PTransformd> parentToSon;
 
  std::vector<sva::MotionVecd> bodyVelW;
 
  std::vector<sva::MotionVecd> bodyVelB;
 
  std::vector<sva::MotionVecd> bodyAccB;
 
  Eigen::Vector3d gravity;
 
  // python binding function
 
  std::vector<Eigen::MatrixXd> python_motionSubspace();
  void python_motionSubspace(const std::vector<Eigen::MatrixXd> & v);
};
 
class RBDYN_DLLAPI ConfigConverter
{
public:
  ConfigConverter(const MultiBody & from, const MultiBody & to);
 
  void convert(const MultiBodyConfig & from, MultiBodyConfig & to) const;
 
  template<typename T>
  void convertJoint(const std::vector<T> & from, std::vector<T> & to) const;
 
  template<typename T>
  std::vector<T> convertJoint(const std::vector<T> & from) const;
 
  // safe version for python binding
 
  static ConfigConverter * sConstructor(const MultiBody & from, const MultiBody & to);
 
  void sConvert(const MultiBodyConfig & from, MultiBodyConfig & to) const;
 
  template<typename T>
  void sConvertJoint(const std::vector<T> & from, std::vector<T> & to) const;
 
private:
  std::vector<int> jInd_;
  std::vector<int> bInd_;
};
 
RBDYN_DLLAPI void paramToVector(const std::vector<std::vector<double>> & v, Eigen::Ref<Eigen::VectorXd> e);
 
RBDYN_DLLAPI void sParamToVector(const std::vector<std::vector<double>> & v, Eigen::Ref<Eigen::VectorXd> e);
 
RBDYN_DLLAPI Eigen::VectorXd paramToVector(const MultiBody & mb, const std::vector<std::vector<double>> & v);
 
RBDYN_DLLAPI Eigen::VectorXd sParamToVector(const MultiBody & mb, const std::vector<std::vector<double>> & v);
 
RBDYN_DLLAPI Eigen::VectorXd dofToVector(const MultiBody & mb, const std::vector<std::vector<double>> & v);
 
RBDYN_DLLAPI Eigen::VectorXd sDofToVector(const MultiBody & mb, const std::vector<std::vector<double>> & v);
 
RBDYN_DLLAPI void vectorToParam(const Eigen::Ref<const Eigen::VectorXd> & e, std::vector<std::vector<double>> & v);
 
RBDYN_DLLAPI void sVectorToParam(const Eigen::Ref<const Eigen::VectorXd> & e, std::vector<std::vector<double>> & v);
 
RBDYN_DLLAPI std::vector<std::vector<double>> vectorToParam(const MultiBody & mb, const Eigen::VectorXd & e);
 
RBDYN_DLLAPI std::vector<std::vector<double>> sVectorToParam(const MultiBody & mb, const Eigen::VectorXd & e);
 
RBDYN_DLLAPI std::vector<std::vector<double>> vectorToDof(const MultiBody & mb, const Eigen::VectorXd & e);
 
RBDYN_DLLAPI std::vector<std::vector<double>> sVectorToDof(const MultiBody & mb, const Eigen::VectorXd & e);
 
template<typename T>
void checkMatchBodiesVector(const MultiBody & mb, const std::vector<T> & vec, const std::string & name);
 
template<typename T>
void checkMatchJointsVector(const MultiBody & mb, const std::vector<T> & vec, const std::string & name);
 
RBDYN_DLLAPI void checkMatchBodyPos(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchParentToSon(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchBodyVel(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchBodyAcc(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchJointConf(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchJointVelocity(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchJointTorque(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchMotionSubspace(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchQ(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchAlpha(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchAlphaD(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI void checkMatchForce(const MultiBody & mb, const MultiBodyConfig & mbc);
 
template<typename T>
void checkMatchBodiesVector(const MultiBody & mb, const std::vector<T> & vec, const std::string & name)
{
  if(int(vec.size()) != mb.nrBodies())
  {
    std::ostringstream str;
    str << name << " size mismatch: expected size " << mb.nrBodies() << " gived " << vec.size();
    throw std::domain_error(str.str());
  }
}
 
template<typename T>
void checkMatchJointsVector(const MultiBody & mb, const std::vector<T> & vec, const std::string & name)
{
  if(int(vec.size()) != mb.nrJoints())
  {
    std::ostringstream str;
    str << name << " size mismatch: expected size " << mb.nrJoints() << " gived " << vec.size();
    throw std::domain_error(str.str());
  }
}
 
template<typename T>
inline void ConfigConverter::convertJoint(const std::vector<T> & from, std::vector<T> & to) const
{
  for(std::size_t i = 0; i < jInd_.size(); ++i)
  {
    to[static_cast<size_t>(jInd_[i])] = from[i + 1];
  }
}
 
template<typename T>
inline void ConfigConverter::sConvertJoint(const std::vector<T> & from, std::vector<T> & to) const
{
  if(from.size() != to.size())
  {
    throw std::domain_error("from and to vector must have the same size");
  }
 
  convertJoint(from, to);
}
 
template<typename T>
inline std::vector<T> ConfigConverter::convertJoint(const std::vector<T> & from) const
{
  std::vector<T> to(from.size());
  for(std::size_t i = 0; i < jInd_.size(); ++i)
  {
    to[static_cast<size_t>(jInd_[i])] = from[i + 1];
  }
 
  return to;
}
 
} // namespace rbd