trajectory_composite.cpp
Go to the documentation of this file.
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 


orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:25