trajectory_composite.cpp
Go to the documentation of this file.
1 /*****************************************************************************
2  * \author
3  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
4  *
5  * \version
6  * LRL V0.2
7  *
8  * \par History
9  * - $log$
10  *
11  * \par Release
12  * $Id: trajectory_composite.cpp 22 2004-09-21 08:58:54Z eaertbellocal $
13  * $Name: $
14  ****************************************************************************/
15 
16 #include "trajectory_composite.hpp"
17 #include "path_composite.hpp"
18 
19 namespace KDL {
20 
21  using namespace std;
22 
23 
25  {
26  }
27 
29  return duration;
30  }
31 
32  Frame Trajectory_Composite::Pos(double time) const {
33  // not optimal, could be done in log(#elem)
34  // or one could buffer the last segment and start looking from there.
35  unsigned int i;
36  double previoustime;
37  Trajectory* traj;
38  if (time < 0) {
39  return vt[0]->Pos(0);
40  }
41  previoustime = 0;
42  for (i=0;i<vt.size();i++) {
43  if (time < vd[i]) {
44  return vt[i]->Pos(time-previoustime);
45  }
46  previoustime = vd[i];
47  }
48  traj = vt[vt.size()-1];
49  return traj->Pos(traj->Duration());
50  }
51 
52 
53  Twist Trajectory_Composite::Vel(double time) const {
54  // not optimal, could be done in log(#elem)
55  unsigned int i;
56  Trajectory* traj;
57  double previoustime;
58  if (time < 0) {
59  return vt[0]->Vel(0);
60  }
61  previoustime = 0;
62  for (i=0;i<vt.size();i++) {
63  if (time < vd[i]) {
64  return vt[i]->Vel(time-previoustime);
65  }
66  previoustime = vd[i];
67  }
68  traj = vt[vt.size()-1];
69  return traj->Vel(traj->Duration());
70  }
71 
72  Twist Trajectory_Composite::Acc(double time) const {
73  // not optimal, could be done in log(#elem)
74  unsigned int i;
75  Trajectory* traj;
76  double previoustime;
77  if (time < 0) {
78  return vt[0]->Acc(0);
79  }
80  previoustime = 0;
81  for (i=0;i<vt.size();i++) {
82  if (time < vd[i]) {
83  return vt[i]->Acc(time-previoustime);
84  }
85  previoustime = vd[i];
86  }
87  traj = vt[vt.size()-1];
88  return traj->Acc(traj->Duration());
89  }
90 
92  vt.insert(vt.end(),elem);
93  duration += elem->Duration();
94  vd.insert(vd.end(),duration);
95  }
96 
98  VectorTraj::iterator it;
99  for (it=vt.begin();it!=vt.end();it++) {
100  delete *it;
101  }
102  vt.erase(vt.begin(),vt.end());
103  vd.erase(vd.begin(),vd.end());
104  }
105 
107  Destroy();
108  }
109 
110 
111  void Trajectory_Composite::Write(ostream& os) const {
112  os << "COMPOSITE[ " << vt.size() << endl;
113  unsigned int i;
114  for (i=0;i<vt.size();i++) {
115  vt[i]->Write(os);
116  }
117  os << "]" << endl;
118  }
119 
122  for (unsigned int i = 0; i < vt.size(); ++i) {
123  comp->Add(vt[i]->Clone());
124  }
125  return comp;
126  }
127 
128 }
129 
130 
virtual double Duration() const
virtual void Add(Trajectory *elem)
virtual Twist Acc(double time) const =0
virtual Frame Pos(double time) const
represents both translational and rotational velocities.
Definition: frames.hpp:720
virtual Twist Vel(double time) const =0
virtual Twist Acc(double time) const
virtual Trajectory * Clone() const
virtual Frame Pos(double time) const =0
virtual double Duration() const =0
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
virtual void Write(std::ostream &os) const
virtual Twist Vel(double time) const


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44