chainiksolvervel_pinv_givens.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 
24 
25 namespace KDL
26 {
28  chain(_chain),
29  nj(chain.getNrOfJoints()),
30  jnt2jac(chain),
31  jac(nj),
32  transpose(nj>6),toggle(true),
33  m(max(6,nj)),
34  n(min(6,nj)),
35  jac_eigen(m,n),
36  U(MatrixXd::Identity(m,m)),
37  V(MatrixXd::Identity(n,n)),
38  B(m,n),
39  S(n),
40  tempi(m),
41  tempj(m),
42  UY(VectorXd::Zero(6)),
43  SUY(VectorXd::Zero(nj)),
44  qdot_eigen(nj),
45  v_in_eigen(6)
46  {
47  }
48 
52  jac.resize(nj);
53  transpose = (nj > 6);
54  m = max(6,nj);
55  n = min(6,nj);
56  jac_eigen.conservativeResize(m,n);
57  U.conservativeResizeLike(MatrixXd::Identity(m,m));
58  V.conservativeResizeLike(MatrixXd::Identity(n,n));
59  B.conservativeResize(m,n);
60  S.conservativeResize(n);
61  tempi.conservativeResize(m);
62  tempj.conservativeResize(n);
63  SUY.conservativeResizeLike(VectorXd::Zero(nj));
64  qdot_eigen.conservativeResize(nj);
65  }
66 
68  {
69  }
70 
71 
72  int ChainIkSolverVel_pinv_givens::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
73  {
74  if (nj != chain.getNrOfJoints())
75  return (error = E_NOT_UP_TO_DATE);
76 
77  if (nj != q_in.rows() || nj != qdot_out.rows())
78  return (error = E_SIZE_MISMATCH);
79 
80  toggle=!toggle;
81 
82  error = jnt2jac.JntToJac(q_in,jac);
83  if (E_NOERROR > error )
84  return error;
85 
86  for(unsigned int i=0;i<6;i++)
87  v_in_eigen(i)=v_in(i);
88 
89  for(unsigned int i=0;i<m;i++){
90  for(unsigned int j=0;j<n;j++)
91  if(transpose)
92  jac_eigen(i,j)=jac(j,i);
93  else
94  jac_eigen(i,j)=jac(i,j);
95  }
97 
98  if(transpose)
99  UY.noalias() = V.transpose() * v_in_eigen;
100  else
101  UY.noalias() = U.transpose() * v_in_eigen;
102 
103  for (unsigned int i = 0; i < n; i++){
104  double wi = UY(i);
105  double alpha = S(i);
106 
107  if (alpha != 0)
108  alpha = 1.0 / alpha;
109  else
110  alpha = 0.0;
111  SUY(i)= alpha * wi;
112  }
113  if(transpose)
114  qdot_eigen.noalias() = U * SUY;
115  else
116  qdot_eigen.noalias() = V * SUY;
117 
118  for (unsigned int j=0;j<chain.getNrOfJoints();j++)
119  qdot_out(j)=qdot_eigen(j);
120 
121  return (error = E_NOERROR);
122 
123  }
124 }
unsigned int rows() const
Definition: jntarray.cpp:72
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:55
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
Chain size changed.
Definition: solveri.hpp:97
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
int svd_eigen_Macie(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, MatrixXd &B, VectorXd &tempi, double threshold, bool toggle)
Input size does not match internal state.
Definition: solveri.hpp:99
represents both translational and rotational velocities.
Definition: frames.hpp:720
virtual void updateInternalDataStructures()
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
double min(double a, double b)
Definition: utility.h:206
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
double max(double a, double b)
Definition: utility.h:198
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)


orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36