Blender  V3.3
jacobian.cpp
Go to the documentation of this file.
1 
4 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 
6 // Version: 1.0
7 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
8 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
9 // URL: http://www.orocos.org/kdl
10 
11 // This library is free software; you can redistribute it and/or
12 // modify it under the terms of the GNU Lesser General Public
13 // License as published by the Free Software Foundation; either
14 // version 2.1 of the License, or (at your option) any later version.
15 
16 // This library is distributed in the hope that it will be useful,
17 // but WITHOUT ANY WARRANTY; without even the implied warranty of
18 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 // Lesser General Public License for more details.
20 
21 // You should have received a copy of the GNU Lesser General Public
22 // License along with this library; if not, write to the Free Software
23 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
24 
25 #include "jacobian.hpp"
26 
27 namespace KDL
28 {
29  Jacobian::Jacobian(unsigned int _size,unsigned int _nr_blocks):
30  size(_size),nr_blocks(_nr_blocks)
31  {
32  twists = new Twist[size*nr_blocks];
33  }
34 
36  size(arg.columns()),
37  nr_blocks(arg.nr_blocks)
38  {
39  twists = new Twist[size*nr_blocks];
40  for(unsigned int i=0;i<size*nr_blocks;i++)
41  twists[i] = arg.twists[i];
42  }
43 
45  {
46  assert(size==arg.size);
47  assert(nr_blocks==arg.nr_blocks);
48  for(unsigned int i=0;i<size;i++)
49  twists[i]=arg.twists[i];
50  return *this;
51  }
52 
53 
55  {
56  delete [] twists;
57  }
58 
59  double Jacobian::operator()(int i,int j)const
60  {
61  assert(i<6*(int)nr_blocks&&j<(int)size);
62  return twists[j+6*(int)(floor((double)i/6))](i%6);
63  }
64 
65  double& Jacobian::operator()(int i,int j)
66  {
67  assert(i<6*(int)nr_blocks&&j<(int)size);
68  return twists[j+6*(int)(floor((double)i/6))](i%6);
69  }
70 
71  unsigned int Jacobian::rows()const
72  {
73  return 6*nr_blocks;
74  }
75 
76  unsigned int Jacobian::columns()const
77  {
78  return size;
79  }
80 
81  void SetToZero(Jacobian& jac)
82  {
83  for(unsigned int i=0;i<jac.size*jac.nr_blocks;i++)
84  SetToZero(jac.twists[i]);
85  }
86 
87  void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
88  {
89  assert(src1.size==dest.size);
90  assert(src1.nr_blocks==dest.nr_blocks);
91  for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
92  dest.twists[i]=src1.twists[i].RefPoint(base_AB);
93  }
94 
95  void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
96  {
97  assert(src1.size==dest.size);
98  assert(src1.nr_blocks==dest.nr_blocks);
99  for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
100  dest.twists[i]=rot*src1.twists[i];
101  }
102 
103  void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
104  {
105  assert(src1.size==dest.size);
106  assert(src1.nr_blocks==dest.nr_blocks);
107  for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
108  dest.twists[i]=frame*src1.twists[i];
109  }
110 
112  {
113  return Equal((*this),arg);
114  }
115 
117  {
118  return !Equal((*this),arg);
119  }
120 
121  bool Equal(const Jacobian& a,const Jacobian& b,double eps)
122  {
123  if(a.rows()==b.rows()&&a.columns()==b.columns()){
124  bool rc=true;
125  for(unsigned int i=0;i<a.columns();i++)
126  rc&=Equal(a.twists[i],b.twists[i],eps);
127  return rc;
128  }else
129  return false;
130  }
131 
132 }
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
bool operator==(const Jacobian &arg)
Definition: jacobian.cpp:111
double operator()(int i, int j) const
Definition: jacobian.cpp:59
Twist * twists
Definition: jacobian.hpp:39
unsigned int columns() const
Definition: jacobian.cpp:76
friend bool Equal(const Jacobian &a, const Jacobian &b, double eps)
Definition: jacobian.cpp:121
Jacobian(unsigned int size, unsigned int nr=1)
Definition: jacobian.cpp:29
Jacobian & operator=(const Jacobian &arg)
Definition: jacobian.cpp:44
bool operator!=(const Jacobian &arg)
Definition: jacobian.cpp:116
unsigned int rows() const
Definition: jacobian.cpp:71
represents rotations in 3 dimensional space.
Definition: frames.hpp:299
represents both translational and rotational velocities.
Definition: frames.hpp:679
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:322
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:143
SyclQueue void * dest
#define rot(x, k)
static unsigned a[3]
Definition: RandGen.cpp:78
Definition: chain.cpp:27
void changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:95
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:81
void changeRefFrame(const Jacobian &src1, const Frame &frame, Jacobian &dest)
Definition: jacobian.cpp:103
void changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:87
IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double=epsilon)
T floor(const T &a)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
const btScalar eps
Definition: poly34.cpp:11