orocos_kdl
jacobian.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
5 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #include "jacobian.hpp"
23 
24 namespace KDL
25 {
27  {
28  }
29 
30 
31  Jacobian::Jacobian(unsigned int nr_of_columns):
32  data(6,nr_of_columns)
33  {
34  data.setZero();
35  }
36 
38  data(arg.data)
39  {
40  }
41 
43  {
44  this->data=arg.data;
45  return *this;
46  }
47 
48 
50  {
51 
52  }
53 
54  void Jacobian::resize(unsigned int new_nr_of_columns)
55  {
56  data.conservativeResize(Eigen::NoChange,new_nr_of_columns);
57  }
58 
59  double Jacobian::operator()(unsigned int i,unsigned int j)const
60  {
61  return data(i,j);
62  }
63 
64  double& Jacobian::operator()(unsigned int i,unsigned int j)
65  {
66  return data(i,j);
67  }
68 
69  unsigned int Jacobian::rows()const
70  {
71  return static_cast<unsigned int>(data.rows());
72  }
73 
74  unsigned int Jacobian::columns()const
75  {
76  return static_cast<unsigned int>(data.cols());
77  }
78 
79  void SetToZero(Jacobian& jac)
80  {
81  jac.data.setZero();
82  }
83 
84  void Jacobian::changeRefPoint(const Vector& base_AB){
85  for(unsigned int i=0;i<data.cols();i++)
86  this->setColumn(i,this->getColumn(i).RefPoint(base_AB));
87  }
88 
89  bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
90  {
91  if(src1.columns()!=dest.columns())
92  return false;
93  for(unsigned int i=0;i<src1.columns();i++)
94  dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB));
95  return true;
96  }
97 
98  void Jacobian::changeBase(const Rotation& rot){
99  for(unsigned int i=0;i<data.cols();i++)
100  this->setColumn(i,rot*this->getColumn(i));;
101  }
102 
103  bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
104  {
105  if(src1.columns()!=dest.columns())
106  return false;
107  for(unsigned int i=0;i<src1.columns();i++)
108  dest.setColumn(i,rot*src1.getColumn(i));;
109  return true;
110  }
111 
112  void Jacobian::changeRefFrame(const Frame& frame){
113  for(unsigned int i=0;i<data.cols();i++)
114  this->setColumn(i,frame*this->getColumn(i));
115  }
116 
117  bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
118  {
119  if(src1.columns()!=dest.columns())
120  return false;
121  for(unsigned int i=0;i<src1.columns();i++)
122  dest.setColumn(i,frame*src1.getColumn(i));
123  return true;
124  }
125 
126  bool Jacobian::operator ==(const Jacobian& arg)const
127  {
128  return Equal((*this),arg);
129  }
130 
131  bool Jacobian::operator!=(const Jacobian& arg)const
132  {
133  return !Equal((*this),arg);
134  }
135 
136  bool Equal(const Jacobian& a,const Jacobian& b,double eps)
137  {
138  if(a.rows()==b.rows()&&a.columns()==b.columns()){
139  return (a.data-b.data).isZero(eps);
140  }else
141  return false;
142  }
143 
144  Twist Jacobian::getColumn(unsigned int i) const{
145  return Twist(Vector(data(0,i),data(1,i),data(2,i)),Vector(data(3,i),data(4,i),data(5,i)));
146  }
147 
148  void Jacobian::setColumn(unsigned int i,const Twist& t){
149  data.col(i).head<3>()=Eigen::Map<const Eigen::Vector3d>(t.vel.data);
150  data.col(i).tail<3>()=Eigen::Map<const Eigen::Vector3d>(t.rot.data);
151  }
152 
153 }
KDL::Vector::data
double data[3]
Definition: frames.hpp:165
KDL::Jacobian::Jacobian
Jacobian()
Definition: jacobian.cpp:26
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:74
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::Jacobian::setColumn
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:148
KDL::Jacobian::operator()
double operator()(unsigned int i, unsigned int j) const
Definition: jacobian.cpp:59
KDL::Jacobian::changeBase
friend bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:103
KDL::Jacobian::operator==
bool operator==(const Jacobian &arg) const
Definition: jacobian.cpp:126
KDL::changeRefFrame
bool changeRefFrame(const Jacobian &src1, const Frame &frame, Jacobian &dest)
Definition: jacobian.cpp:117
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Jacobian::changeRefPoint
friend bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:89
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::Jacobian::resize
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:54
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::Jacobian::operator!=
bool operator!=(const Jacobian &arg) const
Definition: jacobian.cpp:131
KDL::Jacobian::~Jacobian
~Jacobian()
Definition: jacobian.cpp:49
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::Jacobian::rows
unsigned int rows() const
Definition: jacobian.cpp:69
KDL::changeBase
bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:103
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::Twist::RefPoint
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:302
KDL::Jacobian::data
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
Definition: jacobian.hpp:41
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::changeRefPoint
bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:89
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::Jacobian::Equal
friend bool Equal(const Jacobian &a, const Jacobian &b, double eps)
Definition: jacobian.cpp:136
jacobian.hpp
KDL::Jacobian::getColumn
Twist getColumn(unsigned int i) const
Definition: jacobian.cpp:144
KDL::Jacobian::changeRefFrame
friend bool changeRefFrame(const Jacobian &src1, const Frame &frame, Jacobian &dest)
Definition: jacobian.cpp:117
KDL::Jacobian::operator=
Jacobian & operator=(const Jacobian &arg)
Allocates memory if size of this and argument is different.
Definition: jacobian.cpp:42
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303
KDL::Jacobian
Definition: jacobian.hpp:36