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 segmentNr)
00037     {
00038 
00039         if(segmentNr<0)
00040              segmentNr=chain.getNrOfSegments();
00041 
00042         out=FrameVel::Identity();
00043 
00044         if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
00045             return -1;
00046         else if(segmentNr>chain.getNrOfSegments())
00047             return -1;
00048         else{
00049             int j=0;
00050             for (unsigned int i=0;i<segmentNr;i++) {
00051                 //Calculate new Frame_base_ee
00052                 if(chain.getSegment(i).getJoint().getType()!=Joint::None){
00053                     out=out*FrameVel(chain.getSegment(i).pose(in.q(j)),
00054                                      chain.getSegment(i).twist(in.q(j),in.qdot(j)));
00055                     j++;//Only increase jointnr if the segment has a joint
00056                 }else{
00057                     out=out*FrameVel(chain.getSegment(i).pose(0.0),
00058                                      chain.getSegment(i).twist(0.0,0.0));
00059                 }
00060             }
00061             return 0;
00062         }
00063     }
00064 }
00065 


orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:25