trajectory.cpp
Go to the documentation of this file.
1 /*
2 Copyright (C) 2002-2004 Etienne Lachance
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
22 
23 -------------------------------------------------------------------------------
24 Revision_history:
25 
26 2004/06/02: Etienne Lachance
27  -Added class Trajectory_Select.
28 
29 2004/07/01: Ethan Tira-Thompson
30  -Added support for newmat's use_namespace #define, using ROBOOP namespace.
31 
32 2005/06/10: Etienne Lachance
33  -Missing file warning message.
34 
35 2005/11/06: Etienne Lachance
36  - No need to provide a copy constructor and the assignment operator
37  (operator=) for Spl_Quaternion, Spl_Cubic and Spl_path classes. Instead we
38  use the one provide by the compiler.
39 -------------------------------------------------------------------------------
40 */
41 
47 static const char rcsid[] = "$Id: trajectory.cpp,v 1.8 2006/05/16 19:24:26 gourdeau Exp $";
49 
50 #include "trajectory.h"
51 
52 #ifdef use_namespace
53 namespace ROBOOP {
54  using namespace NEWMAT;
55 #endif
56 
57 
73 {
74  int N = pts.Ncols();
75  bad_data = false;
76  nb_path = pts.Nrows()-1;
77 
78  if(!N)
79  {
80  bad_data = true;
81  cerr << "Spl_cubic::Spl_cubic: size of time vector is zero." << endl;
82  return;
83  }
84  if(N <= 3)
85  {
86  bad_data = true;
87  cerr << "Spl_cubic::Spl_cubic: need at least 4 points to produce a cubic spline." << endl;
88  return;
89  }
90  if(!nb_path)
91  {
92  bad_data = true;
93  cerr << "Spl_cubic::Spl_cubic: No data for each points." << endl;
94  return;
95  }
96 
97  ColumnVector delta(N-1), beta(N-1);
98  tk = pts.SubMatrix(1,1,1,N); // time at sampling point
99 
100  for(int i = 1; i <= N-1; i++)
101  {
102  delta(i) = pts(1,i+1) - pts(1,i); // eq 5.58d Angeles
103 
104  if(!delta(i))
105  {
106  bad_data = true;
107  cerr << "Spl_cubic::Spl_cubic: time between input points is zero" << endl;
108  return;
109  }
110  beta(i) = 1/delta(i); // eq 5.58e Angeles
111  }
112 
113  Matrix A(N-2, N-2); A = 0; // eq 5.59 Angeles
114  A(1,1) = 2*(delta(1)+delta(2));
115  A(1,2) = delta(2);
116  for(int j = 2; j <= N-2; j++)
117  {
118  A(j,j-1) = delta(j);
119  A(j,j) = 2*(delta(j)+delta(j+1));
120  if( (j+1) <= A.Ncols())
121  A(j,j+1) = delta(j+1);
122  }
123 
124  Matrix C(N-2, N); C = 0; // eq 5.58c Angeles
125  for(int k = 1; k <= N-2; k++)
126  {
127  C(k,k) = beta(k);
128  C(k,k+1) = -(beta(k)+beta(k+1));
129  C(k,k+2) = beta(k+1);
130  }
131 
132  Matrix _6AiC = 6*A.i()*C; // eq 5.58a Angeles
133 
134  ColumnVector dd_s(N); // second spline derivative at sampling points
135  dd_s(1) = dd_s(N) = 0; // second der is 0 on first and last point of natural splines.
136 
137  Ak = Matrix(nb_path, N-1);
138  Bk = Matrix(nb_path, N-1);
139  Ck = Matrix(nb_path, N-1);
140  Dk = Matrix(nb_path, N-1);
141 
142  for(int ii = 2; ii <= nb_path+1; ii++)
143  {
144  dd_s.SubMatrix(2,N-1,1,1) = _6AiC*pts.SubMatrix(ii,ii,1,N).t();
145 
146  for(int jj = 1; jj < N; jj++)
147  {
148  // eq 5.55a - 5.55d Angeles
149  Ak(ii-1,jj) = 1/(6.0*delta(jj))*(dd_s(jj+1) - dd_s(jj));
150  Bk(ii-1,jj) = 1/2.0*dd_s(jj);
151  Ck(ii-1,jj) = (pts(ii,jj+1)-pts(ii,jj))/delta(jj) -
152  1/6.0*delta(jj)*(dd_s(jj+1) + 2*dd_s(jj));
153  Dk(ii-1,jj) = pts(ii,jj);
154  }
155  }
156 }
157 
160 {
161  if(bad_data)
162  {
163  cerr << "Spl_cubic::interpolating: data is not good. Problems occur in constructor." << endl;
164  return BAD_DATA;
165  }
166 
167  for(int i = 1; i < tk.Ncols(); i++)
168  if( (t >= tk(i)) && (t < tk(i+1)) )
169  {
170  s = Ak.SubMatrix(1,nb_path,i,i)*pow(t - tk(i), 3) +
171  Bk.SubMatrix(1,nb_path,i,i)*pow(t - tk(i),2) +
172  Ck.SubMatrix(1,nb_path,i,i)*(t - tk(i)) +
173  Dk.SubMatrix(1,nb_path,i,i);
174  return 0;
175  }
176 
177  cerr << "Spl_cubic::interpolating: t is out of range." << endl;
178  return NOT_IN_RANGE;
179 }
180 
181 
184 {
185  if(bad_data)
186  {
187  cerr << "Spl_cubic::first_derivative: data is not good. Problems occur in constructor." << endl;
188  return BAD_DATA;
189  }
190 
191  for(int i = 1; i < tk.Ncols(); i++)
192  if( (t >= tk(i)) && (t < tk(i+1)) )
193  {
194  ds = 3*Ak.SubMatrix(1,nb_path,i,i)*pow(t - tk(i), 2) +
195  2*Bk.SubMatrix(1,nb_path,i,i)*(t - tk(i)) +
196  Ck.SubMatrix(1,nb_path,i,i);
197  return 0;
198  }
199 
200  cerr << "Spl_cubic::first_derivative: t not in range." << endl;
201  return NOT_IN_RANGE;
202 }
203 
204 
207 {
208  if(bad_data)
209  {
210  cerr << "Spl_cubic::second_derivative: data is not good. Problems occur in constructor." << endl;
211  return BAD_DATA;
212  }
213 
214  for(int i = 1; i < tk.Ncols(); i++)
215  if( (t >= tk(i)) && (t < tk(i+1)) )
216  {
217  ds = 6*Ak.SubMatrix(1,nb_path,i,i)*(t - tk(i)) +
218  2*Bk.SubMatrix(1,nb_path,i,i);
219  return 0;
220  }
221 
222  cerr << "Spl_cubic::second_derivative: t not in range." << endl;
223  return NOT_IN_RANGE;
224 }
225 
226 // ----------- E N D - E F F E C T O R P A T H W I T H S P L I N E S ----------
227 
228 Spl_path::Spl_path(const string & filename)
268 {
269  const char *ptr_filename = filename.c_str(); //transform string to *char
270 
271  std::ifstream inpointfile(ptr_filename, std::ios::in);
272  point_map pts_map;
273 
274  if(inpointfile)
275  {
276  double time;
277  int nb_path;
278  string temp;
279  pts_map.clear();
280 
281  getline(inpointfile, temp);
282  while(temp.substr(0,1) == "#") { // # comment
283  getline(inpointfile, temp);
284  }
285 
286  if(temp == "JOINT_SPACE")
287  type = JOINT_SPACE;
288  else if (temp == "CARTESIAN_SPACE")
289  type = CARTESIAN_SPACE;
290  else
291  {
292  cerr << "Spl_path::Spl_path: wrong selection type. Should be joint space or cartesian space." << endl;
293  return;
294  }
295 
296  getline(inpointfile, temp);
297  istringstream inputString1(temp);
298  inputString1 >> nb_path;
299  if(nb_path < 1)
300  {
301  cerr << "Spl_path::Spl_path: number of splines should be >= 1." << endl;
302  return;
303  }
304  ColumnVector p(nb_path);
305 
306 
307  while( !inpointfile.eof() )
308  {
309  getline(inpointfile, temp);
310  istringstream inputString2(temp);
311 
312  if(temp.substr(0,1) == " ")
313  break;
314  inputString2 >> time;
315  final_time = time;
316 
317  getline(inpointfile, temp);
318  istringstream inputString3(temp);
319 
320  for(int i = 1; i <= nb_path; i++)
321  inputString3 >> p(i);
322 
323  pts_map.insert(point_map::value_type(time, p));
324  }
325  }
326  else
327  cerr << "Spl_path::Spl_path: can not open file " << filename.c_str() << endl;
328 
329  // Spl_cubic class take as input a Matrix that contain the data.
330  int nb_pts, nb_path;
331  nb_pts = pts_map.size();
332  point_map::const_iterator iter = pts_map.begin();
333  nb_path = iter->second.Nrows();
334 
335  Matrix pts(nb_path+1, nb_pts); pts = 0;
336  {
337  int i = 1;
338  for(iter = pts_map.begin(); iter != pts_map.end(); ++iter)
339  {
340  pts(1,i) = iter->first;
341  pts.SubMatrix(2, nb_path+1, i, i) = iter->second;
342  i++;
343  }
344  }
345  Spl_cubic::operator=(Spl_cubic(pts));
346 }
347 
348 
354 {
355 }
356 
357 short Spl_path::p(const Real t, ColumnVector & p)
359 {
360  if(interpolating(t, p))
361  {
362  cerr << "Spl_path::p_pdot: problem with spline interpolating." << endl;
363  return -4;
364  }
365 
366  return 0;
367 }
368 
371 {
372  if(interpolating(t, p))
373  {
374  cerr << "Spl_path::p_pdot: problem with spline interpolating." << endl;
375  return -4;
376  }
377  if(first_derivative(t, pdot))
378  {
379  cerr << "Spl_path::p_pdot: problem with spline first_derivative." << endl;
380  return -4;
381  }
382 
383  return 0;
384 }
385 
387  ColumnVector & pdotdot)
389 {
390  if(interpolating(t, p))
391  {
392  cerr << "Spl_path::p_pdot_pdotdot: problem with spline interpolating." << endl;
393  return -4;
394  }
395  if(first_derivative(t, pdot))
396  {
397  cerr << "Spl_path::p_pdot_pdotdot: problem with spline first_derivative." << endl;
398  return -4;
399  }
400  if(second_derivative(t, pdotdot))
401  {
402  cerr << "Spl_path::p_pdot_pdotdot: problem with spline first_derivative." << endl;
403  return -4;
404  }
405  return 0;
406 }
407 
408 // -------------------- Q U A T E R N I O N S P L I N E S -------------------
409 
410 
411 Spl_Quaternion::Spl_Quaternion(const string & filename)
437 {
438  const char *ptr_filename = filename.c_str(); //transform string to *char
439 
440  std::ifstream inquatfile(ptr_filename, std::ios::in);
441 
442  if(inquatfile)
443  {
444  double time;
445  string temp;
446  Matrix R(3,3);
447  Quaternion q;
448  quat_data.clear();
449 
450  while( !inquatfile.eof() )
451  {
452  getline(inquatfile, temp);
453  while(temp.substr(0,1) == "#") {
454  getline(inquatfile, temp);
455  }
456  istringstream inputString(temp);
457  if(temp.substr(0,1) == " ")
458  break;
459  inputString >> time;
460 
461  getline(inquatfile, temp);
462  if( (temp.substr(0,1) == "r") || (temp.substr(0,1) == "R") )
463  {
464  getline(inquatfile, temp);
465  istringstream inputString(temp);
466  inputString >> R(1,1) >> R(1,2) >> R(1,3) >> R(2,1) >> R(2,2) >>
467  R(2,3) >> R(3,1) >> R(3,2) >> R(3,3);
468  q = Quaternion(R);
469  }
470  else if( (temp.substr(0,1) == "q") || (temp.substr(0,1) == "Q") )
471  {
472  getline(inquatfile, temp);
473  istringstream inputString(temp);
474  inputString >> R(1,1) >> R(1,2) >> R(1,3) >> R(2,1);
475  q = Quaternion(R(1,1), R(1,2), R(1,3), R(2,1));
476  }
477  else if(temp.substr(0,1) == "")
478  {
479  }
480  else
481  {
482  cerr << "Spl_Quaternion::Spl_Quaternion: format of input file "
483  << filename.c_str() << " is incorrect" << endl;
484  }
485 
486  quat_data.insert( quat_map::value_type(time, q));
487  }
488  }
489  else
490  {
491  cerr << "Spl_Quaternion::Spl_Quaternion: can not open file "
492  << filename.c_str() << endl;
493  }
494 }
495 
496 
499 {
500  quat_data = quat;
501 }
502 
511 {
512  Real dt=0;
513  Quaternion ds;
514  quat_map::const_iterator iter1 = quat_data.begin(), iter2;
515 
516  if( t == iter1->first)
517  {
518  s = iter1->second;
519  return 0;
520  }
521 
522  // Point to interpolate is between the first and the second point. Use Slerp function
523  // since there is not enough points for Squad. Use dt since Slerp and Squad functions
524  // need t from 0 to 1.
525  iter2 = iter1;
526  iter2++;
527  if( t < iter2->first)
528  {
529  dt = (t - iter1->first)/(iter2->first - iter1->first);
530  s = Slerp(iter1->second, iter2->second, dt);
531  return 0;
532  }
533 
534 
535  // From second element to element N-2.
536  // for(iter1 = ++quat_data.begin(); iter1 != ----quat_data.end(); ++iter1)
537  // for(iter1 = ++iter1; iter1 != ----quat_data.end(); ++iter1)
538  for(iter1 = iter2; iter1 != ----quat_data.end(); ++iter1)
539  {
540  iter2=iter1;
541  iter2++;
542  if( (t >= iter1->first) && (t < iter2->first) )
543  {
544  dt = (t - iter1->first)/(iter2->first - iter1->first);
545 
546  Quaternion qn_ = (--iter1)->second, // q(n-1)
547  qn = (++iter1)->second, // q(n)
548  qn_1 = (++iter1)->second, // q(n+1)
549  qn_2 = (++iter1)->second, // q(n+2)
550  q_tmp;
551  ----iter1;
552 
553  // Intermediate point a(n) and a(n+1)
554  Quaternion an = qn*(((qn.i()*qn_1).Log() + (qn.i()*qn_).Log())/-4).exp(),
555  an_1 = qn_1*(((qn_1.i()*qn_2).Log() + (qn_1.i()*qn).Log())/-4).exp();
556 
557  s = Squad(qn, an, an_1, qn_1, dt);
558  return 0;
559  }
560  }
561 
562  // Interpolation between last two points.
563  iter2 = iter1; iter2++;
564  if( (t >= iter1->first) && (t <= iter2->first) )
565  {
566  dt = (t - iter1->first)/(iter2->first - iter1->first);
567  s = Slerp(iter1->second, iter2->second, dt);
568  return 0;
569  }
570 
571  cerr << "Spl_Quaternion::quat_w: t not in range." << endl;
572  return NOT_IN_RANGE;
573 }
574 
577 {
578  Real dt=0;
579  Quaternion ds;
580  quat_map::const_iterator iter1 = quat_data.begin(), iter2;
581 
582  if( t == iter1->first)
583  {
584  s = iter1->second;
585  w = ColumnVector(3); w = 0.0;
586  return 0;
587  }
588 
589  // Point to interpolate is between the first and the second point. Use Slerp function
590  // since there is not enough points for Squad. Use dt since Slerp and Squad functions
591  // need t from 0 to 1.
592  iter2 = iter1;
593  iter2++;
594  if( t < iter2->first)
595  {
596  dt = (t - iter1->first)/(iter2->first - iter1->first);
597  s = Slerp(iter1->second, iter2->second, dt);
598  ds = Slerp_prime(iter1->second, iter2->second, dt);
599  w = Omega(s, ds);
600 
601  return 0;
602  }
603 
604  // From second element to element N-2.
605  // for(iter1 = ++quat_data.begin(); iter1 != ----quat_data.end(); ++iter1)
606  // for(iter1 = ++iter1; iter1 != ----quat_data.end(); ++iter1)
607  for(iter1 = iter2; iter1 != ----quat_data.end(); ++iter1)
608  {
609  iter2=iter1;
610  iter2++;
611  if( (t >= iter1->first) && (t < iter2->first) )
612  {
613  dt = (t - iter1->first)/(iter2->first - iter1->first);
614 
615  Quaternion qn_ = (--iter1)->second, // q(n-1)
616  qn = (++iter1)->second, // q(n)
617  qn_1 = (++iter1)->second, // q(n+1)
618  qn_2 = (++iter1)->second, // q(n+2)
619  q_tmp;
620  ----iter1;
621 
622  // Intermediate point a(n) and a(n+1)
623  Quaternion an = qn*(((qn.i()*qn_1).Log() + (qn.i()*qn_).Log())/-4).exp(),
624  an_1 = qn_1*(((qn_1.i()*qn_2).Log() + (qn_1.i()*qn).Log())/-4).exp();
625 
626  s = Squad(qn, an, an_1, qn_1, dt);
627  ds = Squad_prime(qn, an, an_1, qn_1, dt);
628  w = Omega(s, ds);
629  return 0;
630  }
631  }
632 
633  // Interpolation between last two points.
634  iter2 = iter1; iter2++;
635  if( (t >= iter1->first) && (t <= iter2->first) )
636  {
637  dt = (t - iter1->first)/(iter2->first - iter1->first);
638  s = Slerp(iter1->second, iter2->second, dt);
639  ds = Slerp_prime(iter1->second, iter2->second, dt);
640  w = Omega(s, ds);
641  return 0;
642  }
643 
644  cerr << "Spl_Quaternion::quat_w: t not in range." << endl;
645  return NOT_IN_RANGE;
646 }
647 
648 
649 // -----------------------------------------------------------------------------------------------
650 
651 
654 {
655  type = NONE;
656  quaternion_active = false;
657 }
658 
659 
660 Trajectory_Select::Trajectory_Select(const string & filename)
668 {
669  set_trajectory(filename);
670 }
671 
672 
675 {
676  type = x.type;
677  if(x.quaternion_active)
678  {
679  quaternion_active = x.quaternion_active;
680  path_quat = x.path_quat;
681  }
682  else
683  path = x.path;
684 
685  return *this;
686 }
687 
688 
689 void Trajectory_Select::set_trajectory(const string & filename)
691 {
692  const char *ptr_filename = filename.c_str(); //transform string to *char
693 
694  std::ifstream inpointfile(ptr_filename, std::ios::in);
695 
696  if(inpointfile)
697  {
698  string temp;
699 
700  getline(inpointfile, temp);
701  while(temp.substr(0,1) == "#") { // # comment
702  getline(inpointfile, temp);
703  }
704 
705  if( (temp == "JOINT_SPACE") || (temp == "CARTESIAN_SPACE") )
706  {
707  path = Spl_path(filename);
708  type = path.get_type();
709  quaternion_active = false;
710  }
711  else
712  {
713  path_quat = Spl_Quaternion(filename);
715  quaternion_active = true;
716  }
717  }
718  else
719  cerr << "Trajectory_Select::set_trajectory: invalid input file " << filename << endl;
720 }
721 
722 #ifdef use_namespace
723 }
724 #endif
725 
726 
727 
728 
729 
730 
731 
Quaternion class definition.
Definition: quaternion.h:92
short type
Cartesian or joint space.
Definition: trajectory.h:173
Trajectory_Select()
Constructor.
Definition: trajectory.cpp:652
short quat(const Real t, Quaternion &s)
Quaternion interpollation.
Definition: trajectory.cpp:503
Header file for trajectory generation class.
short type
Cartesian space or joint space.
Definition: trajectory.h:134
Quaternion exp() const
Exponential of a quaternion.
Definition: quaternion.cpp:336
#define BAD_DATA
Definition: trajectory.h:83
double Real
Definition: include.h:307
Quaternion Slerp(const Quaternion &q0, const Quaternion &q1, const Real t)
Spherical Linear Interpolation.
Definition: quaternion.cpp:631
short second_derivative(const Real t, ColumnVector &dds)
Spline second derivative at time t.
Definition: trajectory.cpp:205
Trajectory class selection.
Definition: trajectory.h:164
int Nrows() const
Definition: newmat.h:494
Spl_path path
Spl_path instance.
Definition: trajectory.h:174
#define JOINT_SPACE
Definition: trajectory.h:109
short quat_w(const Real t, Quaternion &s, ColumnVector &w)
Quaternion interpollation and angular velocity.
Definition: trajectory.cpp:575
Cubic quaternions spline.
Definition: trajectory.h:147
#define NOT_IN_RANGE
Definition: trajectory.h:85
short interpolating(const Real t, ColumnVector &s)
Interpolating the spline at time t. Extrapolating is not allowed.
Definition: trajectory.cpp:158
Spl_Quaternion path_quat
Spl_Quaternion instance.
Definition: trajectory.h:175
bool quaternion_active
Using Spl_Quaternion.
Definition: trajectory.h:177
int size() const
Definition: newmat.h:501
TransposedMatrix t() const
Definition: newmat6.cpp:320
Quaternion Squad_prime(const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
Spherical Cubic Interpolation derivative.
Definition: quaternion.cpp:751
Quaternion Slerp_prime(const Quaternion &q0, const Quaternion &q1, const Real t)
Spherical Linear Interpolation derivative.
Definition: quaternion.cpp:692
The usual rectangular matrix.
Definition: newmat.h:625
InvertedMatrix i() const
Definition: newmat6.cpp:329
short p_pdot_pddot(const Real time, ColumnVector &p, ColumnVector &pdot, ColumnVector &pdotdot)
Position, velocity and acceleration vector at time t.
Definition: trajectory.cpp:386
#define CARTESIAN_SPACE
Definition: trajectory.h:110
short p(const Real time, ColumnVector &p)
Position vector at time t.
Definition: trajectory.cpp:357
#define NONE
Quaternion Squad(const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
Spherical Cubic Interpolation.
Definition: quaternion.cpp:725
int Ncols() const
Definition: newmat.h:495
std::map< Real, Quaternion, less< Real > > quat_map
Data at control points.
Definition: trajectory.h:140
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
Definition: newmat.h:2146
Trajectory_Select & operator=(const Trajectory_Select &x)
Overload = operator.
Definition: trajectory.cpp:673
std::map< Real, ColumnVector, less< Real > > point_map
Data at control points.
Definition: trajectory.h:113
void set_trajectory(const std::string &filename)
Trajectory selection.
Definition: trajectory.cpp:689
static const char rcsid[]
RCS/CVS version.
Definition: trajectory.cpp:48
short p_pdot(const Real time, ColumnVector &p, ColumnVector &pdot)
Position and velocity vector at time t.
Definition: trajectory.cpp:369
Column vector.
Definition: newmat.h:1008
Natural cubic splines class.
Definition: trajectory.h:91
short first_derivative(const Real t, ColumnVector &ds)
Spline first derivative at time t.
Definition: trajectory.cpp:182
ReturnMatrix Omega(const Quaternion &q, const Quaternion &q_dot)
Return angular velocity from a quaternion and it&#39;s time derivative.
Definition: quaternion.cpp:560


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:17