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 (error = E_SIZE_MISMATCH);
00048         else if(segmentNr>chain.getNrOfSegments())
00049             return (error = E_OUT_OF_RANGE);
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 (error = E_NOERROR);
00064         }
00065     }
00066 
00067     int ChainFkSolverVel_recursive::JntToCart(const JntArrayVel& in,std::vector<FrameVel>& out,int seg_nr)
00068     {
00069         unsigned int segmentNr;
00070         if(seg_nr<0)
00071             segmentNr=chain.getNrOfSegments();
00072         else
00073             segmentNr = seg_nr;
00074 
00075 
00076 
00077         if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
00078             return -1;
00079         else if(segmentNr>chain.getNrOfSegments())
00080             return -1;
00081         else if(out.size()!=segmentNr)
00082             return -1;
00083         else if(segmentNr == 0)
00084             return -1;
00085         else{
00086             int j=0;
00087             // Initialization
00088             if(chain.getSegment(0).getJoint().getType()!=Joint::None){
00089                 out[0] = FrameVel(chain.getSegment(0).pose(in.q(0)),
00090                                      chain.getSegment(0).twist(in.q(0),in.qdot(0)));
00091                 j++;
00092             }else
00093                 out[0] = FrameVel(chain.getSegment(0).pose(0.0),
00094                                      chain.getSegment(0).twist(0.0,0.0));
00095             
00096             for (unsigned int i=1;i<segmentNr;i++) {
00097                 //Calculate new Frame_base_ee
00098                 if(chain.getSegment(i).getJoint().getType()!=Joint::None){
00099                     out[i]=out[i-1]*FrameVel(chain.getSegment(i).pose(in.q(j)),
00100                                     chain.getSegment(i).twist(in.q(j),in.qdot(j)));
00101                     j++;//Only increase jointnr if the segment has a joint
00102                 }else{
00103                     out[i]=out[i-1]*FrameVel(chain.getSegment(i).pose(0.0),
00104                                      chain.getSegment(i).twist(0.0,0.0));
00105                 }
00106             }
00107             return 0;
00108         }
00109     }
00110 }


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22