MultiBodyGraph.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// includes
// std
#include <map>
#include <memory>
#include <vector>
 
// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>
 
// RBDyn
#include <rbdyn/config.hh>
 
#include "Body.h"
#include "Joint.h"
 
namespace rbd
{
class MultiBody;
 
class RBDYN_DLLAPI MultiBodyGraph
{
public:
  struct Node;
 
  struct Arc
  {
    Arc() {}
 
    Arc(sva::PTransformd X0, const Joint & j, bool forward, std::shared_ptr<Node> n) : X(X0), joint(j), next(n)
    {
      joint.forward(forward);
    }
 
    sva::PTransformd X; 
    Joint joint; 
    std::shared_ptr<Node> next; 
  };
 
  struct Node
  {
    Node(const Body & b) : body(b) {}
 
    Body body;
    std::vector<Arc> arcs; 
  };
 
public:
  MultiBodyGraph(const std::string rootName = "Root");
  MultiBodyGraph(const MultiBodyGraph & mbg);
  ~MultiBodyGraph();
 
  MultiBodyGraph & operator=(const MultiBodyGraph & mbg);
 
  void clear();
 
  void addBody(const Body & B);
 
  void addJoint(const Joint & J);
 
  void linkBodies(const std::string & b1Name,
                  const sva::PTransformd & tB1,
                  const std::string & b2Name,
                  const sva::PTransformd & tB2,
                  const std::string & jointName,
                  bool isB1toB2 = true);
 
  const std::shared_ptr<Node> nodeByName(const std::string & name) const;
 
  const std::shared_ptr<Joint> jointByName(const std::string & name) const;
 
  std::size_t nrNodes() const;
 
  std::size_t nrJoints() const;
 
  MultiBody makeMultiBody(const std::string & rootBodyName,
                          bool isFixed,
                          const sva::PTransformd & X_0_j0 = sva::PTransformd::Identity(),
                          const sva::PTransformd & X_b0_j0 = sva::PTransformd::Identity()) const;
 
  MultiBody makeMultiBody(const std::string & rootBodyName,
                          Joint::Type rootJointType,
                          const sva::PTransformd & X_0_j0 = sva::PTransformd::Identity(),
                          const sva::PTransformd & X_b0_j0 = sva::PTransformd::Identity()) const;
 
  MultiBody makeMultiBody(const std::string & rootBodyName,
                          Joint::Type rootJointType,
                          const Eigen::Vector3d & axis,
                          const sva::PTransformd & X_0_j0 = sva::PTransformd::Identity(),
                          const sva::PTransformd & X_b0_j0 = sva::PTransformd::Identity()) const;
 
  void removeJoint(const std::string & rootBodyName, const std::string & jointName);
 
  void removeJoints(const std::string & rootBodyName, const std::vector<std::string> & joints);
 
  void mergeSubBodies(const std::string & rootBodyName,
                      const std::string & jointName,
                      const std::map<std::string, std::vector<double>> & jointPosByName);
 
  std::map<std::string, sva::PTransformd> bodiesBaseTransform(
      const std::string & rootBodyName,
      const sva::PTransformd & X_b0_j0 = sva::PTransformd::Identity());
 
  std::map<std::string, std::vector<std::string>> successorJoints(const std::string & rootBodyName);
 
  std::map<std::string, std::string> predecessorJoint(const std::string & rootBodyName);
 
  static MultiBodyGraph fixJoints(const MultiBodyGraph & other,
                                  const std::vector<std::string> & jointsToFix,
                                  bool fixAllJoints = false);
 
private:
  bool rmArc(Node & node, const std::string & parentJointName, const std::string & jointName);
 
  void rmArcFromMbg(const Arc & arc);
 
  void rmNodeFromMbg(const std::string & jointNameFrom, const std::shared_ptr<Node> & node);
 
  bool findMergeSubNodes(Node & node,
                         const std::string & parentJointName,
                         const std::string & jointName,
                         const std::map<std::string, std::vector<double>> & jointPosByName);
 
  sva::RBInertiad mergeSubNodes(Node & node,
                                const std::string & parentJointName,
                                const std::map<std::string, std::vector<double>> & jointPosByName);
 
  sva::RBInertiad mergeInertia(const sva::RBInertiad & parentInertia,
                               const sva::RBInertiad & childInertia,
                               const Joint & joint,
                               const sva::PTransformd & X_p_j,
                               const std::map<std::string, std::vector<double>> & jointPosByName);
 
  // copy mbg in this. this must be empty before calling this function.
  void copy(const rbd::MultiBodyGraph & mbg);
 
protected:
  std::string rootJointName_;
  std::vector<std::shared_ptr<Node>> nodes_;
  std::vector<std::shared_ptr<Joint>> joints_;
 
  std::map<std::string, std::shared_ptr<Node>> bodyNameToNode_;
  std::map<std::string, std::shared_ptr<Joint>> jointNameToJoint_;
};
 
} // namespace rbd