Blender V2.61 - r43446

joint.cpp

Go to the documentation of this file.
00001 
00004 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 
00006 // Version: 1.0
00007 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00008 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00009 // URL: http://www.orocos.org/kdl
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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00024 
00025 #include "joint.hpp"
00026 
00027 namespace KDL {
00028 
00029     Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
00030                  const double& _inertia, const double& _damping, const double& _stiffness):
00031         type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00032     {
00033         // for sphere and swing, offset is not used, assume no offset
00034     }
00035 
00036     Joint::Joint(const Joint& in):
00037     type(in.type),scale(in.scale),offset(in.offset),
00038         inertia(in.inertia),damping(in.damping),stiffness(in.stiffness)
00039     {
00040     }
00041 
00042     Joint& Joint::operator=(const Joint& in)
00043     {
00044         type=in.type;
00045         scale=in.scale;
00046         offset=in.offset;
00047         inertia=in.inertia;
00048         damping=in.damping;
00049         stiffness=in.stiffness;
00050         return *this;
00051     }
00052 
00053 
00054     Joint::~Joint()
00055     {
00056     }
00057 
00058     Frame Joint::pose(const double& q)const
00059     {
00060 
00061         switch(type){
00062         case RotX:
00063             return Frame(Rotation::RotX(scale*q+offset));
00064             break;
00065         case RotY:
00066             return  Frame(Rotation::RotY(scale*q+offset));
00067             break;
00068         case RotZ:
00069             return  Frame(Rotation::RotZ(scale*q+offset));
00070             break;
00071         case TransX:
00072             return  Frame(Vector(scale*q+offset,0.0,0.0));
00073             break;
00074         case TransY:
00075             return Frame(Vector(0.0,scale*q+offset,0.0));
00076             break;
00077         case TransZ:
00078             return Frame(Vector(0.0,0.0,scale*q+offset));
00079             break;
00080         case Sphere:
00081             // the joint angles represent a rotation vector expressed in the base frame of the joint
00082             // (= the frame you get when there is no offset nor rotation)
00083             return Frame(Rot(Vector((&q)[0], (&q)[1], (&q)[2])));
00084             break;
00085         case Swing:
00086             // the joint angles represent a 2D rotation vector in the XZ planee of the base frame of the joint
00087             // (= the frame you get when there is no offset nor rotation)
00088             return Frame(Rot(Vector((&q)[0], 0.0, (&q)[1])));
00089             break;
00090         default:
00091             return Frame::Identity();
00092             break;
00093         }
00094     }
00095 
00096     Twist Joint::twist(const double& qdot, int dof)const
00097     {
00098         switch(type){
00099         case RotX:
00100             return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
00101             break;
00102         case RotY:
00103             return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
00104             break;
00105         case RotZ:
00106             return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
00107             break;
00108         case TransX:
00109             return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
00110             break;
00111         case TransY:
00112             return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
00113             break;
00114         case TransZ:
00115             return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
00116             break;
00117         case Swing:
00118             switch (dof) {
00119             case 0:
00120                 return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
00121             case 1:
00122                 return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
00123             }
00124             return Twist::Zero();
00125         case Sphere:
00126             switch (dof) {
00127             case 0:
00128                 return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
00129             case 1:
00130                 return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
00131             case 2:
00132                 return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
00133             }
00134             return Twist::Zero();
00135         default:
00136             return Twist::Zero();
00137             break;
00138         }
00139     }
00140 
00141     unsigned int Joint::getNDof() const
00142     {
00143         switch (type) {
00144         case Sphere:
00145             return 3;
00146         case Swing:
00147             return 2;
00148         case None:
00149             return 0;
00150         default:
00151             return 1;
00152         }
00153     }
00154 
00155 } // end of namespace KDL
00156