Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
Kinematics.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of the Technische Universitaet Muenchen nor the names of
14 // its contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifndef _RL_KIN_KINEMATICS_H_
31 #define _RL_KIN_KINEMATICS_H_
32 
33 #include <string>
34 #include <vector>
35 #include <boost/shared_ptr.hpp>
36 #include <boost/graph/adjacency_list.hpp>
37 #include <rl/math/Transform.h>
38 #include <rl/math/Vector.h>
39 
40 namespace rl
41 {
42  namespace kin
43  {
44  class Element;
45  class Frame;
46  class Link;
47  class Joint;
48  class Transform;
49 
50  class Kinematics
51  {
52  public:
53  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54 
55  enum Type
56  {
59  };
60 
61  Kinematics();
62 
63  virtual ~Kinematics();
64 
68  bool areColliding(const ::std::size_t& i, const ::std::size_t& j) const;
69 
75  virtual void clip(::rl::math::Vector& q) const;
76 
77  virtual Kinematics* clone() const;
78 
79  static Kinematics* create(const ::std::string& filename);
80 
88 
96  virtual const ::rl::math::Transform& forwardPosition(const ::std::size_t& i = 0) const;
97 
109  virtual void forwardForce(const ::rl::math::Vector& tau, ::rl::math::Vector& f) const;
110 
121  virtual void forwardVelocity(const ::rl::math::Vector& qdot, ::rl::math::Vector& xdot) const;
122 
126  ::std::size_t getBodies() const;
127 
131  ::std::size_t getDof() const;
132 
138  const ::rl::math::Transform& getFrame(const ::std::size_t& i) const;
139 
146 
153 
160 
161  ::std::string getManufacturer() const;
162 
163  ::rl::math::Real getMaximum(const ::std::size_t& i) const;
164 
165  void getMaximum(::rl::math::Vector& max) const;
166 
167  ::rl::math::Real getMinimum(const ::std::size_t& i) const;
168 
169  void getMinimum(::rl::math::Vector& min) const;
170 
171  ::std::string getName() const;
172 
176  ::std::size_t getOperationalDof() const;
177 
183  void getPosition(::rl::math::Vector& q) const;
184 
185  ::rl::math::Real getSpeed(const ::std::size_t& i) const;
186 
187  void getSpeed(::rl::math::Vector& speed) const;
188 
189  Type getType(const ::std::size_t& i);
190 
192 
203  virtual void inverseForce(const ::rl::math::Vector& f, ::rl::math::Vector& tau) const;
204 
206 
219  virtual bool inversePosition(const ::rl::math::Transform& x, ::rl::math::Vector& q, const ::std::size_t& iterations = 1000);
220 
231  virtual void inverseVelocity(const ::rl::math::Vector& xdot, ::rl::math::Vector& qdot) const;
232 
236  bool isColliding(const ::std::size_t& i) const;
237 
241  virtual bool isSingular() const;
242 
248  virtual bool isValid(const ::rl::math::Vector& q) const;
249 
251 
253 
255 
256  virtual ::rl::math::Real newDistance(const ::rl::math::Real& dist, const ::rl::math::Real& oldOff, const ::rl::math::Real& newOff, const int& cuttingDimension) const;
257 
261  void setColliding(const ::std::size_t& i, const bool& doesCollide);
262 
266  void setColliding(const ::std::size_t& i, const ::std::size_t& j, const bool& doCollide);
267 
274 
275  virtual void step(const ::rl::math::Vector& q1, const ::rl::math::Vector& qdot, ::rl::math::Vector& q2) const;
276 
277  ::rl::math::Transform& tool(const ::std::size_t& i = 0);
278 
279  const ::rl::math::Transform& tool(const ::std::size_t& i = 0) const;
280 
282 
284 
292  virtual void updateFrames();
293 
301  virtual void updateJacobian();
302 
317  virtual void updateJacobianInverse(const ::rl::math::Real& lambda = 0.0f, const bool& doSvd = true);
318 
320 
322 
323  protected:
324  typedef ::boost::adjacency_list<
325  ::boost::vecS,
326  ::boost::vecS,
327  ::boost::bidirectionalS,
328  ::boost::shared_ptr< Frame >,
329  ::boost::shared_ptr< Transform >,
330  ::boost::no_property,
331  ::boost::vecS
332  > Tree;
333 
334  typedef ::boost::graph_traits< Tree >::edge_descriptor Edge;
335 
336  typedef ::boost::graph_traits< Tree >::edge_iterator EdgeIterator;
337 
338  typedef ::std::pair< EdgeIterator, EdgeIterator > EdgeIteratorPair;
339 
340  typedef ::boost::graph_traits< Tree >::in_edge_iterator InEdgeIterator;
341 
342  typedef ::std::pair< InEdgeIterator, InEdgeIterator > InEdgeIteratorPair;
343 
344  typedef ::boost::graph_traits< Tree >::out_edge_iterator OutEdgeIterator;
345 
346  typedef ::std::pair< OutEdgeIterator, OutEdgeIterator > OutEdgeIteratorPair;
347 
348  typedef ::boost::graph_traits< Tree >::vertex_descriptor Vertex;
349 
350  typedef ::boost::graph_traits< Tree >::vertex_iterator VertexIterator;
351 
352  typedef ::std::pair< VertexIterator, VertexIterator > VertexIteratorPair;
353 
354  void update();
355 
356  void update(Vertex& u);
357 
358  ::std::vector< Element* > elements;
359 
360  ::std::vector< Frame* > frames;
361 
362  ::std::vector< Vertex > leaves;
363 
364  ::std::vector< Link* > links;
365 
367 
369 
370  ::std::vector< Joint* > joints;
371 
372  ::std::string manufacturer;
373 
374  ::std::string name;
375 
377 
378  ::std::vector< Edge > tools;
379 
380  ::std::vector< Transform* > transforms;
381 
383 
384  private:
385 
386  };
387  }
388 }
389 
390 #endif // _RL_KIN_KINEMATICS_H_