chainfksolvervel_recursive.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>
00002 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00003 
00004 // Version: 1.0
00005 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00007 // URL: http://www.orocos.org/kdl
00008 
00009 // This library is free software; you can redistribute it and/or
00010 // modify it under the terms of the GNU Lesser General Public
00011 // License as published by the Free Software Foundation; either
00012 // version 2.1 of the License, or (at your option) any later version.
00013 
00014 // This library is distributed in the hope that it will be useful,
00015 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017 // Lesser General Public License for more details.
00018 
00019 // You should have received a copy of the GNU Lesser General Public
00020 // License along with this library; if not, write to the Free Software
00021 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00022 
00023 #include "chainfksolvervel_recursive.hpp"
00024 
00025 namespace KDL
00026 {
00027     ChainFkSolverVel_recursive::ChainFkSolverVel_recursive(const Chain& _chain):
00028         chain(_chain)
00029     {
00030     }
00031 
00032     ChainFkSolverVel_recursive::~ChainFkSolverVel_recursive()
00033     {
00034     }
00035 
00036     int ChainFkSolverVel_recursive::JntToCart(const JntArrayVel& in,FrameVel& out,int seg_nr)
00037     {
00038         unsigned int segmentNr;
00039         if(seg_nr<0)
00040             segmentNr=chain.getNrOfSegments();
00041         else
00042             segmentNr = seg_nr;
00043 
00044         out=FrameVel::Identity();
00045 
00046         if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
00047             return -1;
00048         else if(segmentNr>chain.getNrOfSegments())
00049             return -1;
00050         else{
00051             int j=0;
00052             for (unsigned int i=0;i<segmentNr;i++) {
00053                 //Calculate new Frame_base_ee
00054                 if(chain.getSegment(i).getJoint().getType()!=Joint::None){
00055                     out=out*FrameVel(chain.getSegment(i).pose(in.q(j)),
00056                                      chain.getSegment(i).twist(in.q(j),in.qdot(j)));
00057                     j++;//Only increase jointnr if the segment has a joint
00058                 }else{
00059                     out=out*FrameVel(chain.getSegment(i).pose(0.0),
00060                                      chain.getSegment(i).twist(0.0,0.0));
00061                 }
00062             }
00063             return 0;
00064         }
00065     }
00066 }
00067 


orocos_kdl
Author(s):
autogenerated on Mon Oct 6 2014 03:11:16