MRPT  1.9.9
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-2019, 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) */
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(
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(
97  const mrpt::poses::CPose3D& pose0) const
98 {
99  MRPT_UNUSED_PARAM(pose0);
100  const size_t N = m_links.size();
101 
102  poses.resize(N + 1);
103 
104  CPose3D p = m_origin; // Cummulative pose
105 
106  poses[0] = p;
107 
108  for (size_t i = 0; i < N; i++)
109  {
110  // Build the 3D pose change of the i'th link:
111  const double th = m_links[i].theta;
112  const double alpha = m_links[i].alpha;
113  const double d = m_links[i].d;
114  const double a = m_links[i].a;
115 
116  const double t_vals[3] = {a * cos(th), a * sin(th), d};
117  const double r_vals[3 * 3] = {cos(th),
118  -sin(th) * cos(alpha),
119  sin(th) * sin(alpha),
120  sin(th),
121  cos(th) * cos(alpha),
122  -cos(th) * sin(alpha),
123  0,
124  sin(alpha),
125  cos(alpha)};
126 
127  const CMatrixDouble33 R(r_vals);
128  const CArrayDouble<3> t(t_vals);
129 
130  CPose3D link(R, t);
131 
132  p.composeFrom(p, link);
133 
134  poses[i + 1] = p;
135  }
136 }
137 
138 const float R = 0.01f;
139 
140 void addBar_D(mrpt::opengl::CSetOfObjects::Ptr& objs, const double d)
141 {
143  mrpt::make_aligned_shared<mrpt::opengl::CCylinder>(R, R, d);
144  gl_cyl->setColor_u8(mrpt::img::TColor(0x00, 0x00, 0xff));
145  gl_cyl->setName("cyl.d");
146 
147  objs->insert(gl_cyl);
148 }
149 
150 void addBar_A(mrpt::opengl::CSetOfObjects::Ptr& objs, const double a)
151 {
153  mrpt::make_aligned_shared<mrpt::opengl::CCylinder>(R, R, -a);
154  gl_cyl2->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00));
155  gl_cyl2->setPose(mrpt::poses::CPose3D(0, 0, 0, 0, DEG2RAD(90), 0));
156  gl_cyl2->setName("cyl.a");
157 
158  objs->insert(gl_cyl2);
159 }
160 
161 void CKinematicChain::getAs3DObject(
164 {
165  ASSERT_(obj);
166  const size_t N = m_links.size();
167 
168  // Recompute current poses:
170  recomputeAllPoses(all_poses);
171 
172  m_last_gl_objects.resize(N + 1);
173 
174  // Ground [0] and Links [1-N]:
175  for (size_t i = 0; i <= N; i++)
176  {
179  gl_corner->setPose(all_poses[i]);
180 
181  gl_corner->setName(mrpt::format("%u", static_cast<unsigned int>(i)));
182  gl_corner->enableShowName();
183 
184  if (i < N) addBar_D(gl_corner, m_links[i].d);
185  if (i > 0) addBar_A(gl_corner, m_links[i - 1].a);
186 
187  obj->insert(gl_corner);
188  m_last_gl_objects[i] = gl_corner;
189  }
190 
191  if (out_all_poses) out_all_poses->swap(all_poses);
192 }
193 
194 void CKinematicChain::update3DObject(
196 {
197  ASSERTMSG_(
198  (m_links.size() + 1) == m_last_gl_objects.size(),
199  "The kinematic chain has changed since the last call to "
200  "getAs3DObject()");
201 
202  const size_t N = m_links.size();
203 
204  // Recompute current poses:
206  recomputeAllPoses(all_poses);
207 
208  for (size_t i = 0; i <= N; i++)
209  {
211  std::dynamic_pointer_cast<mrpt::opengl::CSetOfObjects>(
212  m_last_gl_objects[i]);
213  gl_objs->setPose(all_poses[i]);
214 
215  if (i < N)
216  {
218  std::dynamic_pointer_cast<mrpt::opengl::CCylinder>(
219  gl_objs->getByName("cyl.d"));
220  const double d = m_links[i].d;
221  glCyl->setHeight(d);
222  }
223 
224  if (i > 0)
225  {
227  std::dynamic_pointer_cast<mrpt::opengl::CCylinder>(
228  gl_objs->getByName("cyl.a"));
229  const double a = m_links[i - 1].a;
230  // glCyl2->setPose( mrpt::poses::CPose3D(0,0,d, 0, DEG2RAD(90),0) );
231  glCyl2->setHeight(-a);
232  }
233  }
234 
235  if (out_all_poses) out_all_poses->swap(all_poses);
236 }
237 
238 /** Erases all links and leave the robot arm empty. */
240 {
241  m_links.clear();
242  m_last_gl_objects.clear();
243 }
244 
247 {
248  uint32_t version;
249  in >> version;
250  switch (version)
251  {
252  case 0:
253  in >> o.theta >> o.d >> o.a >> o.alpha >> o.is_prismatic;
254  break;
255  default:
257  }
258  return in;
259 }
262 {
263  const uint32_t version = 0;
264  out << version;
265  out << o.theta << o.d << o.a << o.alpha << o.is_prismatic;
266  return out;
267 }
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3529
GLdouble GLdouble t
Definition: glext.h:3695
A set of object, which are referenced to the coordinates framework established in this object...
Definition: CSetOfObjects.h:26
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:190
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
double DEG2RAD(const double x)
Degrees to radians.
void addBar_A(mrpt::opengl::CSetOfObjects::Ptr &objs, const double a)
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, TKinematicLink &o)
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
STL namespace.
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
unsigned char uint8_t
Definition: rptypes.h:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:138
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:161
This base provides a set of functions for maths stuff.
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:149
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:52
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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...
GLuint in
Definition: glext.h:7391
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
A RGB color - 8bit.
Definition: TColor.h:20
unsigned __int32 uint32_t
Definition: rptypes.h:50
A open-loop kinematic chain model, suitable to robotic manipulators.
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:182
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: bef67d9c5 Mon Apr 15 00:03:11 2019 +0200 at lun abr 15 00:10:13 CEST 2019