00001 /*************************************************************************** 00002 tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 rotational_interpolation_sa.cxx 00003 00004 rotational_interpolation_sa.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: rotational_interpolation_singleaxis.cpp,v 1.1.1.1.2.2 2003/02/24 13:13:06 psoetens Exp $ 00039 * $Name: $ 00040 ****************************************************************************/ 00041 00042 00043 #include "rotational_interpolation_sa.hpp" 00044 #include "trajectory.hpp" 00045 00046 namespace KDL { 00047 00048 00049 RotationalInterpolation_SingleAxis::RotationalInterpolation_SingleAxis() 00050 {}; 00051 00052 void RotationalInterpolation_SingleAxis::SetStartEnd(Rotation start,Rotation end) { 00053 R_base_start = start; 00054 R_base_end = end; 00055 Rotation R_start_end = R_base_start.Inverse()*R_base_end; 00056 angle = R_start_end.GetRotAngle(rot_start_end); 00057 } 00058 00059 Rotation RotationalInterpolation_SingleAxis::Pos(double theta) const { 00060 return R_base_start* Rotation::Rot2(rot_start_end,theta); 00061 } 00062 00063 Vector RotationalInterpolation_SingleAxis::Vel(double theta,double thetad) const { 00064 return R_base_start * ( rot_start_end*thetad ); 00065 } 00066 00067 Vector RotationalInterpolation_SingleAxis::Acc(double theta,double thetad,double thetadd) const { 00068 return R_base_start * ( rot_start_end* thetadd); 00069 } 00070 00071 double RotationalInterpolation_SingleAxis::Angle() { 00072 return angle; 00073 } 00074 00075 void RotationalInterpolation_SingleAxis::Write(std::ostream& os) const { 00076 os << "SingleAxis[] " << std::endl; 00077 } 00078 00079 RotationalInterpolation_SingleAxis::~RotationalInterpolation_SingleAxis() { 00080 } 00081 00082 00083 RotationalInterpolation* RotationalInterpolation_SingleAxis::Clone() const { 00084 return new RotationalInterpolation_SingleAxis(); 00085 } 00086 00087 } 00088