rigid_body.h

 
#ifndef SPATIAL_DYN_STRUCTS_RIGID_BODY_H_
#define SPATIAL_DYN_STRUCTS_RIGID_BODY_H_
 
#include <ostream>  // std::ostream
#include <string>   // std::string
#include <vector>   // std::vector
 
#include "spatial_dyn/eigen/spatial_math.h"
#include "spatial_dyn/structs/graphics.h"
#include "spatial_dyn/structs/joint.h"
 
namespace spatial_dyn {
 
class ArticulatedBody;
 
class RigidBody {
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  RigidBody() = default;
  virtual ~RigidBody() = default;
 
  RigidBody(const std::string& name) : name(name) {}
 
  mutable std::string name;
 
  mutable std::vector<Graphics> graphics;
 
  int id() const { return id_; }
  void set_id(int id) { id_ = id; }
 
  int id_parent() const { return id_parent_; }
  void set_id_parent(int id_parent) { id_parent_ = id_parent; }
 
  const Eigen::Isometry3d& T_to_parent() const { return T_to_parent_; }
 
  template<typename Derived>
  void set_T_to_parent(const Eigen::RotationBase<Derived,3>& ori_in_parent,
                       const Eigen::Vector3d& pos_in_parent) {
    T_to_parent_ = Eigen::Translation3d(pos_in_parent) * ori_in_parent;
  }
 
  void set_T_to_parent(const Eigen::Isometry3d& T_to_parent) { T_to_parent_ = T_to_parent; }
 
  const SpatialInertiad& inertia() const { return inertia_; }
 
  void set_inertia(double mass, const Eigen::Vector3d& com, const Eigen::Vector6d& I_com_flat);
 
  void set_inertia(const SpatialInertiad& inertia);
 
  const Joint& joint() const { return joint_; }
 
  void set_joint(const Joint& joint) { joint_ = joint; }
 
 protected:
 
  int id_ = -1;
  int id_parent_ = -1;
  Eigen::Isometry3d T_to_parent_ = Eigen::Isometry3d::Identity();
  SpatialInertiad inertia_ = SpatialInertiad(1, Eigen::Vector3d::Zero(), Eigen::Vector6d::Zero());
  Joint joint_;
 
  friend class ArticulatedBody;
 
};
 
std::ostream& operator<<(std::ostream& os, const RigidBody& rb);
 
}  // namespace spatial_dyn
 
#endif  // SPATIAL_DYN_STRUCTS_RIGID_BODY_H_