jacobian.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 "jacobian.hpp"
00023 
00024 namespace KDL
00025 {
00026     using namespace Eigen;
00027 
00028     Jacobian::Jacobian()
00029     {
00030     }
00031 
00032 
00033     Jacobian::Jacobian(unsigned int nr_of_columns):
00034         data(6,nr_of_columns)
00035     {
00036     }
00037     
00038     Jacobian::Jacobian(const Jacobian& arg):
00039         data(arg.data)
00040     {
00041     }
00042 
00043     Jacobian& Jacobian::operator = (const Jacobian& arg)
00044     { 
00045         this->data=arg.data;
00046         return *this;
00047     }
00048 
00049 
00050     Jacobian::~Jacobian()
00051     {
00052         
00053     }
00054 
00055     void Jacobian::resize(unsigned int new_nr_of_columns)
00056     {
00057         data.conservativeResize(Eigen::NoChange,new_nr_of_columns);
00058     }
00059 
00060     double Jacobian::operator()(unsigned int i,unsigned int j)const
00061     {
00062         return data(i,j);
00063     }
00064 
00065     double& Jacobian::operator()(unsigned int i,unsigned int j)
00066     {
00067         return data(i,j);
00068     }
00069 
00070     unsigned int Jacobian::rows()const
00071     {
00072         return data.rows();
00073     }
00074 
00075     unsigned int Jacobian::columns()const
00076     {
00077         return data.cols();
00078     }
00079 
00080     void SetToZero(Jacobian& jac)
00081     {
00082         jac.data.setZero();
00083     }
00084 
00085     void Jacobian::changeRefPoint(const Vector& base_AB){
00086         for(unsigned int i=0;i<data.cols();i++)
00087             this->setColumn(i,this->getColumn(i).RefPoint(base_AB));
00088     }
00089 
00090     bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
00091     {
00092         if(src1.columns()!=dest.columns())
00093             return false;
00094         for(unsigned int i=0;i<src1.columns();i++)
00095             dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB));
00096         return true;
00097     }
00098     
00099     void Jacobian::changeBase(const Rotation& rot){
00100         for(unsigned int i=0;i<data.cols();i++)
00101             this->setColumn(i,rot*this->getColumn(i));;
00102     }
00103 
00104     bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
00105     {
00106         if(src1.columns()!=dest.columns())
00107             return false;
00108         for(unsigned int i=0;i<src1.columns();i++)
00109             dest.setColumn(i,rot*src1.getColumn(i));;
00110         return true;
00111     }
00112 
00113     void Jacobian::changeRefFrame(const Frame& frame){
00114         for(unsigned int i=0;i<data.cols();i++)
00115             this->setColumn(i,frame*this->getColumn(i));
00116     }
00117     
00118     bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
00119     {
00120         if(src1.columns()!=dest.columns())
00121             return false;
00122         for(unsigned int i=0;i<src1.columns();i++)
00123             dest.setColumn(i,frame*src1.getColumn(i));
00124         return true;
00125     }
00126 
00127     bool Jacobian::operator ==(const Jacobian& arg)const
00128     {
00129         return Equal((*this),arg);
00130     }
00131     
00132     bool Jacobian::operator!=(const Jacobian& arg)const
00133     {
00134         return !Equal((*this),arg);
00135     }
00136     
00137     bool Equal(const Jacobian& a,const Jacobian& b,double eps)
00138     {
00139         if(a.rows()==b.rows()&&a.columns()==b.columns()){
00140             return a.data.isApprox(b.data,eps);
00141         }else
00142             return false;
00143     }
00144     
00145     Twist Jacobian::getColumn(unsigned int i) const{
00146         return Twist(Vector(data(0,i),data(1,i),data(2,i)),Vector(data(3,i),data(4,i),data(5,i)));
00147     }
00148     
00149     void Jacobian::setColumn(unsigned int i,const Twist& t){
00150         data.col(i).head<3>()=Eigen::Map<const Vector3d>(t.vel.data);
00151         data.col(i).tail<3>()=Eigen::Map<const Vector3d>(t.rot.data);
00152     }
00153 
00154 }


orocos_kdl
Author(s):
autogenerated on Sat Nov 3 2018 02:33:14