path_line.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002   tag: Erwin Aertbelien  Mon May 10 19:10:36 CEST 2004  path_line.cxx
00003 
00004                         path_line.cxx -  description
00005                            -------------------
00006     begin                : Mon May 10 2004
00007     copyright            : (C) 2004 Erwin Aertbelien
00008     email                : erwin.aertbelien@mech.kuleuven.ac.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU Lesser General Public            *
00013  *   License as published by the Free Software Foundation; either          *
00014  *   version 2.1 of the License, or (at your option) any later version.    *
00015  *                                                                         *
00016  *   This library is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00019  *   Lesser General Public License for more details.                       *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU Lesser General Public      *
00022  *   License along with this library; if not, write to the Free Software   *
00023  *   Foundation, Inc., 59 Temple Place,                                    *
00024  *   Suite 330, Boston, MA  02111-1307  USA                                *
00025  *                                                                         *
00026  ***************************************************************************/
00027 /*****************************************************************************
00028  *  \author
00029  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
00030  *
00031  *  \version
00032  *              ORO_Geometry V0.2
00033  *
00034  *      \par History
00035  *              - $log$
00036  *
00037  *      \par Release
00038  *              $Id: path_line.cpp,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $
00039  *              $Name:  $
00040  ****************************************************************************/
00041 
00042 
00043 #include "path_line.hpp"
00044 
00045 namespace KDL {
00046 
00047 Path_Line::Path_Line(const Frame& startpos,
00048                    const Frame& endpos,
00049                    RotationalInterpolation* _orient,
00050                    double _eqradius,
00051            bool _aggregate ):
00052                            orient(_orient),
00053                            V_base_start(startpos.p),
00054                            V_base_end(endpos.p),
00055                            eqradius(_eqradius),
00056                aggregate(_aggregate)
00057    {
00058                 V_start_end = V_base_end - V_base_start;
00059                 double dist = V_start_end.Normalize();
00060                 orient->SetStartEnd(startpos.M,endpos.M);
00061                 double alpha = orient->Angle();
00062 
00063                 // See what has the slowest eq. motion, and adapt
00064                 // the other to this slower motion
00065                 // use eqradius to transform between rot and transl.
00066 
00067         // Only modify if non zero (prevent division by zero)
00068                 if ( alpha != 0 && alpha*eqradius > dist) {
00069                         // rotational_interpolation is the limitation
00070                         pathlength = alpha*eqradius;
00071                         scalerot   = 1/eqradius;
00072                         scalelin   = dist/pathlength;
00073                 } else if ( dist != 0 ) {
00074                         // translation is the limitation
00075                         pathlength = dist;
00076                         scalerot   = alpha/pathlength;
00077                         scalelin   = 1;
00078                 } else {
00079             // both were zero
00080             pathlength = 0;
00081             scalerot   = 1;
00082             scalelin   = 1;
00083         }
00084    }
00085 
00086 Path_Line::Path_Line(const Frame& startpos,
00087                    const Twist& starttwist,
00088                    RotationalInterpolation* _orient,
00089                    double _eqradius,
00090            bool _aggregate ):
00091                            orient(_orient),
00092                            V_base_start(startpos.p),
00093                            V_base_end(startpos.p + starttwist.vel),
00094                            eqradius(_eqradius),
00095                aggregate(_aggregate)
00096    {
00097        // startframe and starttwist are expressed in Wo.
00098        // after 1 time unit, startframe has translated over starttwist.vel
00099        // and rotated over starttwist.rot.Norm() (both vectors can be zero)
00100        // Thus the frame on the path after 1 time unit is defined by
00101        // startframe.Integrate(starttwist, 1);
00102                 V_start_end = V_base_end - V_base_start;
00103                 double dist = V_start_end.Normalize(); // distance traveled during 1 time unit
00104                 orient->SetStartEnd(startpos.M, (startpos*Frame( Rotation::Rot(starttwist.rot, starttwist.rot.Norm() ), starttwist.vel )).M);
00105                 double alpha = orient->Angle();        // rotation during 1 time unit
00106 
00107                 // See what has the slowest eq. motion, and adapt
00108                 // the other to this slower motion
00109                 // use eqradius to transform between rot and transl.
00110         // Only modify if non zero (prevent division by zero)
00111                 if ( alpha != 0 && alpha*eqradius > dist) {
00112                         // rotational_interpolation is the limitation
00113                         pathlength = alpha*eqradius;
00114                         scalerot   = 1/eqradius;
00115                         scalelin   = dist/pathlength;
00116                 } else if ( dist != 0 ) {
00117                         // translation is the limitation
00118                         pathlength = dist;
00119                         scalerot   = alpha/pathlength;
00120                         scalelin   = 1;
00121                 } else {
00122             // both were zero
00123             pathlength = 0;
00124             scalerot   = 1;
00125             scalelin   = 1;
00126         }
00127    }
00128 
00129 double Path_Line::LengthToS(double length) {
00130         return length/scalelin;
00131 }
00132 double Path_Line::PathLength(){
00133         return pathlength;
00134 }
00135 Frame Path_Line::Pos(double s) const  {
00136         return Frame(orient->Pos(s*scalerot),V_base_start + V_start_end*s*scalelin );
00137 }
00138 
00139 Twist Path_Line::Vel(double s,double sd) const  {
00140         return Twist( V_start_end*sd*scalelin, orient->Vel(s*scalerot,sd*scalerot) );
00141 }
00142 
00143 Twist Path_Line::Acc(double s,double sd,double sdd) const  {
00144         return Twist( V_start_end*sdd*scalelin, orient->Acc(s*scalerot,sd*scalerot,sdd*scalerot) );
00145 }
00146 
00147 
00148 Path_Line::~Path_Line() {
00149     if (aggregate)
00150         delete orient;
00151 }
00152 
00153 Path* Path_Line::Clone() {
00154     if (aggregate )
00155         return new Path_Line(
00156                              Frame(orient->Pos(0),V_base_start),
00157                              Frame(orient->Pos(pathlength*scalerot),V_base_end),
00158                              orient->Clone(),
00159                              eqradius,
00160                              true
00161                              );
00162     // else :
00163     return new Path_Line(
00164                          Frame(orient->Pos(0),V_base_start),
00165                          Frame(orient->Pos(pathlength*scalerot),V_base_end),
00166                          orient,
00167                          eqradius,
00168                          false
00169                          );
00170 
00171 }
00172 
00173 void Path_Line::Write(std::ostream& os)  {
00174         os << "LINE[ ";
00175         os << "  " << Frame(orient->Pos(0),V_base_start) << std::endl;
00176         os << "  " << Frame(orient->Pos(pathlength*scalerot),V_base_end) << std::endl;
00177         os << "  ";orient->Write(os);
00178         os << "  " << eqradius;
00179         os << "]"  << std::endl;
00180 }
00181 
00182 
00183 }
00184 


orocos_kdl
Author(s):
autogenerated on Sat Oct 7 2017 03:04:28