Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, Individual contributors, see AUTHORS file |
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
18
19 namespace mrpt
20 {
21
22  namespace kinematics
23  {
25
26  /** An individual kinematic chain element (one link) which builds up a CKinematicChain.
27  * The parameterization of the SE(3) transformation from the starting point to the end point
28  * follows a Denavit-Hartenberg standard parameterization: [theta, d, a, alpha].
29  */
31  {
32  double theta; //!< Rotation from X_i to X_{i+1} (radians)
33  double d; //!< Distance along Z_i to the common normal between Z_i and Z_{i+1}
34  double a; //!< Distance along the common normal (in the same direction than the new X_{i+1})
35  double alpha; //!< Rotation along X_{i+1} to transform Z_i into Z_{i+1}
36
37  bool is_prismatic; //!< "false": Is revolute ("q_i" is "theta"), "true": is prismatic ("q_i" is "d")
38
39  TKinematicLink(double _theta,double _d, double _a, double _alpha, bool _is_prismatic) : theta(_theta),d(_d),a(_a),alpha(_alpha),is_prismatic(_is_prismatic) {}
40  TKinematicLink() : theta(0),d(0),a(0),alpha(0),is_prismatic(false) { }
41  };
42
45
46  /** A open-loop kinematic chain model, suitable to robotic manipulators.
47  * Each link is parameterized with standard Denavit-Hartenberg standard parameterization [theta, d, a, alpha].
48  *
49  * The orientation of the first link can be modified with setOriginPose(), which defaults to standard XYZ axes with +Z pointing upwards.
50  *
51  * \sa CPose3D
52  * \ingroup kinematics_grp
53  */
55  {
56  // This must be added to any CSerializable derived class:
58
59  private:
60  mutable std::vector<mrpt::opengl::CRenderizablePtr> m_last_gl_objects; //!< Smart pointers to the last objects for each link, as returned in getAs3DObject(), for usage within update3DObject()
61
62  std::vector<TKinematicLink> m_links; //!< The links of this robot arm
63  mrpt::poses::CPose3D m_origin; //!< The pose of the first link.
64
65  public:
66
67  /** Return the number of links */
68  size_t size() const { return m_links.size(); }
69
70  /** Erases all links and leave the robot arm empty. */
71  void clear();
72
73  /** Appends a new link to the robotic arm, with the given Denavit-Hartenberg parameters (see TKinematicLink for further details) */
74  void addLink(double theta, double d, double a, double alpha, bool is_prismatic);
75
76  /** Removes one link from the kinematic chain (0<=idx<N) */
77  void removeLink(const size_t idx);
78
79  /** Get a ref to a given link (read-only) */
80  const TKinematicLink& getLink(const size_t idx) const;
81
82  /** Get a ref to a given link (read-write) */
84
85  /** Can be used to define a first degree of freedom along a +Z axis which does not coincide with the global +Z axis. */
86  void setOriginPose(const mrpt::poses::CPose3D &new_pose);
87
88  /** Returns the current pose of the first link. */
89  const mrpt::poses::CPose3D & getOriginPose() const;
90
91  /** Get all the DOFs of the arm at once, returning them in a vector with all the "q_i" values, which
92  * can be interpreted as rotations (radians) or displacements (meters) depending on links being "revolute" or "prismatic".
93  * The vector is automatically resized to the correct size (the number of links).
94  * \tparam VECTOR Can be any Eigen vector, mrpt::math::CVectorDouble, or std::vector<double>
95  */
96  template <class VECTOR>
97  void getConfiguration(VECTOR &v) const
98  {
99  const size_t N=m_links.size();
100  v.resize(N);
101  for (size_t i=0;i<N;i++) {
103  v[i] = m_links[i].d;
104  else v[i] = m_links[i].theta;
105  }
106  }
107
108  /** Set all the DOFs of the arm at once, from a vector with all the "q_i" values, which
109  * are interpreted as rotations (radians) or displacements (meters) depending on links being "revolute" or "prismatic".
110  * \exception std::exception If the size of the vector doesn't match the number of links.
111  * \tparam VECTOR Can be any Eigen vector, mrpt::math::CVectorDouble, or std::vector<double>
112  */
113  template <class VECTOR>
114  void setConfiguration(const VECTOR &v)
115  {
116  ASSERT_EQUAL_(v.size(),this->size())
117  const size_t N=m_links.size();
118  for (size_t i=0;i<N;i++) {
120  m_links[i].d = v[i];
121  else m_links[i].theta = v[i];
122  }
123  }
124
125  /** Constructs a 3D representation of the kinematic chain, in its current state.
126  * You can call update3DObject() to update the kinematic state of these OpenGL objects in the future, since
127  * an internal list of smart pointers to the constructed objects is kept internally. This is more efficient
128  * than calling this method again to build a new representation.
129  * \param[out] out_all_poses Optional output vector, will contain the poses in the format of recomputeAllPoses()
130  * \sa update3DObject
131  */
132  void getAs3DObject(
133  mrpt::opengl::CSetOfObjectsPtr &inout_gl_obj,
135  ) const;
136
137  /** Read getAs3DObject() for a description.
138  * \param[out] out_all_poses Optional output vector, will contain the poses in the format of recomputeAllPoses()
139  */
140  void update3DObject( mrpt::aligned_containers<mrpt::poses::CPose3D>::vector_t *out_all_poses = NULL ) const;
141
142  /** Go thru all the links of the chain and compute the global pose of each link. The "ground" link pose "pose0" defaults to the origin of coordinates,
143  * but anything else can be passed as the optional argument.
144  * The returned vector has N+1 elements (N=number of links), since [0] contains the base frame, [1] the pose after the first link, and so on.
145  */
146  void recomputeAllPoses( mrpt::aligned_containers<mrpt::poses::CPose3D>::vector_t & poses, const mrpt::poses::CPose3D & pose0 = mrpt::poses::CPose3D() )const;
147
148
149  }; // End of class def.
151
152  } // End of namespace
153
154
155  // Specialization must occur in the same namespace
156  // (This is to ease serialization)
157  namespace utils
158  {
160  } // End of namespace
161
162 } // End of namespace
163
164 #endif
#define ASSERT_EQUAL_(__A, __B)
The links of this robot arm.
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:64
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
mrpt::poses::CPose3D m_origin
The pose of the first link.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
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...
::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CKinematicChainPtr &pObj)
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...
const GLdouble * v
Definition: glext.h:3603
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...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
std::vector< mrpt::opengl::CRenderizablePtr > m_last_gl_objects
Smart pointers to the last objects for each link, as returned in getAs3DObject(), for usage within up...
GLuint in
Definition: glext.h:6301
GLsizeiptr size
Definition: glext.h:3779