$search
00001 /*************************************************************************** 00002 tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_circle.cxx 00003 00004 path_circle.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_circle.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $ 00039 * $Name: $ 00040 ****************************************************************************/ 00041 00042 00043 #include "path_circle.hpp" 00044 #include "utilities/error.h" 00045 00046 namespace KDL { 00047 00048 00049 00050 Path_Circle::Path_Circle(const Frame& F_base_start, 00051 const Vector& _V_base_center, 00052 const Vector& V_base_p, 00053 const Rotation& R_base_end, 00054 double alpha, 00055 RotationalInterpolation* _orient, 00056 double _eqradius, 00057 bool _aggregate) : 00058 orient(_orient) , 00059 eqradius(_eqradius), 00060 aggregate(_aggregate) 00061 { 00062 F_base_center.p = _V_base_center; 00063 orient->SetStartEnd(F_base_start.M,R_base_end); 00064 double oalpha = orient->Angle(); 00065 00066 Vector x(F_base_start.p - F_base_center.p); 00067 radius = x.Normalize(); 00068 if (radius < epsilon) throw Error_MotionPlanning_Circle_ToSmall(); 00069 Vector tmpv(V_base_p-F_base_center.p); 00070 tmpv.Normalize(); 00071 Vector z( x * tmpv); 00072 double n = z.Normalize(); 00073 if (n < epsilon) throw Error_MotionPlanning_Circle_No_Plane(); 00074 F_base_center.M = Rotation(x,z*x,z); 00075 double dist = alpha*radius; 00076 // See what has the slowest eq. motion, and adapt 00077 // the other to this slower motion 00078 // use eqradius to transform between rot and transl. 00079 // the same as for lineair motion 00080 if (oalpha*eqradius > dist) { 00081 // rotational_interpolation is the limitation 00082 pathlength = oalpha*eqradius; 00083 scalerot = 1/eqradius; 00084 scalelin = dist/pathlength; 00085 } else { 00086 // translation is the limitation 00087 pathlength = dist; 00088 scalerot = oalpha/pathlength; 00089 scalelin = 1; 00090 } 00091 } 00092 00093 00094 00095 double Path_Circle::LengthToS(double length) { 00096 return length/scalelin; 00097 } 00098 00099 00100 double Path_Circle::PathLength() { 00101 return pathlength; 00102 } 00103 00104 Frame Path_Circle::Pos(double s) const { 00105 double p = s*scalelin / radius; 00106 return Frame(orient->Pos(s*scalerot), 00107 F_base_center*Vector(radius*cos(p),radius*sin(p),0) 00108 ); 00109 00110 } 00111 00112 Twist Path_Circle::Vel(double s,double sd) const { 00113 double p = s*scalelin / radius; 00114 double v = sd*scalelin / radius; 00115 return Twist( F_base_center.M*Vector(-radius*sin(p)*v,radius*cos(p)*v,0), 00116 orient->Vel(s*scalerot,sd*scalerot) 00117 ); 00118 } 00119 00120 Twist Path_Circle::Acc(double s,double sd,double sdd) const { 00121 double p = s*scalelin / radius; 00122 double cp = cos(p); 00123 double sp = sin(p); 00124 double v = sd*scalelin / radius; 00125 double a = sdd*scalelin / radius; 00126 return Twist( F_base_center.M*Vector( 00127 -radius*cp*v*v - radius*sp*a, 00128 -radius*sp*v*v + radius*cp*a, 00129 0 00130 ), 00131 orient->Acc(s*scalerot,sd*scalerot,sdd*scalerot) 00132 ); 00133 } 00134 00135 Path* Path_Circle::Clone() { 00136 return new Path_Circle( 00137 Pos(0), 00138 F_base_center.p, 00139 F_base_center.M.UnitY(), 00140 orient->Pos(pathlength*scalerot), 00141 pathlength*scalelin/radius/deg2rad, 00142 orient->Clone(), 00143 eqradius, 00144 aggregate 00145 ); 00146 } 00147 00148 Path_Circle::~Path_Circle() { 00149 if (aggregate) 00150 delete orient; 00151 } 00152 00153 00154 00155 void Path_Circle::Write(std::ostream& os) { 00156 os << "CIRCLE[ "; 00157 os << " " << Pos(0) << std::endl; 00158 os << " " << F_base_center.p << std::endl; 00159 os << " " << F_base_center.M.UnitY() << std::endl; 00160 os << " " << orient->Pos(pathlength*scalerot) << std::endl; 00161 os << " " << pathlength*scalelin/radius/deg2rad << std::endl; 00162 os << " ";orient->Write(os); 00163 os << " " << eqradius; 00164 os << "]"<< std::endl; 00165 } 00166 00167 00168 } 00169