libalmath  2.5.7.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
rigidbodysystembuilder.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Aldebaran. All rights reserved.
3  *
4  */
5 
6 // Interface for a RigidBodySystemBuilder (as in the "builder design pattern")
7 // Implement this interface in order to initialize a rigid bodies simulator
8 // or to export a rigid body system in some description format.
9 //
10 // When converting from a description format to another, a
11 // RigidBodySystemBuilder specific to the output format, is often used with
12 // a "kinematic tree traverser", which is specific to the input format.
13 
14 #ifndef LIB_ALMATH_SCENEGRAPH_RIGIDBODYSYSTEMBUILDER_H
15 #define LIB_ALMATH_SCENEGRAPH_RIGIDBODYSYSTEMBUILDER_H
16 
17 #include <string>
18 #include <set>
19 #include <Eigen/Geometry>
20 #include <stdexcept>
21 #include <boost/format.hpp>
23 
24 namespace AL {
25 namespace RigidBodySystemBuilder {
26 
27 enum struct JointType { FreeFlyer, Rx, Ry, Rz };
28 
29 class Config {
30  public:
31  Config() : no_static_frame(false) {}
33  std::string joint_name_format = "%1%_joint";
34  std::string galilean_frame;
35 
36  // map body name name to joint name
37  std::string joint_name(const std::string &body_name) const {
38  return boost::str(boost::format(joint_name_format) % body_name);
39  }
40 };
41 
42 template <typename T>
43 struct LinkData {
44  typedef T Scalar;
45  typedef Eigen::Transform<T, 3, Eigen::AffineCompact, Eigen::DontAlign> Pose;
47  typedef typename BodyMass::Vector3 Vector3;
48  typedef typename BodyMass::Matrix3 Matrix3;
49 
50  std::string parent_body;
51  std::string new_body;
52  std::string new_joint;
56 
57  template <typename S>
58  LinkData<S> cast() const {
60  new_joint, pose_parent_new.template cast<S>(),
61  joint_type, body_mass.template cast<S>()};
62  }
63 };
64 
65 template <typename T>
67  typedef T Scalar;
68  typedef Eigen::Transform<T, 3, Eigen::AffineCompact, Eigen::DontAlign> Pose;
69 
70  std::string parent_frame;
71  std::string new_static_frame;
72  std::string new_static_transform;
74 
75  template <typename S>
79  pose_parent_new.template cast<S>()};
80  }
81 };
82 
83 // Interface for a rigid body system builder.
84 template <typename T>
85 class Interface {
86  public:
87  typedef T Scalar;
89  typedef typename Link::BodyMass BodyMass;
90  typedef typename BodyMass::Vector3 Vector3;
91  typedef typename BodyMass::Matrix3 Matrix3;
93  typedef typename StaticFrame::Pose Pose;
94  virtual ~Interface() {}
95 
96  virtual const Config &config() const = 0;
97  virtual void addLink(Link link) = 0;
98  virtual void addStaticFrame(StaticFrame sframe) = 0;
99 
100  // add a link
101  void add(const std::string &parent_body, const std::string &new_body,
102  const Pose &H_parent_joint, JointType joint_type,
103  const BodyMass &mass, const std::string &new_joint) {
104  addLink(Link{parent_body, new_body, new_joint, H_parent_joint, joint_type,
105  mass});
106  }
107  void add(const std::string &parent_body, const std::string &new_body,
108  const Pose &H_parent_joint, JointType joint_type,
109  const BodyMass &mass) {
110  add(parent_body, new_body, H_parent_joint, joint_type, mass,
111  this->config().joint_name(new_body));
112  }
113 
114  // add a static frame
115  void add(const std::string &parent_frame, const std::string &new_static_frame,
116  const Pose &H_parent_new, const std::string &new_static_transform) {
117  if (this->config().no_static_frame) return;
118  addStaticFrame(StaticFrame{parent_frame, new_static_frame,
119  new_static_transform, H_parent_new});
120  }
121 
122  void add(const std::string &parent_frame, const std::string &new_static_frame,
123  const Pose &H_parent_new) {
124  add(parent_frame, new_static_frame, H_parent_new,
125  this->config().joint_name(new_static_frame));
126  }
127 };
128 
129 #define TYPEDEF_Interface_TYPES \
130  typedef typename Interface<T>::Scalar Scalar; \
131  typedef typename Interface<T>::Link Link; \
132  typedef typename Interface<T>::BodyMass BodyMass; \
133  typedef typename Interface<T>::Vector3 Vector3; \
134  typedef typename Interface<T>::Matrix3 Matrix3; \
135  typedef typename Interface<T>::StaticFrame StaticFrame; \
136  typedef typename Interface<T>::Pose Pose
137 
138 // Interface for a rigid body system builder.
139 template <typename T>
140 class Decorator : public Interface<T> {
141  public:
143  Decorator(Interface<T> &builder) : _builder(builder) {}
144  virtual ~Decorator() {}
145 
146  const Config &config() const { return _builder.config(); }
147 
148  virtual void addLink(Link link) { _builder.addLink(link); }
149 
150  virtual void addStaticFrame(StaticFrame sframe) {
151  _builder.addStaticFrame(sframe);
152  }
153 
154  protected:
156 };
157 
158 template <typename TD, typename TB>
159 class Adapter : public Interface<TD> {
160  public:
161  Adapter(Interface<TB> &builder) : _builder(builder) {}
162  virtual ~Adapter() {}
163 
164  const Config &config() const { return _builder.config(); }
165 
166  virtual void addLink(LinkData<TD> link) {
167  _builder.addLink(link.template cast<TB>());
168  }
169 
170  virtual void addStaticFrame(StaticFrameData<TD> sframe) {
171  _builder.addStaticFrame(sframe.template cast<TB>());
172  }
173 
174  protected:
176 };
177 
178 template <typename T>
179 class NamesChecker : public Decorator<T> {
180  public:
182 
183  NamesChecker(Interface<T> &builder) : Decorator<T>(builder) {}
184 
185  void addLink(Link link) {
186  if (!xIsKnownBody(link.parent_body)) {
187  std::stringstream ss;
188  ss << "cannot add body \"" << link.new_body
189  << "\": unknown parent body \"" << link.parent_body << "\"";
190  throw std::runtime_error(ss.str());
191  }
192  if (xIsKnownFrame(link.new_body)) {
193  std::stringstream ss;
194  ss << "cannot add body \"" << link.new_body << "\":"
195  << " there is already a frame with this name";
196  throw std::runtime_error(ss.str());
197  }
198  if (xIsKnownTransform(link.new_joint)) {
199  std::stringstream ss;
200  ss << "cannot add joint \"" + link.new_joint + "\":"
201  << " there is already a transform with this name";
202  throw std::runtime_error(ss.str());
203  }
204  this->_builder.addLink(link);
205  _known_bodies.insert(link.new_body);
206  _known_joints.insert(link.new_joint);
207  }
208 
210  if (!xIsKnownFrame(sframe.parent_frame)) {
211  throw std::runtime_error(
212  "cannot add static frame \"" + sframe.new_static_frame +
213  "\": unknown parent frame \"" + sframe.parent_frame + "\"");
214  }
215 
216  if (xIsKnownFrame(sframe.new_static_frame)) {
217  throw std::runtime_error("cannot add static frame \"" +
218  sframe.new_static_frame +
219  "\": there is already a frame with this name");
220  }
221  if (xIsKnownTransform(sframe.new_static_transform)) {
222  throw std::runtime_error(
223  "cannot add static transform \"" + sframe.new_static_transform +
224  "\": there is already a transform with this name");
225  }
226  this->_builder.addStaticFrame(sframe);
227  _known_sframes.insert(sframe.new_static_frame);
228  _known_stransforms.insert(sframe.new_static_transform);
229  }
230 
231  private:
232  // sets of known names. Preloaded with the galilean frame
233  std::set<std::string> _known_bodies =
234  std::set<std::string>({this->_builder.config().galilean_frame});
235  std::set<std::string> _known_joints;
236  std::set<std::string> _known_sframes;
237  std::set<std::string> _known_stransforms;
238 
239  bool xIsKnownBody(const std::string &name) const {
240  return _known_bodies.find(name) != _known_bodies.end();
241  }
242 
243  // bodies and static frames
244  bool xIsKnownFrame(const std::string &name) const {
245  return xIsKnownBody(name) ||
246  (_known_sframes.find(name) != _known_sframes.end());
247  }
248 
249  bool xIsKnownJoint(const std::string &name) const {
250  return _known_joints.find(name) != _known_joints.end();
251  }
252 
253  // joints and static transforms
254  bool xIsKnownTransform(const std::string &name) const {
255  return xIsKnownJoint(name) ||
256  (_known_stransforms.find(name) != _known_stransforms.end());
257  }
258 };
259 
260 template <typename T>
261 class InertiaEraser : public Decorator<T> {
262  public:
264  InertiaEraser(Interface<T> &builder) : Decorator<T>(builder) {}
265  void addLink(Link link) {
266  link.body_mass.rotational_inertia = Matrix3::Zero();
267  this->_builder.addLink(link);
268  }
269 };
270 
271 #undef TYPEDEF_Interface_TYPES
272 }
273 }
274 
275 #endif
virtual void addStaticFrame(StaticFrameData< TD > sframe)
void add(const std::string &parent_body, const std::string &new_body, const Pose &H_parent_joint, JointType joint_type, const BodyMass &mass, const std::string &new_joint)
Eigen::Transform< T, 3, Eigen::AffineCompact, Eigen::DontAlign > Pose
std::string joint_name(const std::string &body_name) const
void add(const std::string &parent_body, const std::string &new_body, const Pose &H_parent_joint, JointType joint_type, const BodyMass &mass)
std::string name(const ptree &pt)
virtual void addLink(LinkData< TD > link)
void add(const std::string &parent_frame, const std::string &new_static_frame, const Pose &H_parent_new)
virtual void addStaticFrame(StaticFrame sframe)
Eigen::Transform< T, 3, Eigen::AffineCompact, Eigen::DontAlign > Pose
Eigen::Matrix< Scalar, 3, 3, Eigen::DontAlign > Matrix3
Definition: bodymass.h:18
Eigen::Matrix< Scalar, 3, 1, Eigen::DontAlign > Vector3
Definition: bodymass.h:17
void add(const std::string &parent_frame, const std::string &new_static_frame, const Pose &H_parent_new, const std::string &new_static_transform)
Matrix3 rotational_inertia
Definition: bodymass.h:25