1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110
|
/***************************************************************************
tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 trajectory_segment.cxx
trajectory_segment.cxx - description
-------------------
begin : Mon May 10 2004
copyright : (C) 2004 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Lesser General Public *
* License as published by the Free Software Foundation; either *
* version 2.1 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: trajectory_segment.cpp,v 1.1.1.1.2.7 2003/07/23 16:44:26 psoetens Exp $
* $Name: $
****************************************************************************/
#include "trajectory_segment.hpp"
namespace KDL {
Trajectory_Segment::Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, bool _aggregate):
motprof(_motprof),geom(_geom), aggregate(_aggregate)
{
// assume everything is set or at least check if Duration() != 0
}
Trajectory_Segment::Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, double _duration, bool _aggregate):
motprof(_motprof),geom(_geom), aggregate(_aggregate)
{
// the duration was specified so assume motprof not yet set.
motprof->SetProfileDuration(0, geom->PathLength(), _duration);
}
Path* Trajectory_Segment::GetPath()
{
return geom;
}
VelocityProfile* Trajectory_Segment::GetProfile()
{
return motprof;
}
double Trajectory_Segment::Duration() const
{
return motprof->Duration();
}
Frame Trajectory_Segment::Pos(double time) const
{
return geom->Pos(motprof->Pos(time));
}
Twist Trajectory_Segment::Vel(double time) const
{
return geom->Vel(motprof->Pos(time),motprof->Vel(time));
}
Twist Trajectory_Segment::Acc(double time) const
{
return geom->Acc(motprof->Pos(time),motprof->Vel(time),motprof->Acc(time));
}
void Trajectory_Segment::Write(std::ostream& os) const
{
os << "SEGMENT[ " << std::endl;
os << " ";geom->Write(os);os << std::endl;
os << " ";motprof->Write(os);os << std::endl;
os << "]";
}
Trajectory_Segment::~Trajectory_Segment()
{
if (aggregate)
{
delete motprof;
delete geom;
}
}
}
|