00001 /***************************************************************************** 00002 * \author 00003 * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 00004 * 00005 * \version 00006 * LRL V0.2 00007 * 00008 * \par History 00009 * - $log$ 00010 * 00011 * \par Release 00012 * $Id: trajectory_composite.cpp 22 2004-09-21 08:58:54Z eaertbellocal $ 00013 * $Name: $ 00014 ****************************************************************************/ 00015 00016 #include "trajectory_composite.hpp" 00017 #include "path_composite.hpp" 00018 00019 namespace KDL { 00020 00021 using namespace std; 00022 00023 00024 Trajectory_Composite::Trajectory_Composite():duration(0.0) 00025 { 00026 } 00027 00028 double Trajectory_Composite::Duration() const{ 00029 return duration; 00030 } 00031 00032 Frame Trajectory_Composite::Pos(double time) const { 00033 // not optimal, could be done in log(#elem) 00034 // or one could buffer the last segment and start looking from there. 00035 unsigned int i; 00036 double previoustime; 00037 Trajectory* traj; 00038 if (time < 0) { 00039 return vt[0]->Pos(0); 00040 } 00041 previoustime = 0; 00042 for (i=0;i<vt.size();i++) { 00043 if (time < vd[i]) { 00044 return vt[i]->Pos(time-previoustime); 00045 } 00046 previoustime = vd[i]; 00047 } 00048 traj = vt[vt.size()-1]; 00049 return traj->Pos(traj->Duration()); 00050 } 00051 00052 00053 Twist Trajectory_Composite::Vel(double time) const { 00054 // not optimal, could be done in log(#elem) 00055 unsigned int i; 00056 Trajectory* traj; 00057 double previoustime; 00058 if (time < 0) { 00059 return vt[0]->Vel(0); 00060 } 00061 previoustime = 0; 00062 for (i=0;i<vt.size();i++) { 00063 if (time < vd[i]) { 00064 return vt[i]->Vel(time-previoustime); 00065 } 00066 previoustime = vd[i]; 00067 } 00068 traj = vt[vt.size()-1]; 00069 return traj->Vel(traj->Duration()); 00070 } 00071 00072 Twist Trajectory_Composite::Acc(double time) const { 00073 // not optimal, could be done in log(#elem) 00074 unsigned int i; 00075 Trajectory* traj; 00076 double previoustime; 00077 if (time < 0) { 00078 return vt[0]->Acc(0); 00079 } 00080 previoustime = 0; 00081 for (i=0;i<vt.size();i++) { 00082 if (time < vd[i]) { 00083 return vt[i]->Acc(time-previoustime); 00084 } 00085 previoustime = vd[i]; 00086 } 00087 traj = vt[vt.size()-1]; 00088 return traj->Acc(traj->Duration()); 00089 } 00090 00091 void Trajectory_Composite::Add(Trajectory* elem) { 00092 vt.insert(vt.end(),elem); 00093 duration += elem->Duration(); 00094 vd.insert(vd.end(),duration); 00095 } 00096 00097 void Trajectory_Composite::Destroy() { 00098 VectorTraj::iterator it; 00099 for (it=vt.begin();it!=vt.end();it++) { 00100 delete *it; 00101 } 00102 vt.erase(vt.begin(),vt.end()); 00103 vd.erase(vd.begin(),vd.end()); 00104 } 00105 00106 Trajectory_Composite::~Trajectory_Composite() { 00107 Destroy(); 00108 } 00109 00110 00111 void Trajectory_Composite::Write(ostream& os) const { 00112 os << "COMPOSITE[ " << vt.size() << endl; 00113 unsigned int i; 00114 for (i=0;i<vt.size();i++) { 00115 vt[i]->Write(os); 00116 } 00117 os << "]" << endl; 00118 } 00119 00120 Trajectory* Trajectory_Composite::Clone() const{ 00121 Trajectory_Composite* comp = new Trajectory_Composite(); 00122 for (unsigned int i = 0; i < vt.size(); ++i) { 00123 comp->Add(vt[i]->Clone()); 00124 } 00125 return comp; 00126 } 00127 00128 } 00129 00130