joint.cpp
Go to the documentation of this 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       q_previous = 0;
00033     }
00034 
00035     // constructor for joint along x,y or z axis, at origin of reference frame
00036     Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
00037                  const double& _inertia, const double& _damping, const double& _stiffness):
00038       name("NoName"),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00039     {
00040       if (type == RotAxis || type == TransAxis) throw joint_type_ex;
00041       q_previous = 0;
00042     }
00043 
00044     // constructor for joint along arbitrary axis, at arbitrary origin
00045     Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, 
00046                  const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
00047       name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00048       , axis(_axis / _axis.Norm()), origin(_origin)
00049     {
00050       if (type != RotAxis && type != TransAxis) throw joint_type_ex;
00051 
00052       // initialize
00053       joint_pose.p = origin;
00054       joint_pose.M = Rotation::Rot2(axis, offset);
00055       q_previous = 0;
00056     }
00057 
00058     // constructor for joint along arbitrary axis, at arbitrary origin
00059     Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, 
00060                  const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
00061           name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness),
00062           axis(_axis / _axis.Norm()),origin(_origin)
00063     {
00064       if (type != RotAxis && type != TransAxis) throw joint_type_ex;
00065 
00066       // initialize
00067       joint_pose.p = origin;
00068       joint_pose.M = Rotation::Rot2(axis, offset);
00069       q_previous = 0;
00070     }
00071 
00072     Joint::~Joint()
00073     {
00074     }
00075 
00076     Frame Joint::pose(const double& q)const
00077     {
00078         switch(type){
00079         case RotAxis:
00080             // calculate the rotation matrix around the vector "axis"
00081             if (q != q_previous){
00082                 q_previous = q;
00083                 joint_pose.M = Rotation::Rot2(axis, scale*q+offset);
00084             }
00085             return joint_pose;
00086         case RotX:
00087             return Frame(Rotation::RotX(scale*q+offset));
00088         case RotY:
00089             return  Frame(Rotation::RotY(scale*q+offset));
00090         case RotZ:
00091             return  Frame(Rotation::RotZ(scale*q+offset));
00092         case TransAxis:
00093             return Frame(origin + (axis * (scale*q+offset)));
00094         case TransX:
00095             return  Frame(Vector(scale*q+offset,0.0,0.0));
00096         case TransY:
00097             return Frame(Vector(0.0,scale*q+offset,0.0));
00098         case TransZ:
00099             return Frame(Vector(0.0,0.0,scale*q+offset));
00100         case None:
00101             return Frame::Identity();
00102         }
00103         return Frame::Identity();
00104     }
00105 
00106     Twist Joint::twist(const double& qdot)const
00107     {
00108         switch(type){
00109         case RotAxis:
00110             return Twist(Vector(0,0,0), axis * ( scale * qdot));
00111         case RotX:
00112             return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
00113         case RotY:
00114             return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
00115         case RotZ:
00116             return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
00117         case TransAxis:
00118             return Twist(axis * (scale * qdot), Vector(0,0,0));
00119         case TransX:
00120             return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
00121         case TransY:
00122             return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
00123         case TransZ:
00124             return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
00125         case None:
00126             return Twist::Zero();
00127         }
00128         return Twist::Zero();
00129     }
00130 
00131   Vector Joint::JointAxis() const
00132   {
00133     switch(type)
00134       {
00135       case RotAxis:
00136         return axis;
00137       case RotX:
00138         return Vector(1.,0.,0.);
00139       case RotY:
00140         return Vector(0.,1.,0.);
00141       case RotZ:
00142         return Vector(0.,0.,1.);
00143       case TransAxis:
00144         return axis;
00145       case TransX:
00146         return Vector(1.,0.,0.);
00147       case TransY:
00148         return Vector(0.,1.,0.);
00149       case TransZ:
00150         return Vector(0.,0.,1.);
00151       case None:
00152         return Vector::Zero();
00153       }
00154     return Vector::Zero();
00155   }
00156 
00157   Vector Joint::JointOrigin() const
00158   {
00159     return origin;
00160   }
00161 
00162 } // end of namespace KDL
00163 


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