Main MRPT website > C++ reference for MRPT 1.5.7
CKinematicChain.cpp
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 |
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 
10 #include "kinematics-precomp.h" // Precompiled headers
11 
13 #include <mrpt/utils/CStream.h>
16 #include <mrpt/opengl/CCylinder.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::math;
21 using namespace mrpt::utils;
22 using namespace mrpt::poses;
23 using namespace mrpt::kinematics;
24 using namespace std;
25 
27 
28 
29 /** Appends a new link to the robotic arm, with the given Denavit-Hartenberg parameters (see TKinematicLink for further details) */
30 void CKinematicChain::addLink(double theta, double d, double a, double alpha, bool is_prismatic)
31 {
32  m_links.push_back( TKinematicLink(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 /*---------------------------------------------------------------
65  Implements the writing to a CStream capability of CSerializable objects
66  ---------------------------------------------------------------*/
67 void CKinematicChain::writeToStream(mrpt::utils::CStream &out,int *version) const
68 {
69  if (version)
70  *version = 1;
71  else
72  {
73  out << m_links << m_origin;
74  }
75 }
76 
77 /*---------------------------------------------------------------
78  Implements the reading from a CStream capability of CSerializable objects
79  ---------------------------------------------------------------*/
80 void CKinematicChain::readFromStream(mrpt::utils::CStream &in,int version)
81 {
82  switch(version)
83  {
84  case 0:
85  case 1:
86  {
87  in >> m_links;
88  if (version>=1)
89  {
90  in >> m_origin;
91  }
92  else m_origin=mrpt::poses::CPose3D();
93  } break;
94  default:
96  };
97 }
98 
99 /** 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,
100  * but anything else can be passed as the optional argument. */
101 void CKinematicChain::recomputeAllPoses(mrpt::aligned_containers<mrpt::poses::CPose3D>::vector_t & poses,
102  const mrpt::poses::CPose3D & pose0 )const
103 {
104  MRPT_UNUSED_PARAM(pose0);
105  const size_t N=m_links.size();
106 
107  poses.resize(N+1);
108 
109  CPose3D p = m_origin; // Cummulative pose
110 
111  poses[0] = p;
112 
113  for (size_t i=0;i<N;i++)
114  {
115  // Build the 3D pose change of the i'th link:
116  const double th = m_links[i].theta;
117  const double alpha = m_links[i].alpha;
118  const double d = m_links[i].d;
119  const double a = m_links[i].a;
120 
121  const double t_vals[3] = {
122  a * cos(th),
123  a * sin(th),
124  d };
125  const double r_vals[3*3] = {
126  cos(th), -sin(th)*cos(alpha), sin(th)*sin(alpha),
127  sin(th), cos(th)*cos(alpha), -cos(th)*sin(alpha),
128  0, sin(alpha), cos(alpha) };
129 
130  const CMatrixDouble33 R(r_vals);
131  const CArrayDouble<3> t(t_vals);
132 
133  CPose3D link(R,t);
134 
135  p.composeFrom(p,link);
136 
137  poses[i+1] = p;
138  }
139 }
140 
141 const float R = 0.01f;
142 
143 void addBar_D(mrpt::opengl::CSetOfObjectsPtr &objs, const double d)
144 {
145 
146  mrpt::opengl::CCylinderPtr gl_cyl = mrpt::opengl::CCylinder::Create(R,R, d );
147  gl_cyl->setColor_u8( mrpt::utils::TColor(0x00,0x00,0xff) );
148  gl_cyl->setName("cyl.d");
149 
150  objs->insert(gl_cyl);
151 }
152 
153 void addBar_A(mrpt::opengl::CSetOfObjectsPtr &objs, const double a)
154 {
155  mrpt::opengl::CCylinderPtr gl_cyl2 = mrpt::opengl::CCylinder::Create(R,R, -a );
156  gl_cyl2->setColor_u8( mrpt::utils::TColor(0xff,0x00,0x00) );
157  gl_cyl2->setPose( mrpt::poses::CPose3D(0,0,0, 0, DEG2RAD(90),0) );
158  gl_cyl2->setName("cyl.a");
159 
160  objs->insert(gl_cyl2);
161 }
162 
163 void CKinematicChain::getAs3DObject(
164  mrpt::opengl::CSetOfObjectsPtr &obj,
166  ) const
167 {
168  ASSERT_(obj.present())
169  const size_t N=m_links.size();
170 
171  // Recompute current poses:
173  recomputeAllPoses( all_poses );
174 
175  m_last_gl_objects.resize(N+1);
176 
177  // Ground [0] and Links [1-N]:
178  for (size_t i=0;i<=N;i++)
179  {
180  mrpt::opengl::CSetOfObjectsPtr gl_corner = mrpt::opengl::stock_objects::CornerXYZSimple( 0.1f, 3.0f );
181  gl_corner->setPose( all_poses[i] );
182 
183  gl_corner->setName( mrpt::format("%u",static_cast<unsigned int>(i)) );
184  gl_corner->enableShowName();
185 
186  if (i<N) addBar_D(gl_corner,m_links[i].d);
187  if (i>0) addBar_A(gl_corner,m_links[i-1].a);
188 
189  obj->insert(gl_corner);
190  m_last_gl_objects[i] = gl_corner;
191  }
192 
193  if (out_all_poses)
194  out_all_poses->swap(all_poses);
195 }
196 
197 void CKinematicChain::update3DObject(mrpt::aligned_containers<mrpt::poses::CPose3D>::vector_t *out_all_poses ) const
198 {
199  ASSERTMSG_((m_links.size()+1)==m_last_gl_objects.size(), "The kinematic chain has changed since the last call to getAs3DObject()")
200 
201  const size_t N=m_links.size();
202 
203  // Recompute current poses:
205  recomputeAllPoses( all_poses );
206 
207  for (size_t i=0;i<=N;i++)
208  {
209  mrpt::opengl::CSetOfObjectsPtr gl_objs = mrpt::opengl::CSetOfObjectsPtr(m_last_gl_objects[i]);
210  gl_objs->setPose( all_poses[i] );
211 
212  if (i<N)
213  {
214  mrpt::opengl::CCylinderPtr glCyl = mrpt::opengl::CCylinderPtr( gl_objs->getByName("cyl.d") );
215  const double d = m_links[i].d;
216  glCyl->setHeight( d );
217  }
218 
219  if (i>0)
220  {
221  mrpt::opengl::CCylinderPtr glCyl2 = mrpt::opengl::CCylinderPtr( gl_objs->getByName("cyl.a") );
222  const double a = m_links[i-1].a;
223  //glCyl2->setPose( mrpt::poses::CPose3D(0,0,d, 0, DEG2RAD(90),0) );
224  glCyl2->setHeight( -a );
225  }
226  }
227 
228  if (out_all_poses)
229  out_all_poses->swap(all_poses);
230 }
231 
232 /** Erases all links and leave the robot arm empty. */
234 {
235  m_links.clear();
236  m_last_gl_objects.clear();
237 }
238 
239 
241 {
243  in >> version;
244  switch(version)
245  {
246  case 0:
247  in >> o.theta >> o.d >> o.a >> o.alpha >> o.is_prismatic;
248  break;
249  default:
251  }
252  return in;
253 }
255 {
256  const uint32_t version = 0;
257  out << version;
258  out << o.theta << o.d << o.a << o.alpha << o.is_prismatic;
259  return out;
260 }
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
static CCylinderPtr Create()
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble t
Definition: glext.h:3610
void addBar_A(mrpt::opengl::CSetOfObjectsPtr &objs, const double a)
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define ASSERT_BELOW_(__A, __B)
STL namespace.
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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 base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CSetOfObjectsPtr OPENGL_IMPEXP 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:26
::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CKinematicChainPtr &pObj)
int version
Definition: mrpt_jpeglib.h:898
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void addBar_D(mrpt::opengl::CSetOfObjectsPtr &objs, const double d)
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
unsigned __int32 uint32_t
Definition: rptypes.h:49
A open-loop kinematic chain model, suitable to robotic manipulators.
#define ASSERTMSG_(f, __ERROR_MSG)
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
GLfloat GLfloat p
Definition: glext.h:5587
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
KINEMATICS_IMPEXP mrpt::utils::CStream & operator<<(mrpt::utils::CStream &out, const TKinematicLink &o)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019