MRPT  2.0.1
CKinematicChain.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "kinematics-precomp.h" // Precompiled headers
11 
13 #include <mrpt/opengl/CCylinder.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::math;
21 using namespace mrpt::poses;
22 using namespace mrpt::kinematics;
23 using namespace std;
24 
26 
27 /** Appends a new link to the robotic arm, with the given Denavit-Hartenberg
28  * parameters (see TKinematicLink for further details) */
29 void CKinematicChain::addLink(
30  double theta, double d, double a, double alpha, bool is_prismatic)
31 {
32  m_links.emplace_back(theta, d, a, alpha, is_prismatic);
33 }
34 
35 /** Removes one link from the kinematic chain (0<=idx<N) */
36 void CKinematicChain::removeLink(const size_t idx)
37 {
38  ASSERT_BELOW_(idx, m_links.size());
39  m_links.erase(m_links.begin() + idx);
40 }
41 
42 const TKinematicLink& CKinematicChain::getLink(const size_t idx) const
43 {
44  ASSERT_BELOW_(idx, m_links.size());
45  return m_links[idx];
46 }
47 
48 TKinematicLink& CKinematicChain::getLinkRef(const size_t idx)
49 {
50  ASSERT_BELOW_(idx, m_links.size());
51  return m_links[idx];
52 }
53 
54 void CKinematicChain::setOriginPose(const mrpt::poses::CPose3D& new_pose)
55 {
56  m_origin = new_pose;
57 }
58 
59 const mrpt::poses::CPose3D& CKinematicChain::getOriginPose() const
60 {
61  return m_origin;
62 }
63 
64 uint8_t CKinematicChain::serializeGetVersion() const { return 1; }
65 void CKinematicChain::serializeTo(mrpt::serialization::CArchive& out) const
66 {
67  out << m_links << m_origin;
68 }
69 
70 void CKinematicChain::serializeFrom(
71  mrpt::serialization::CArchive& in, uint8_t version)
72 {
73  switch (version)
74  {
75  case 0:
76  case 1:
77  {
78  in >> m_links;
79  if (version >= 1)
80  {
81  in >> m_origin;
82  }
83  else
84  m_origin = mrpt::poses::CPose3D();
85  }
86  break;
87  default:
89  };
90 }
91 
92 /** Go thru all the links of the chain and compute the global pose of each link.
93  * The "ground" link pose "pose0" defaults to the origin of coordinates,
94  * but anything else can be passed as the optional argument. */
95 void CKinematicChain::recomputeAllPoses(
96  std::vector<mrpt::poses::CPose3D>& poses,
97  [[maybe_unused]] const mrpt::poses::CPose3D& pose0) const
98 {
99  const size_t N = m_links.size();
100 
101  poses.resize(N + 1);
102 
103  CPose3D p = m_origin; // Cummulative pose
104 
105  poses[0] = p;
106 
107  for (size_t i = 0; i < N; i++)
108  {
109  // Build the 3D pose change of the i'th link:
110  const double th = m_links[i].theta;
111  const double alpha = m_links[i].alpha;
112  const double d = m_links[i].d;
113  const double a = m_links[i].a;
114 
115  const double t_vals[3] = {a * cos(th), a * sin(th), d};
116  const double r_vals[3 * 3] = {cos(th),
117  -sin(th) * cos(alpha),
118  sin(th) * sin(alpha),
119  sin(th),
120  cos(th) * cos(alpha),
121  -cos(th) * sin(alpha),
122  0,
123  sin(alpha),
124  cos(alpha)};
125 
126  const CMatrixDouble33 R(r_vals);
127  const CVectorFixedDouble<3> t(t_vals);
128 
129  CPose3D link(R, t);
130 
131  p.composeFrom(p, link);
132 
133  poses[i + 1] = p;
134  }
135 }
136 
137 const float R = 0.01f;
138 
139 void addBar_D(mrpt::opengl::CSetOfObjects::Ptr& objs, const double d)
140 {
143  gl_cyl->setColor_u8(mrpt::img::TColor(0x00, 0x00, 0xff));
144  gl_cyl->setName("cyl.d");
145 
146  objs->insert(gl_cyl);
147 }
148 
149 void addBar_A(mrpt::opengl::CSetOfObjects::Ptr& objs, const double a)
150 {
153  gl_cyl2->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00));
154  gl_cyl2->setPose(mrpt::poses::CPose3D(0, 0, 0, 0, 90.0_deg, 0));
155  gl_cyl2->setName("cyl.a");
156 
157  objs->insert(gl_cyl2);
158 }
159 
160 void CKinematicChain::getAs3DObject(
162  std::vector<mrpt::poses::CPose3D>* out_all_poses) const
163 {
164  ASSERT_(obj);
165  const size_t N = m_links.size();
166 
167  // Recompute current poses:
168  std::vector<mrpt::poses::CPose3D> all_poses;
169  recomputeAllPoses(all_poses);
170 
171  m_last_gl_objects.resize(N + 1);
172 
173  // Ground [0] and Links [1-N]:
174  for (size_t i = 0; i <= N; i++)
175  {
178  gl_corner->setPose(all_poses[i]);
179 
180  gl_corner->setName(mrpt::format("%u", static_cast<unsigned int>(i)));
181  gl_corner->enableShowName();
182 
183  if (i < N) addBar_D(gl_corner, m_links[i].d);
184  if (i > 0) addBar_A(gl_corner, m_links[i - 1].a);
185 
186  obj->insert(gl_corner);
187  m_last_gl_objects[i] = gl_corner;
188  }
189 
190  if (out_all_poses) out_all_poses->swap(all_poses);
191 }
192 
193 void CKinematicChain::update3DObject(
194  std::vector<mrpt::poses::CPose3D>* out_all_poses) const
195 {
196  ASSERTMSG_(
197  (m_links.size() + 1) == m_last_gl_objects.size(),
198  "The kinematic chain has changed since the last call to "
199  "getAs3DObject()");
200 
201  const size_t N = m_links.size();
202 
203  // Recompute current poses:
204  std::vector<mrpt::poses::CPose3D> all_poses;
205  recomputeAllPoses(all_poses);
206 
207  for (size_t i = 0; i <= N; i++)
208  {
210  std::dynamic_pointer_cast<mrpt::opengl::CSetOfObjects>(
211  m_last_gl_objects[i]);
212  gl_objs->setPose(all_poses[i]);
213 
214  if (i < N)
215  {
217  std::dynamic_pointer_cast<mrpt::opengl::CCylinder>(
218  gl_objs->getByName("cyl.d"));
219  const float d = mrpt::d2f(m_links[i].d);
220  glCyl->setHeight(d);
221  }
222 
223  if (i > 0)
224  {
226  std::dynamic_pointer_cast<mrpt::opengl::CCylinder>(
227  gl_objs->getByName("cyl.a"));
228  const float a = mrpt::d2f(m_links[i - 1].a);
229  glCyl2->setHeight(-a);
230  }
231  }
232 
233  if (out_all_poses) out_all_poses->swap(all_poses);
234 }
235 
236 /** Erases all links and leave the robot arm empty. */
238 {
239  m_links.clear();
240  m_last_gl_objects.clear();
241 }
242 
245 {
246  uint32_t version;
247  in >> version;
248  switch (version)
249  {
250  case 0:
251  in >> o.theta >> o.d >> o.a >> o.alpha >> o.is_prismatic;
252  break;
253  default:
255  }
256  return in;
257 }
260 {
261  const uint32_t version = 0;
262  out << version;
263  out << o.theta << o.d << o.a << o.alpha << o.is_prismatic;
264  return out;
265 }
static Ptr Create(Args &&... args)
Definition: CCylinder.h:31
A set of object, which are referenced to the coordinates framework established in this object...
Definition: CSetOfObjects.h:26
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void addBar_A(mrpt::opengl::CSetOfObjects::Ptr &objs, const double a)
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, TKinematicLink &o)
STL namespace.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
float d2f(const double d)
shortcut for static_cast<float>(double)
This base provides a set of functions for maths stuff.
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:556
A cylinder or cone whose base lies in the XY plane.
Definition: CCylinder.h:29
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &out, const TKinematicLink &o)
void addBar_D(mrpt::opengl::CSetOfObjects::Ptr &objs, const double d)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
CSetOfObjects::Ptr CornerXYZSimple(float scale=1.0, float lineWidth=1.0)
Returns three arrows representing a X,Y,Z 3D corner (just thick lines instead of complex arrows for f...
A RGB color - 8bit.
Definition: TColor.h:25
A open-loop kinematic chain model, suitable to robotic manipulators.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020