- Aldebaran documentation |
- C++ Libraries |
- index

libalmath 2.8.7.4

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

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>
22 #include <almath/scenegraph/bodymass.h>
23
24 namespace AL {
25 namespace Math {
26 namespace RigidBodySystemBuilder {
27
28 enum struct JointType { FreeFlyer, Rx, Ry, Rz };
29
30 class Config {
31 public:
32 Config() : no_static_frame(false) {}
33 bool no_static_frame;
34 std::string joint_name_format = "%1%_joint";
35 std::string galilean_frame;
36
37 // map body name name to joint name
38 std::string joint_name(const std::string &body_name) const {
39 return boost::str(boost::format(joint_name_format) % body_name);
40 }
41 };
42
43 template <typename T>
44 struct LinkData {
45 typedef T Scalar;
46
47 typedef Math::BodyMass<Scalar> BodyMass;
48 typedef typename BodyMass::Vector3 Vector3;
49 typedef typename BodyMass::Matrix3 Matrix3;
50 // use Eigen::DontAlign because otherwise, when Scalar is double, Eigen
51 // would require a 16 bytes alignement constraint on LinkData
52 typedef Eigen::Transform<Scalar, 3, Eigen::AffineCompact, Eigen::DontAlign>
53 AffineCompact3;
54 std::string parent_body;
55 std::string new_body;
56 std::string new_joint;
57 AffineCompact3 pose_parent_new;
58 JointType joint_type;
59 BodyMass body_mass;
60
61 template <typename S>
62 LinkData<S> cast() const {
63 return LinkData<S>{parent_body, new_body,
64 new_joint, pose_parent_new.template cast<S>(),
65 joint_type, body_mass.template cast<S>()};
66 }
67 };
68
69 template <typename T>
70 struct StaticFrameData {
71 typedef T Scalar;
72 // use Eigen::DontAlign because otherwise, when Scalar is double, Eigen
73 // would require a 16 bytes alignement constraint on LinkData
74 typedef Eigen::Transform<Scalar, 3, Eigen::AffineCompact, Eigen::DontAlign>
75 AffineCompact3;
76 std::string parent_frame;
77 std::string new_static_frame;
78 std::string new_static_transform;
79 AffineCompact3 pose_parent_new;
80
81 template <typename S>
82 StaticFrameData<S> cast() const {
83 return StaticFrameData<S>{parent_frame, new_static_frame,
84 new_static_transform,
85 pose_parent_new.template cast<S>()};
86 }
87 };
88
89 // Interface for a rigid body system builder.
90 template <typename T>
91 class Interface {
92 public:
93 typedef T Scalar;
94 typedef LinkData<Scalar> Link;
95 typedef typename Link::BodyMass BodyMass;
96 typedef typename BodyMass::Vector3 Vector3;
97 typedef typename BodyMass::Matrix3 Matrix3;
98 typedef StaticFrameData<Scalar> StaticFrame;
99 typedef typename StaticFrame::AffineCompact3 AffineCompact3;
100 virtual ~Interface() {}
101
102 virtual const Config &config() const = 0;
103 virtual void addLink(Link link) = 0;
104 virtual void addStaticFrame(StaticFrame sframe) = 0;
105
106 // add a link
107 void add(const std::string &parent_body, const std::string &new_body,
108 const AffineCompact3 &H_parent_joint, JointType joint_type,
109 const BodyMass &mass, const std::string &new_joint) {
110 addLink(Link{parent_body, new_body, new_joint, H_parent_joint, joint_type,
111 mass});
112 }
113 void add(const std::string &parent_body, const std::string &new_body,
114 const AffineCompact3 &H_parent_joint, JointType joint_type,
115 const BodyMass &mass) {
116 add(parent_body, new_body, H_parent_joint, joint_type, mass,
117 this->config().joint_name(new_body));
118 }
119
120 // add a static frame
121 void add(const std::string &parent_frame,
122 const std::string &new_static_frame,
123 const AffineCompact3 &H_parent_new,
124 const std::string &new_static_transform) {
125 if (this->config().no_static_frame) return;
126 addStaticFrame(StaticFrame{parent_frame, new_static_frame,
127 new_static_transform, H_parent_new});
128 }
129
130 void add(const std::string &parent_frame,
131 const std::string &new_static_frame,
132 const AffineCompact3 &H_parent_new) {
133 add(parent_frame, new_static_frame, H_parent_new,
134 this->config().joint_name(new_static_frame));
135 }
136 };
137
138 #define TYPEDEF_Interface_TYPES \
139 typedef typename Interface<T>::Scalar Scalar; \
140 typedef typename Interface<T>::Link Link; \
141 typedef typename Interface<T>::BodyMass BodyMass; \
142 typedef typename Interface<T>::Vector3 Vector3; \
143 typedef typename Interface<T>::Matrix3 Matrix3; \
144 typedef typename Interface<T>::StaticFrame StaticFrame; \
145 typedef typename Interface<T>::AffineCompact3 AffineCompact3
146
147 // Interface for a rigid body system builder.
148 template <typename T>
149 class Decorator : public Interface<T> {
150 public:
151 TYPEDEF_Interface_TYPES;
152 Decorator(Interface<T> &builder) : _builder(builder) {}
153 virtual ~Decorator() {}
154
155 const Config &config() const { return _builder.config(); }
156
157 virtual void addLink(Link link) { _builder.addLink(link); }
158
159 virtual void addStaticFrame(StaticFrame sframe) {
160 _builder.addStaticFrame(sframe);
161 }
162
163 protected:
164 Interface<T> &_builder;
165 };
166
167 template <typename TD, typename TB>
168 class Adapter : public Interface<TD> {
169 public:
170 Adapter(Interface<TB> &builder) : _builder(builder) {}
171 virtual ~Adapter() {}
172
173 const Config &config() const { return _builder.config(); }
174
175 virtual void addLink(LinkData<TD> link) {
176 _builder.addLink(link.template cast<TB>());
177 }
178
179 virtual void addStaticFrame(StaticFrameData<TD> sframe) {
180 _builder.addStaticFrame(sframe.template cast<TB>());
181 }
182
183 protected:
184 Interface<TB> &_builder;
185 };
186
187 template <typename T>
188 class NamesChecker : public Decorator<T> {
189 public:
190 TYPEDEF_Interface_TYPES;
191
192 NamesChecker(Interface<T> &builder) : Decorator<T>(builder) {}
193
194 void addLink(Link link) {
195 if (!xIsKnownBody(link.parent_body)) {
196 std::stringstream ss;
197 ss << "cannot add body \"" << link.new_body
198 << "\": unknown parent body \"" << link.parent_body << "\"";
199 throw std::runtime_error(ss.str());
200 }
201 if (xIsKnownFrame(link.new_body)) {
202 std::stringstream ss;
203 ss << "cannot add body \"" << link.new_body << "\":"
204 << " there is already a frame with this name";
205 throw std::runtime_error(ss.str());
206 }
207 if (xIsKnownTransform(link.new_joint)) {
208 std::stringstream ss;
209 ss << "cannot add joint \"" + link.new_joint + "\":"
210 << " there is already a transform with this name";
211 throw std::runtime_error(ss.str());
212 }
213 this->_builder.addLink(link);
214 _known_bodies.insert(link.new_body);
215 _known_joints.insert(link.new_joint);
216 }
217
218 void addStaticFrame(StaticFrame sframe) {
219 if (!xIsKnownFrame(sframe.parent_frame)) {
220 throw std::runtime_error(
221 "cannot add static frame \"" + sframe.new_static_frame +
222 "\": unknown parent frame \"" + sframe.parent_frame + "\"");
223 }
224
225 if (xIsKnownFrame(sframe.new_static_frame)) {
226 throw std::runtime_error("cannot add static frame \"" +
227 sframe.new_static_frame +
228 "\": there is already a frame with this name");
229 }
230 if (xIsKnownTransform(sframe.new_static_transform)) {
231 throw std::runtime_error(
232 "cannot add static transform \"" + sframe.new_static_transform +
233 "\": there is already a transform with this name");
234 }
235 this->_builder.addStaticFrame(sframe);
236 _known_sframes.insert(sframe.new_static_frame);
237 _known_stransforms.insert(sframe.new_static_transform);
238 }
239
240 private:
241 // sets of known names. Preloaded with the galilean frame
242 std::set<std::string> _known_bodies =
243 std::set<std::string>({this->_builder.config().galilean_frame});
244 std::set<std::string> _known_joints;
245 std::set<std::string> _known_sframes;
246 std::set<std::string> _known_stransforms;
247
248 bool xIsKnownBody(const std::string &name) const {
249 return _known_bodies.find(name) != _known_bodies.end();
250 }
251
252 // bodies and static frames
253 bool xIsKnownFrame(const std::string &name) const {
254 return xIsKnownBody(name) ||
255 (_known_sframes.find(name) != _known_sframes.end());
256 }
257
258 bool xIsKnownJoint(const std::string &name) const {
259 return _known_joints.find(name) != _known_joints.end();
260 }
261
262 // joints and static transforms
263 bool xIsKnownTransform(const std::string &name) const {
264 return xIsKnownJoint(name) ||
265 (_known_stransforms.find(name) != _known_stransforms.end());
266 }
267 };
268
269 template <typename T>
270 class InertiaEraser : public Decorator<T> {
271 public:
272 TYPEDEF_Interface_TYPES;
273 InertiaEraser(Interface<T> &builder) : Decorator<T>(builder) {}
274 void addLink(Link link) {
275 link.body_mass.rotational_inertia = Matrix3::Zero();
276 this->_builder.addLink(link);
277 }
278 };
279
280 #undef TYPEDEF_Interface_TYPES
281 }
282 }
283 }
284 #endif
AL::Math::RigidBodySystemBuilder::LinkData::new_joint
std::string new_joint
Definition: rigidbodysystembuilder.h:56
AL::Math::RigidBodySystemBuilder::Interface::StaticFrame
StaticFrameData< Scalar > StaticFrame
Definition: rigidbodysystembuilder.h:98
AL::Math::RigidBodySystemBuilder::Interface::add
void add(const std::string &parent_body, const std::string &new_body, const AffineCompact3 &H_parent_joint, JointType joint_type, const BodyMass &mass)
Definition: rigidbodysystembuilder.h:113
AL::Math::RigidBodySystemBuilder::Config::no_static_frame
bool no_static_frame
Definition: rigidbodysystembuilder.h:33
AL::Math::RigidBodySystemBuilder::Interface::Link
LinkData< Scalar > Link
Definition: rigidbodysystembuilder.h:94
AL::Math::RigidBodySystemBuilder::StaticFrameData::AffineCompact3
Eigen::Transform< Scalar, 3, Eigen::AffineCompact, Eigen::DontAlign > AffineCompact3
Definition: rigidbodysystembuilder.h:75
AL::Math::RigidBodySystemBuilder::Decorator::addLink
virtual void addLink(Link link)
Definition: rigidbodysystembuilder.h:157
AL::Math::RigidBodySystemBuilder::Config::joint_name
std::string joint_name(const std::string &body_name) const
Definition: rigidbodysystembuilder.h:38
AL::Math::RigidBodySystemBuilder::Interface::BodyMass
Link::BodyMass BodyMass
Definition: rigidbodysystembuilder.h:95
AL::Math::RigidBodySystemBuilder::LinkData::Matrix3
BodyMass::Matrix3 Matrix3
Definition: rigidbodysystembuilder.h:49
AL::urdf::name
ALMATH_API std::string name(const ptree &pt)
AL::Math::RigidBodySystemBuilder::Interface::~Interface
virtual ~Interface()
Definition: rigidbodysystembuilder.h:100
AL::Math::RigidBodySystemBuilder::Adapter::addLink
virtual void addLink(LinkData< TD > link)
Definition: rigidbodysystembuilder.h:175
AL::Math::RigidBodySystemBuilder::NamesChecker
Definition: rigidbodysystembuilder.h:188
AL::Math::RigidBodySystemBuilder::Decorator::_builder
Interface< T > & _builder
Definition: rigidbodysystembuilder.h:164
AL::Math::RigidBodySystemBuilder::Adapter::addStaticFrame
virtual void addStaticFrame(StaticFrameData< TD > sframe)
Definition: rigidbodysystembuilder.h:179
AL::Math::RigidBodySystemBuilder::StaticFrameData::pose_parent_new
AffineCompact3 pose_parent_new
Definition: rigidbodysystembuilder.h:79
AL::Math::RigidBodySystemBuilder::Config
Definition: rigidbodysystembuilder.h:30
AL::Math::RigidBodySystemBuilder::Decorator::~Decorator
virtual ~Decorator()
Definition: rigidbodysystembuilder.h:153
AL::Math::RigidBodySystemBuilder::Decorator
Definition: rigidbodysystembuilder.h:149
AL::Math::RigidBodySystemBuilder::LinkData::cast
LinkData< S > cast() const
Definition: rigidbodysystembuilder.h:62
AL::Math::RigidBodySystemBuilder::LinkData::joint_type
JointType joint_type
Definition: rigidbodysystembuilder.h:58
AL::Math::RigidBodySystemBuilder::LinkData::AffineCompact3
Eigen::Transform< Scalar, 3, Eigen::AffineCompact, Eigen::DontAlign > AffineCompact3
Definition: rigidbodysystembuilder.h:53
AL::Math::RigidBodySystemBuilder::Adapter
Definition: rigidbodysystembuilder.h:168
AL::Math::RigidBodySystemBuilder::LinkData::body_mass
BodyMass body_mass
Definition: rigidbodysystembuilder.h:59
AL::Math::RigidBodySystemBuilder::Config::joint_name_format
std::string joint_name_format
Definition: rigidbodysystembuilder.h:34
AL::Math::RigidBodySystemBuilder::JointType::Ry
AL::Math::RigidBodySystemBuilder::Decorator::config
const Config & config() const
Definition: rigidbodysystembuilder.h:155
AL::Math::RigidBodySystemBuilder::Interface::add
void add(const std::string &parent_frame, const std::string &new_static_frame, const AffineCompact3 &H_parent_new, const std::string &new_static_transform)
Definition: rigidbodysystembuilder.h:121
AL::Math::RigidBodySystemBuilder::LinkData::pose_parent_new
AffineCompact3 pose_parent_new
Definition: rigidbodysystembuilder.h:57
AL::Math::RigidBodySystemBuilder::Interface::Scalar
T Scalar
Definition: rigidbodysystembuilder.h:93
AL::Math::RigidBodySystemBuilder::StaticFrameData::new_static_transform
std::string new_static_transform
Definition: rigidbodysystembuilder.h:78
AL::Math::RigidBodySystemBuilder::InertiaEraser::addLink
void addLink(Link link)
Definition: rigidbodysystembuilder.h:274
AL::Math::RigidBodySystemBuilder::NamesChecker::addStaticFrame
void addStaticFrame(StaticFrame sframe)
Definition: rigidbodysystembuilder.h:218
AL::Math::RigidBodySystemBuilder::StaticFrameData
Definition: rigidbodysystembuilder.h:70
AL::Math::RigidBodySystemBuilder::Interface::add
void add(const std::string &parent_body, const std::string &new_body, const AffineCompact3 &H_parent_joint, JointType joint_type, const BodyMass &mass, const std::string &new_joint)
Definition: rigidbodysystembuilder.h:107
AL::Math::RigidBodySystemBuilder::JointType::Rz
AL::Math::RigidBodySystemBuilder::Adapter::Adapter
Adapter(Interface< TB > &builder)
Definition: rigidbodysystembuilder.h:170
AL::Math::RigidBodySystemBuilder::LinkData::parent_body
std::string parent_body
Definition: rigidbodysystembuilder.h:54
AL::Math::RigidBodySystemBuilder::Interface::Matrix3
BodyMass::Matrix3 Matrix3
Definition: rigidbodysystembuilder.h:97
AL::Math::RigidBodySystemBuilder::JointType::FreeFlyer
bodymass.h
AL::Math::RigidBodySystemBuilder::JointType::Rx
AL::Math::RigidBodySystemBuilder::Adapter::~Adapter
virtual ~Adapter()
Definition: rigidbodysystembuilder.h:171
AL::Math::RigidBodySystemBuilder::NamesChecker::addLink
void addLink(Link link)
Definition: rigidbodysystembuilder.h:194
AL::Math::RigidBodySystemBuilder::Decorator::TYPEDEF_Interface_TYPES
TYPEDEF_Interface_TYPES
Definition: rigidbodysystembuilder.h:151
AL::Math::RigidBodySystemBuilder::Interface::Vector3
BodyMass::Vector3 Vector3
Definition: rigidbodysystembuilder.h:96
AL::Math::RigidBodySystemBuilder::Decorator::Decorator
Decorator(Interface< T > &builder)
Definition: rigidbodysystembuilder.h:152
AL::Math::RigidBodySystemBuilder::Adapter::config
const Config & config() const
Definition: rigidbodysystembuilder.h:173
AL::Math::RigidBodySystemBuilder::Interface
Definition: rigidbodysystembuilder.h:91
AL::Math::RigidBodySystemBuilder::StaticFrameData::parent_frame
std::string parent_frame
Definition: rigidbodysystembuilder.h:76
AL::Math::BodyMass< Scalar >
AL::Math::BodyMass< Scalar >::Matrix3
Eigen::Matrix< Scalar, 3, 3, Eigen::DontAlign > Matrix3
Definition: bodymass.h:49
AL::Math::BodyMass< Scalar >::Vector3
Eigen::Matrix< Scalar, 3, 1, Eigen::DontAlign > Vector3
Definition: bodymass.h:48
AL::Math::RigidBodySystemBuilder::Adapter::_builder
Interface< TB > & _builder
Definition: rigidbodysystembuilder.h:184
AL::Math::RigidBodySystemBuilder::Config::Config
Config()
Definition: rigidbodysystembuilder.h:32
AL::Math::RigidBodySystemBuilder::Decorator::addStaticFrame
virtual void addStaticFrame(StaticFrame sframe)
Definition: rigidbodysystembuilder.h:159
AL::Math::RigidBodySystemBuilder::LinkData
Definition: rigidbodysystembuilder.h:44
AL::Math::RigidBodySystemBuilder::Config::galilean_frame
std::string galilean_frame
Definition: rigidbodysystembuilder.h:35
AL::Math::RigidBodySystemBuilder::NamesChecker::NamesChecker
NamesChecker(Interface< T > &builder)
Definition: rigidbodysystembuilder.h:192
AL::Math::RigidBodySystemBuilder::StaticFrameData::Scalar
T Scalar
Definition: rigidbodysystembuilder.h:71
AL::Math::RigidBodySystemBuilder::Interface::add
void add(const std::string &parent_frame, const std::string &new_static_frame, const AffineCompact3 &H_parent_new)
Definition: rigidbodysystembuilder.h:130
AL::Math::RigidBodySystemBuilder::LinkData::Scalar
T Scalar
Definition: rigidbodysystembuilder.h:45
AL::Math::RigidBodySystemBuilder::InertiaEraser::InertiaEraser
InertiaEraser(Interface< T > &builder)
Definition: rigidbodysystembuilder.h:273
AL::Math::RigidBodySystemBuilder::InertiaEraser
Definition: rigidbodysystembuilder.h:270
AL::Math::RigidBodySystemBuilder::InertiaEraser::TYPEDEF_Interface_TYPES
TYPEDEF_Interface_TYPES
Definition: rigidbodysystembuilder.h:272
AL::Math::RigidBodySystemBuilder::LinkData::BodyMass
Math::BodyMass< Scalar > BodyMass
Definition: rigidbodysystembuilder.h:47
AL::Math::RigidBodySystemBuilder::StaticFrameData::cast
StaticFrameData< S > cast() const
Definition: rigidbodysystembuilder.h:82
AL::Math::RigidBodySystemBuilder::JointType
JointType
Definition: rigidbodysystembuilder.h:28
AL::Math::RigidBodySystemBuilder::NamesChecker::TYPEDEF_Interface_TYPES
TYPEDEF_Interface_TYPES
Definition: rigidbodysystembuilder.h:190
AL::Math::RigidBodySystemBuilder::LinkData::Vector3
BodyMass::Vector3 Vector3
Definition: rigidbodysystembuilder.h:48
AL::Math::RigidBodySystemBuilder::Interface::AffineCompact3
StaticFrame::AffineCompact3 AffineCompact3
Definition: rigidbodysystembuilder.h:99
AL::Math::BodyMass::rotational_inertia
Matrix3 rotational_inertia
Definition: bodymass.h:56
AL::Math::RigidBodySystemBuilder::LinkData::new_body
std::string new_body
Definition: rigidbodysystembuilder.h:55
AL::Math::RigidBodySystemBuilder::StaticFrameData::new_static_frame
std::string new_static_frame
Definition: rigidbodysystembuilder.h:77