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


orocos_kdl
Author(s):
autogenerated on Wed Jul 1 2020 03:17:39