Debian Codegraph @

.deprecated/articulated_model/joint.h

raw
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Dalibor Matura, Jia Pan */
37
38 #ifndef FCL_ARTICULATED_MODEL_JOINT_H
39 #define FCL_ARTICULATED_MODEL_JOINT_H
40
41 #include "fcl/common/data_types.h"
42
43 #include <string>
44 #include <vector>
45 #include <map>
46 #include <limits>
47 #include <memory>
48
49 namespace fcl
50 {
51
52 class JointConfig;
53 class Link;
54
55 enum JointType {JT_UNKNOWN, JT_PRISMATIC, JT_REVOLUTE, JT_BALLEULER};
56
57 /// @brief Base Joint
58 template <typename S>
59 class Joint
60 {
61 public:
62
63 Joint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
64 const Transform3<S>& transform_to_parent,
65 const std::string& name);
66
67 Joint(const std::string& name);
68
69 virtual ~Joint() {}
70
71 const std::string& getName() const;
72 void setName(const std::string& name);
73
74 virtual Transform3<S> getLocalTransform() const = 0;
75
76 virtual std::size_t getNumDofs() const = 0;
77
78 std::shared_ptr<JointConfig> getJointConfig() const;
79 void setJointConfig(const std::shared_ptr<JointConfig>& joint_cfg);
80
81 std::shared_ptr<Link> getParentLink() const;
82 std::shared_ptr<Link> getChildLink() const;
83
84 void setParentLink(const std::shared_ptr<Link>& link);
85 void setChildLink(const std::shared_ptr<Link>& link);
86
87 JointType getJointType() const;
88
89 const Transform3<S>& getTransformToParent() const;
90 void setTransformToParent(const Transform3<S>& t);
91
92 protected:
93
94 /// links to parent and child are only for connection, so weak_ptr to avoid cyclic dependency
95 std::weak_ptr<Link> link_parent_, link_child_;
96
97 JointType type_;
98
99 std::string name_;
100
101 std::shared_ptr<JointConfig> joint_cfg_;
102
103 Transform3<S> transform_to_parent_;
104
105 public:
106 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
107 };
108
109 template <typename S>
110 class PrismaticJoint : public Joint
111 {
112 public:
113 PrismaticJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
114 const Transform3<S>& transform_to_parent,
115 const std::string& name,
116 const Vector3<S>& axis);
117
118 virtual ~PrismaticJoint() {}
119
120 Transform3<S> getLocalTransform() const;
121
122 std::size_t getNumDofs() const;
123
124 const Vector3<S>& getAxis() const;
125
126 protected:
127 Vector3<S> axis_;
128 };
129
130 template <typename S>
131 class RevoluteJoint : public Joint
132 {
133 public:
134 RevoluteJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
135 const Transform3<S>& transform_to_parent,
136 const std::string& name,
137 const Vector3<S>& axis);
138
139 virtual ~RevoluteJoint() {}
140
141 Transform3<S> getLocalTransform() const;
142
143 std::size_t getNumDofs() const;
144
145 const Vector3<S>& getAxis() const;
146
147 protected:
148 Vector3<S> axis_;
149 };
150
151 template <typename S>
152 class BallEulerJoint : public Joint
153 {
154 public:
155 BallEulerJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
156 const Transform3<S>& transform_to_parent,
157 const std::string& name);
158
159 virtual ~BallEulerJoint() {}
160
161 std::size_t getNumDofs() const;
162
163 Transform3<S> getLocalTransform() const;
164 };
165
166 //============================================================================//
167 // //
168 // Implementations //
169 // //
170 //============================================================================//
171
172 //==============================================================================
173 template <typename S>
174 Joint<S>::Joint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
175 const Transform3<S>& transform_to_parent,
176 const std::string& name) :
177 link_parent_(link_parent), link_child_(link_child),
178 name_(name),
179 transform_to_parent_(transform_to_parent)
180 {}
181
182 //==============================================================================
183 template <typename S>
184 Joint<S>::Joint(const std::string& name) :
185 name_(name)
186 {
187 }
188
189 //==============================================================================
190 template <typename S>
191 const std::string& Joint<S>::getName() const
192 {
193 return name_;
194 }
195
196 //==============================================================================
197 template <typename S>
198 void Joint<S>::setName(const std::string& name)
199 {
200 name_ = name;
201 }
202
203 //==============================================================================
204 template <typename S>
205 std::shared_ptr<JointConfig> Joint<S>::getJointConfig() const
206 {
207 return joint_cfg_;
208 }
209
210 //==============================================================================
211 template <typename S>
212 void Joint<S>::setJointConfig(const std::shared_ptr<JointConfig>& joint_cfg)
213 {
214 joint_cfg_ = joint_cfg;
215 }
216
217 //==============================================================================
218 template <typename S>
219 std::shared_ptr<Link> Joint<S>::getParentLink() const
220 {
221 return link_parent_.lock();
222 }
223
224 //==============================================================================
225 template <typename S>
226 std::shared_ptr<Link> Joint<S>::getChildLink() const
227 {
228 return link_child_.lock();
229 }
230
231 //==============================================================================
232 template <typename S>
233 void Joint<S>::setParentLink(const std::shared_ptr<Link>& link)
234 {
235 link_parent_ = link;
236 }
237
238 //==============================================================================
239 template <typename S>
240 void Joint<S>::setChildLink(const std::shared_ptr<Link>& link)
241 {
242 link_child_ = link;
243 }
244
245 //==============================================================================
246 template <typename S>
247 JointType Joint<S>::getJointType() const
248 {
249 return type_;
250 }
251
252 //==============================================================================
253 template <typename S>
254 const Transform3<S>& Joint<S>::getTransformToParent() const
255 {
256 return transform_to_parent_;
257 }
258
259 //==============================================================================
260 template <typename S>
261 void Joint<S>::setTransformToParent(const Transform3<S>& t)
262 {
263 transform_to_parent_ = t;
264 }
265
266 //==============================================================================
267 template <typename S>
268 PrismaticJoint<S>::PrismaticJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
269 const Transform3<S>& transform_to_parent,
270 const std::string& name,
271 const Vector3<S>& axis) :
272 Joint(link_parent, link_child, transform_to_parent, name),
273 axis_(axis)
274 {
275 type_ = JT_PRISMATIC;
276 }
277
278 //==============================================================================
279 template <typename S>
280 const Vector3<S>& PrismaticJoint<S>::getAxis() const
281 {
282 return axis_;
283 }
284
285 //==============================================================================
286 template <typename S>
287 std::size_t PrismaticJoint<S>::getNumDofs() const
288 {
289 return 1;
290 }
291
292 //==============================================================================
293 template <typename S>
294 Transform3<S> PrismaticJoint<S>::getLocalTransform() const
295 {
296 const Quaternion<S> quat(transform_to_parent_.linear());
297 const Vector3<S>& transl = transform_to_parent_.translation();
298
299 Transform3<S> tf = Transform3<S>::Identity();
300 tf.linear() = quat.toRotationMatrix();
301 tf.translation() = quat * (axis_ * (*joint_cfg_)[0]) + transl;
302
303 return tf;
304 }
305
306 //==============================================================================
307 template <typename S>
308 RevoluteJoint<S>::RevoluteJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
309 const Transform3<S>& transform_to_parent,
310 const std::string& name,
311 const Vector3<S>& axis) :
312 Joint(link_parent, link_child, transform_to_parent, name),
313 axis_(axis)
314 {
315 type_ = JT_REVOLUTE;
316 }
317
318 //==============================================================================
319 template <typename S>
320 const Vector3<S>& RevoluteJoint<S>::getAxis() const
321 {
322 return axis_;
323 }
324
325 //==============================================================================
326 template <typename S>
327 std::size_t RevoluteJoint<S>::getNumDofs() const
328 {
329 return 1;
330 }
331
332 //==============================================================================
333 template <typename S>
334 Transform3<S> RevoluteJoint<S>::getLocalTransform() const
335 {
336 Transform3<S> tf = Transform3<S>::Identity();
337 tf.linear() = transform_to_parent_.linear() * AngleAxis<S>((*joint_cfg_)[0], axis_);
338 tf.translation() = transform_to_parent_.translation();
339
340 return tf;
341 }
342
343 //==============================================================================
344 template <typename S>
345 BallEulerJoint<S>::BallEulerJoint(const std::shared_ptr<Link>& link_parent, const std::shared_ptr<Link>& link_child,
346 const Transform3<S>& transform_to_parent,
347 const std::string& name) :
348 Joint(link_parent, link_child, transform_to_parent, name)
349 {}
350
351 //==============================================================================
352 template <typename S>
353 std::size_t BallEulerJoint<S>::getNumDofs() const
354 {
355 return 3;
356 }
357
358 //==============================================================================
359 template <typename S>
360 Transform3<S> BallEulerJoint<S>::getLocalTransform() const
361 {
362 Matrix3<S> rot(
363 AngleAxis<S>((*joint_cfg_)[0], Eigen::Vector3<S>::UnitX())
364 * AngleAxis<S>((*joint_cfg_)[1], Eigen::Vector3<S>::UnitY())
365 * AngleAxis<S>((*joint_cfg_)[2], Eigen::Vector3<S>::UnitZ()));
366
367 Transform3<S> tf = Transform3<S>::Identity();
368 tf.linear() = rot;
369
370 return transform_to_parent_ * tf;
371 }
372
373 } // namespace fcl
374
375 #endif