Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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.resize(6,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 }