articulated_body.h

 
#ifndef SPATIAL_DYN_STRUCTS_ARTICULATED_BODY_H_
#define SPATIAL_DYN_STRUCTS_ARTICULATED_BODY_H_
 
#include <functional>  // std::function
#include <map>         // std::map
#include <memory>      // std::unique_ptr
#include <ostream>     // std::ostream
#include <string>      // std::string
#include <utility>     // std::pair
#include <vector>      // std::vector
 
#include "spatial_dyn/eigen/spatial_math.h"
#include "spatial_dyn/structs/rigid_body.h"
 
namespace spatial_dyn {
 
class ArticulatedBody {
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  ArticulatedBody();
 
  ArticulatedBody(const std::string& name);
 
  ArticulatedBody(const ArticulatedBody& ab);
 
  ArticulatedBody(ArticulatedBody&& ab);
 
  virtual ~ArticulatedBody();
 
  ArticulatedBody& operator=(const ArticulatedBody& ab);
 
  // ArticulatedBody& operator=(ArticulatedBody&& ab) = default;
 
  mutable std::string name;
 
  mutable std::vector<Graphics> graphics;
 
  size_t dof() const { return dof_; };
 
  int AddRigidBody(const RigidBody& rb, int id_parent = -1);
 
  const std::vector<RigidBody>& rigid_bodies() const { return rigid_bodies_; };
 
  const RigidBody& rigid_bodies(int i) const;
 
  const Eigen::VectorXd& q() const { return q_; };
 
  double q(int i) const;
 
  virtual void set_q(Eigen::Ref<const Eigen::VectorXd> q);
 
  const Eigen::VectorXd& dq() const { return dq_; };
 
  double dq(int i) const;
 
  virtual void set_dq(Eigen::Ref<const Eigen::VectorXd> dq);
 
  const SpatialMotiond& g() const { return g_; };
 
  void set_g(const Eigen::Vector3d& g);
 
  virtual void AddLoad(const SpatialInertiad& inertia, int idx_link = -1);
 
  virtual void ReplaceLoad(const SpatialInertiad& inertia, int idx_link = -1);
 
  virtual void ClearLoad(int idx_link = -1);
 
  const std::map<size_t, SpatialInertiad>& inertia_load() const { return inertia_load_; }
 
  const Eigen::Isometry3d& T_base_to_world() const { return T_base_to_world_; };
 
  template<typename Derived>
  void set_T_base_to_world(const Eigen::RotationBase<Derived,3>& ori_in_world,
                           const Eigen::Vector3d& pos_in_world) {
    T_base_to_world_ = Eigen::Translation3d(pos_in_world) * ori_in_world;
  }
 
  void set_T_base_to_world(const Eigen::Isometry3d& T_to_world);
 
  const SpatialInertiad& inertia_base() const { return inertia_base_; }
 
  void set_inertia_base(double mass, const Eigen::Vector3d& com, const Eigen::Vector6d& I_com_flat);
 
  void set_inertia_base(const SpatialInertiad& inertia);
 
  const Eigen::Isometry3d& T_to_parent(int i) const;
 
  Eigen::Isometry3d T_to_parent(int i, double q) const;
 
  Eigen::Isometry3d T_from_parent(int i) const { return T_to_parent(i).inverse(); };
 
  Eigen::Isometry3d T_from_parent(int i, double q) const { return T_to_parent(i, q).inverse(); };
 
  const Eigen::Isometry3d& T_to_world(int i) const;
 
  Eigen::Isometry3d T_to_world(int i, Eigen::Ref<const Eigen::VectorXd> q) const;
 
  Eigen::Isometry3d T_from_world(int i) const { return T_to_world(i).inverse(); };
 
  Eigen::Isometry3d T_from_world(int i, Eigen::Ref<const Eigen::VectorXd> q) const {
    return T_to_world(i, q).inverse();
  };
 
  const std::vector<int>& ancestors(int i) const;
 
  const std::vector<int>& subtree(int i) const;
 
  Eigen::VectorXd Map(const std::function<double(const RigidBody& rb)>& rb_function) const;
 
  struct Cache;                   // Defined in articulated_body_cache.h
  std::unique_ptr<Cache> cache_;  // Pointer to cache for internal use
 
 protected:
 
  void ExpandDof(int id, int id_parent);
 
  size_t dof_ = 0;
  std::vector<RigidBody> rigid_bodies_;
 
  Eigen::VectorXd q_;
  Eigen::VectorXd dq_;
  SpatialMotiond g_ = -9.81 * SpatialMotiond::UnitLinZ();
 
  std::map<size_t, SpatialInertiad> inertia_load_;
 
  Eigen::Isometry3d T_base_to_world_ = Eigen::Isometry3d::Identity();
  SpatialInertiad inertia_base_ = SpatialInertiad(1, Eigen::Vector3d::Zero(), Eigen::Vector6d::Zero());
 
  std::vector<std::vector<int>> ancestors_;  // Ancestors from base to link i
  std::vector<std::vector<int>> subtrees_;   // Descendants of link i including link i
 
};
 
std::ostream& operator<<(std::ostream& os, const ArticulatedBody& ab);
 
}  // namespace spatial_dyn
 
#endif  // SPATIAL_DYN_STRUCTS_ARTICULATED_BODY_H_