orocos_kdl
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 
22  {
23  }
24 
25  double Trajectory_Composite::Duration() const{
26  return duration;
27  }
28 
29  Frame Trajectory_Composite::Pos(double time) const {
30  // not optimal, could be done in log(#elem)
31  // or one could buffer the last segment and start looking from there.
32  unsigned int i;
33  double previoustime;
34  Trajectory* traj;
35  if (time < 0) {
36  return vt[0]->Pos(0);
37  }
38  previoustime = 0;
39  for (i=0;i<vt.size();i++) {
40  if (time < vd[i]) {
41  return vt[i]->Pos(time-previoustime);
42  }
43  previoustime = vd[i];
44  }
45  traj = vt[vt.size()-1];
46  return traj->Pos(traj->Duration());
47  }
48 
49 
50  Twist Trajectory_Composite::Vel(double time) const {
51  // not optimal, could be done in log(#elem)
52  unsigned int i;
53  Trajectory* traj;
54  double previoustime;
55  if (time < 0) {
56  return vt[0]->Vel(0);
57  }
58  previoustime = 0;
59  for (i=0;i<vt.size();i++) {
60  if (time < vd[i]) {
61  return vt[i]->Vel(time-previoustime);
62  }
63  previoustime = vd[i];
64  }
65  traj = vt[vt.size()-1];
66  return traj->Vel(traj->Duration());
67  }
68 
69  Twist Trajectory_Composite::Acc(double time) const {
70  // not optimal, could be done in log(#elem)
71  unsigned int i;
72  Trajectory* traj;
73  double previoustime;
74  if (time < 0) {
75  return vt[0]->Acc(0);
76  }
77  previoustime = 0;
78  for (i=0;i<vt.size();i++) {
79  if (time < vd[i]) {
80  return vt[i]->Acc(time-previoustime);
81  }
82  previoustime = vd[i];
83  }
84  traj = vt[vt.size()-1];
85  return traj->Acc(traj->Duration());
86  }
87 
88  void Trajectory_Composite::Add(Trajectory* elem) {
89  vt.insert(vt.end(),elem);
90  duration += elem->Duration();
91  vd.insert(vd.end(),duration);
92  }
93 
94  void Trajectory_Composite::Destroy() {
95  VectorTraj::iterator it;
96  for (it=vt.begin();it!=vt.end();it++) {
97  delete *it;
98  }
99  vt.erase(vt.begin(),vt.end());
100  vd.erase(vd.begin(),vd.end());
101  }
102 
103  Trajectory_Composite::~Trajectory_Composite() {
104  Destroy();
105  }
106 
107  void Trajectory_Composite::Write(std::ostream& os) const {
108  os << "COMPOSITE[ " << vt.size() << std::endl;
109  unsigned int i;
110  for (i=0;i<vt.size();i++) {
111  vt[i]->Write(os);
112  }
113  os << "]" << std::endl;
114  }
115 
116  Trajectory* Trajectory_Composite::Clone() const{
118  for (unsigned int i = 0; i < vt.size(); ++i) {
119  comp->Add(vt[i]->Clone());
120  }
121  return comp;
122  }
123 
124 }
path_composite.hpp
KDL::Add
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:80
trajectory_composite.hpp
KDL::Trajectory::Vel
virtual Twist Vel(double time) const =0
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Trajectory_Composite::Add
virtual void Add(Trajectory *elem)
Definition: trajectory_composite.cpp:101
KDL::Trajectory_Composite
Definition: trajectory_composite.hpp:45
std::ostream
KDL::Trajectory::Pos
virtual Frame Pos(double time) const =0
KDL::Trajectory_Composite::Trajectory_Composite
Trajectory_Composite()
Definition: trajectory_composite.cpp:34
std::endl
T endl(T... args)
KDL::Trajectory::Duration
virtual double Duration() const =0
std::time
T time(T... args)
KDL::Trajectory
Definition: trajectory.hpp:129