jntarray.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 "jntarray.hpp"
00023 
00024 namespace KDL
00025 {
00026     using namespace Eigen;
00027 
00028     JntArray::JntArray()
00029     {
00030     }
00031 
00032     JntArray::JntArray(unsigned int _size):
00033         data(_size)
00034     {
00035         data.setZero();
00036     }
00037 
00038 
00039     JntArray::JntArray(const JntArray& arg):
00040         data(arg.data)
00041     {
00042     }
00043 
00044     JntArray& JntArray::operator = (const JntArray& arg)
00045     {
00046         data=arg.data;
00047         return *this;
00048     }
00049 
00050 
00051     JntArray::~JntArray()
00052     {
00053     }
00054 
00055     void JntArray::resize(unsigned int newSize)
00056     {
00057         data.resize(newSize);
00058     }
00059 
00060     double JntArray::operator()(unsigned int i,unsigned int j)const
00061     {
00062         assert(j==0);
00063         return data(i);
00064     }
00065 
00066     double& JntArray::operator()(unsigned int i,unsigned int j)
00067     {
00068         assert(j==0);
00069         return data(i);
00070     }
00071 
00072     unsigned int JntArray::rows()const
00073     {
00074         return data.rows();
00075     }
00076 
00077     unsigned int JntArray::columns()const
00078     {
00079         return data.cols();
00080     }
00081 
00082     void Add(const JntArray& src1,const JntArray& src2,JntArray& dest)
00083     {
00084         dest.data=src1.data+src2.data;
00085     }
00086 
00087     void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest)
00088     {
00089         dest.data=src1.data-src2.data;
00090     }
00091 
00092     void Multiply(const JntArray& src,const double& factor,JntArray& dest)
00093     {
00094         dest.data=factor*src.data;
00095     }
00096 
00097     void Divide(const JntArray& src,const double& factor,JntArray& dest)
00098     {
00099         dest.data=src.data/factor;
00100     }
00101 
00102     void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
00103     {
00104         Eigen::Matrix<double,6,1> t=jac.data.lazyProduct(src.data);
00105         dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5)));
00106     }
00107     
00108     void SetToZero(JntArray& array)
00109     {
00110         array.data.setZero();
00111     }
00112 
00113     bool Equal(const JntArray& src1, const JntArray& src2,double eps)
00114     {
00115         if(src1.rows()!=src2.rows())
00116             return false;
00117         return src1.data.isApprox(src2.data,eps);
00118     }
00119 
00120     bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
00121     //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
00122 
00123 }
00124 
00125 


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