Main MRPT website > C++ reference for MRPT 1.9.9
CKinematicChain.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef mrpt_CKinematicChain_H
10 #define mrpt_CKinematicChain_H
11 
12 #include <mrpt/poses/CPose3D.h>
16 
17 namespace mrpt
18 {
19 namespace kinematics
20 {
21 /** An individual kinematic chain element (one link) which builds up a
22  * CKinematicChain.
23  * The parameterization of the SE(3) transformation from the starting point to
24  * the end point
25  * follows a Denavit-Hartenberg standard parameterization: [theta, d, a,
26  * alpha].
27  */
29 {
30  /** Rotation from X_i to X_{i+1} (radians) */
31  double theta;
32  /** Distance along Z_i to the common normal between Z_i and Z_{i+1} */
33  double d;
34  /** Distance along the common normal (in the same direction than the new
35  * X_{i+1}) */
36  double a;
37  /** Rotation along X_{i+1} to transform Z_i into Z_{i+1} */
38  double alpha;
39 
40  /** "false": Is revolute ("q_i" is "theta"), "true": is prismatic ("q_i" is
41  * "d") */
43 
45  double _theta, double _d, double _a, double _alpha, bool _is_prismatic)
46  : theta(_theta),
47  d(_d),
48  a(_a),
49  alpha(_alpha),
50  is_prismatic(_is_prismatic)
51  {
52  }
53  TKinematicLink() : theta(0), d(0), a(0), alpha(0), is_prismatic(false) {}
54 };
55 
57  mrpt::serialization::CArchive& in, TKinematicLink& o);
59  mrpt::serialization::CArchive& out, const TKinematicLink& o);
60 
61 /** A open-loop kinematic chain model, suitable to robotic manipulators.
62  * Each link is parameterized with standard Denavit-Hartenberg standard
63  * parameterization [theta, d, a, alpha].
64  *
65  * The orientation of the first link can be modified with setOriginPose(),
66  * which defaults to standard XYZ axes with +Z pointing upwards.
67  *
68  * \sa CPose3D
69  * \ingroup kinematics_grp
70  */
72 {
74 
75  private:
76  /** Smart pointers to the last objects for each link, as returned in
77  * getAs3DObject(), for usage within update3DObject() */
78  mutable std::vector<mrpt::opengl::CRenderizable::Ptr> m_last_gl_objects;
79 
80  /** The links of this robot arm */
81  std::vector<TKinematicLink> m_links;
82  /** The pose of the first link. */
84 
85  public:
86  /** Return the number of links */
87  size_t size() const { return m_links.size(); }
88  /** Erases all links and leave the robot arm empty. */
89  void clear();
90 
91  /** Appends a new link to the robotic arm, with the given Denavit-Hartenberg
92  * parameters (see TKinematicLink for further details) */
93  void addLink(
94  double theta, double d, double a, double alpha, bool is_prismatic);
95 
96  /** Removes one link from the kinematic chain (0<=idx<N) */
97  void removeLink(const size_t idx);
98 
99  /** Get a ref to a given link (read-only) */
100  const TKinematicLink& getLink(const size_t idx) const;
101 
102  /** Get a ref to a given link (read-write) */
103  TKinematicLink& getLinkRef(const size_t idx);
104 
105  /** Can be used to define a first degree of freedom along a +Z axis which
106  * does not coincide with the global +Z axis. */
107  void setOriginPose(const mrpt::poses::CPose3D& new_pose);
108 
109  /** Returns the current pose of the first link. */
110  const mrpt::poses::CPose3D& getOriginPose() const;
111 
112  /** Get all the DOFs of the arm at once, returning them in a vector with all
113  * the "q_i" values, which
114  * can be interpreted as rotations (radians) or displacements (meters)
115  * depending on links being "revolute" or "prismatic".
116  * The vector is automatically resized to the correct size (the number of
117  * links).
118  * \tparam VECTOR Can be any Eigen vector, mrpt::math::CVectorDouble, or
119  * std::vector<double>
120  */
121  template <class VECTOR>
122  void getConfiguration(VECTOR& v) const
123  {
124  const size_t N = m_links.size();
125  v.resize(N);
126  for (size_t i = 0; i < N; i++)
127  {
128  if (m_links[i].is_prismatic)
129  v[i] = m_links[i].d;
130  else
131  v[i] = m_links[i].theta;
132  }
133  }
134 
135  /** Set all the DOFs of the arm at once, from a vector with all the "q_i"
136  * values, which
137  * are interpreted as rotations (radians) or displacements (meters)
138  * depending on links being "revolute" or "prismatic".
139  * \exception std::exception If the size of the vector doesn't match the
140  * number of links.
141  * \tparam VECTOR Can be any Eigen vector, mrpt::math::CVectorDouble, or
142  * std::vector<double>
143  */
144  template <class VECTOR>
145  void setConfiguration(const VECTOR& v)
146  {
147  ASSERT_EQUAL_(v.size(), this->size());
148  const size_t N = m_links.size();
149  for (size_t i = 0; i < N; i++)
150  {
151  if (m_links[i].is_prismatic)
152  m_links[i].d = v[i];
153  else
154  m_links[i].theta = v[i];
155  }
156  }
157 
158  /** Constructs a 3D representation of the kinematic chain, in its current
159  * state.
160  * You can call update3DObject() to update the kinematic state of these
161  * OpenGL objects in the future, since
162  * an internal list of smart pointers to the constructed objects is kept
163  * internally. This is more efficient
164  * than calling this method again to build a new representation.
165  * \param[out] out_all_poses Optional output vector, will contain the poses
166  * in the format of recomputeAllPoses()
167  * \sa update3DObject
168  */
169  void getAs3DObject(
170  mrpt::opengl::CSetOfObjects::Ptr& inout_gl_obj,
172  nullptr) const;
173 
174  /** Read getAs3DObject() for a description.
175  * \param[out] out_all_poses Optional output vector, will contain the poses
176  * in the format of recomputeAllPoses()
177  */
178  void update3DObject(
180  nullptr) const;
181 
182  /** Go thru all the links of the chain and compute the global pose of each
183  * link. The "ground" link pose "pose0" defaults to the origin of
184  * coordinates,
185  * but anything else can be passed as the optional argument.
186  * The returned vector has N+1 elements (N=number of links), since [0]
187  * contains the base frame, [1] the pose after the first link, and so on.
188  */
189  void recomputeAllPoses(
191  const mrpt::poses::CPose3D& pose0 = mrpt::poses::CPose3D()) const;
192 
193 }; // End of class def.
194 
195 } // namespace kinematics
196 
197 // Specialization must occur in the same namespace
198 // (This is to ease serialization)
199 namespace typemeta
200 {
202 } // namespace typemeta
203 
204 } // namespace mrpt
205 
206 #endif
std::vector< TKinematicLink > m_links
The links of this robot arm.
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Declares a typename to be "namespace::type".
Definition: TTypeName.h:115
void recomputeAllPoses(mrpt::aligned_std_vector< mrpt::poses::CPose3D > &poses, const mrpt::poses::CPose3D &pose0=mrpt::poses::CPose3D()) const
Go thru all the links of the chain and compute the global pose of each link.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &inout_gl_obj, mrpt::aligned_std_vector< mrpt::poses::CPose3D > *out_all_poses=nullptr) const
Constructs a 3D representation of the kinematic chain, in its current state.
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, TKinematicLink &o)
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
mrpt::poses::CPose3D m_origin
The pose of the first link.
TKinematicLink & getLinkRef(const size_t idx)
Get a ref to a given link (read-write)
void removeLink(const size_t idx)
Removes one link from the kinematic chain (0<=idx<N)
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
void getConfiguration(VECTOR &v) const
Get all the DOFs of the arm at once, returning them in a vector with all the "q_i" values...
void setConfiguration(const VECTOR &v)
Set all the DOFs of the arm at once, from a vector with all the "q_i" values, which are interpreted a...
void addLink(double theta, double d, double a, double alpha, bool is_prismatic)
Appends a new link to the robotic arm, with the given Denavit-Hartenberg parameters (see TKinematicLi...
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &out, const TKinematicLink &o)
const mrpt::poses::CPose3D & getOriginPose() const
Returns the current pose of the first link.
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
GLuint in
Definition: glext.h:7274
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
void setOriginPose(const mrpt::poses::CPose3D &new_pose)
Can be used to define a first degree of freedom along a +Z axis which does not coincide with the glob...
const TKinematicLink & getLink(const size_t idx) const
Get a ref to a given link (read-only)
A open-loop kinematic chain model, suitable to robotic manipulators.
void clear()
Erases all links and leave the robot arm empty.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
size_t size() const
Return the number of links.
std::vector< mrpt::opengl::CRenderizable::Ptr > m_last_gl_objects
Smart pointers to the last objects for each link, as returned in getAs3DObject(), for usage within up...
void update3DObject(mrpt::aligned_std_vector< mrpt::poses::CPose3D > *out_all_poses=nullptr) const
Read getAs3DObject() for a description.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019