orocos_kdl: orocos_kdl: joint.cpp Source File

00001 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 #include "joint.hpp"
00023 
00024 namespace KDL {
00025 
00026     // constructor for joint along x,y or z axis, at origin of reference frame
00027     Joint::Joint(const std::string& _name, const JointType& _type, const double& _scale, const double& _offset,
00028                  const double& _inertia, const double& _damping, const double& _stiffness):
00029       name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00030     {
00031       if (type == RotAxis || type == TransAxis) throw joint_type_ex;
00032     }
00033 
00034     // constructor for joint along x,y or z axis, at origin of reference frame
00035     Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
00036                  const double& _inertia, const double& _damping, const double& _stiffness):
00037       name("NoName"),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00038     {
00039       if (type == RotAxis || type == TransAxis) throw joint_type_ex;
00040     }
00041 
00042     // constructor for joint along arbitrary axis, at arbitrary origin
00043     Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, 
00044                  const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
00045       name(_name), origin(_origin), axis(_axis / _axis.Norm()), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00046     {
00047       if (type != RotAxis && type != TransAxis) throw joint_type_ex;
00048 
00049       // initialize
00050       joint_pose.p = origin;
00051       joint_pose.M = Rotation::Rot2(axis, offset);
00052       q_previous = 0;
00053     }
00054 
00055     // constructor for joint along arbitrary axis, at arbitrary origin
00056     Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, 
00057                  const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
00058       name("NoName"), origin(_origin), axis(_axis / _axis.Norm()), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00059     {
00060       if (type != RotAxis && type != TransAxis) throw joint_type_ex;
00061 
00062       // initialize
00063       joint_pose.p = origin;
00064       joint_pose.M = Rotation::Rot2(axis, offset);
00065       q_previous = 0;
00066     }
00067 
00068     Joint::~Joint()
00069     {
00070     }
00071 
00072     Frame Joint::pose(const double& q)const
00073     {
00074 
00075         switch(type){
00076         case RotAxis:{
00077             // calculate the rotation matrix around the vector "axis"
00078             if (q != q_previous){
00079                 q_previous = q;
00080                 joint_pose.M = Rotation::Rot2(axis, scale*q+offset);
00081             }
00082             return joint_pose;
00083             break;}
00084         case RotX:
00085             return Frame(Rotation::RotX(scale*q+offset));
00086             break;
00087         case RotY:
00088             return  Frame(Rotation::RotY(scale*q+offset));
00089             break;
00090         case RotZ:
00091             return  Frame(Rotation::RotZ(scale*q+offset));
00092             break;
00093         case TransAxis:
00094             return Frame(origin + (axis * (scale*q+offset)));
00095             break;
00096         case TransX:
00097             return  Frame(Vector(scale*q+offset,0.0,0.0));
00098             break;
00099         case TransY:
00100             return Frame(Vector(0.0,scale*q+offset,0.0));
00101             break;
00102         case TransZ:
00103             return Frame(Vector(0.0,0.0,scale*q+offset));
00104             break;
00105         case None:
00106             return Frame::Identity();
00107             break;
00108         }
00109     }
00110 
00111     Twist Joint::twist(const double& qdot)const
00112     {
00113         switch(type){
00114         case RotAxis:
00115             return Twist(Vector(0,0,0), axis * ( scale * qdot));
00116             break;
00117         case RotX:
00118             return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
00119             break;
00120         case RotY:
00121             return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
00122             break;
00123         case RotZ:
00124             return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
00125             break;
00126         case TransAxis:
00127             return Twist(axis * (scale * qdot), Vector(0,0,0));
00128             break;
00129         case TransX:
00130             return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
00131             break;
00132         case TransY:
00133             return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
00134             break;
00135         case TransZ:
00136             return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
00137             break;
00138         case None:
00139             return Twist::Zero();
00140             break;
00141         }
00142     }
00143 
00144   Vector Joint::JointAxis() const
00145   {
00146     switch(type)
00147       {
00148       case RotAxis:
00149         return axis;
00150         break;
00151       case RotX:
00152         return Vector(1.,0.,0.);
00153         break;
00154       case RotY:
00155         return Vector(0.,1.,0.);
00156         break;
00157       case RotZ:
00158         return Vector(0.,0.,1.);
00159         break;
00160       case TransAxis:
00161         return axis;
00162         break;
00163       case TransX:
00164         return Vector(1.,0.,0.);
00165         break;
00166       case TransY:
00167         return Vector(0.,1.,0.);
00168         break;
00169       case TransZ:
00170         return Vector(0.,0.,1.);
00171         break;
00172       case None:
00173         return Vector::Zero();
00174         break;
00175       }
00176   }
00177 
00178   Vector Joint::JointOrigin() const
00179   {
00180     return origin;
00181   }
00182 
00183 } // end of namespace KDL
00184