$search
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