jacobian.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
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 {
26  using namespace Eigen;
27 
29  {
30  }
31 
32 
33  Jacobian::Jacobian(unsigned int nr_of_columns):
34  data(6,nr_of_columns)
35  {
36  }
37 
39  data(arg.data)
40  {
41  }
42 
44  {
45  this->data=arg.data;
46  return *this;
47  }
48 
49 
51  {
52 
53  }
54 
55  void Jacobian::resize(unsigned int new_nr_of_columns)
56  {
57  data.conservativeResize(Eigen::NoChange,new_nr_of_columns);
58  }
59 
60  double Jacobian::operator()(unsigned int i,unsigned int j)const
61  {
62  return data(i,j);
63  }
64 
65  double& Jacobian::operator()(unsigned int i,unsigned int j)
66  {
67  return data(i,j);
68  }
69 
70  unsigned int Jacobian::rows()const
71  {
72  return data.rows();
73  }
74 
75  unsigned int Jacobian::columns()const
76  {
77  return data.cols();
78  }
79 
80  void SetToZero(Jacobian& jac)
81  {
82  jac.data.setZero();
83  }
84 
85  void Jacobian::changeRefPoint(const Vector& base_AB){
86  for(unsigned int i=0;i<data.cols();i++)
87  this->setColumn(i,this->getColumn(i).RefPoint(base_AB));
88  }
89 
90  bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
91  {
92  if(src1.columns()!=dest.columns())
93  return false;
94  for(unsigned int i=0;i<src1.columns();i++)
95  dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB));
96  return true;
97  }
98 
99  void Jacobian::changeBase(const Rotation& rot){
100  for(unsigned int i=0;i<data.cols();i++)
101  this->setColumn(i,rot*this->getColumn(i));;
102  }
103 
104  bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
105  {
106  if(src1.columns()!=dest.columns())
107  return false;
108  for(unsigned int i=0;i<src1.columns();i++)
109  dest.setColumn(i,rot*src1.getColumn(i));;
110  return true;
111  }
112 
113  void Jacobian::changeRefFrame(const Frame& frame){
114  for(unsigned int i=0;i<data.cols();i++)
115  this->setColumn(i,frame*this->getColumn(i));
116  }
117 
118  bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
119  {
120  if(src1.columns()!=dest.columns())
121  return false;
122  for(unsigned int i=0;i<src1.columns();i++)
123  dest.setColumn(i,frame*src1.getColumn(i));
124  return true;
125  }
126 
127  bool Jacobian::operator ==(const Jacobian& arg)const
128  {
129  return Equal((*this),arg);
130  }
131 
132  bool Jacobian::operator!=(const Jacobian& arg)const
133  {
134  return !Equal((*this),arg);
135  }
136 
137  bool Equal(const Jacobian& a,const Jacobian& b,double eps)
138  {
139  if(a.rows()==b.rows()&&a.columns()==b.columns()){
140  return a.data.isApprox(b.data,eps);
141  }else
142  return false;
143  }
144 
145  Twist Jacobian::getColumn(unsigned int i) const{
146  return Twist(Vector(data(0,i),data(1,i),data(2,i)),Vector(data(3,i),data(4,i),data(5,i)));
147  }
148 
149  void Jacobian::setColumn(unsigned int i,const Twist& t){
150  data.col(i).head<3>()=Eigen::Map<const Vector3d>(t.vel.data);
151  data.col(i).tail<3>()=Eigen::Map<const Vector3d>(t.rot.data);
152  }
153 
154 }
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:55
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.hpp:303
Vector vel
The velocity of that point.
Definition: frames.hpp:722
friend void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:80
double operator()(unsigned int i, unsigned int j) const
Definition: jacobian.cpp:60
unsigned int columns() const
Definition: jacobian.cpp:75
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
Definition: jacobian.hpp:41
bool operator==(const Jacobian &arg) const
Definition: jacobian.cpp:127
represents both translational and rotational velocities.
Definition: frames.hpp:720
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
double data[3]
Definition: frames.hpp:163
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:149
friend bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:104
friend bool changeRefFrame(const Jacobian &src1, const Frame &frame, Jacobian &dest)
Definition: jacobian.cpp:118
friend bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:90
Twist getColumn(unsigned int i) const
Definition: jacobian.cpp:145
Jacobian & operator=(const Jacobian &arg)
Allocates memory if size of this and argument is different.
Definition: jacobian.cpp:43
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
friend bool Equal(const Jacobian &a, const Jacobian &b, double eps)
Definition: jacobian.cpp:137
unsigned int rows() const
Definition: jacobian.cpp:70
bool operator!=(const Jacobian &arg) const
Definition: jacobian.cpp:132


orocos_kdl
Author(s):
autogenerated on Sun Mar 15 2020 03:13:51