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  UY(VectorXd::Zero(6)),
42  SUY(VectorXd::Zero(nj)),
43  qdot_eigen(nj),
44  v_in_eigen(6)
45  {
46  }
47 
51  jac.resize(nj);
52  transpose = (nj > 6);
53  m = max(6,nj);
54  n = min(6,nj);
55  jac_eigen.conservativeResize(m,n);
56  U.conservativeResizeLike(MatrixXd::Identity(m,m));
57  V.conservativeResizeLike(MatrixXd::Identity(n,n));
58  B.conservativeResize(m,n);
59  S.conservativeResize(n);
60  tempi.conservativeResize(m);
61  SUY.conservativeResizeLike(VectorXd::Zero(nj));
62  qdot_eigen.conservativeResize(nj);
63  }
64 
66  {
67  }
68 
69 
70  int ChainIkSolverVel_pinv_givens::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
71  {
72  if (nj != chain.getNrOfJoints())
73  return (error = E_NOT_UP_TO_DATE);
74 
75  if (nj != q_in.rows() || nj != qdot_out.rows())
76  return (error = E_SIZE_MISMATCH);
77 
78  toggle=!toggle;
79 
80  error = jnt2jac.JntToJac(q_in,jac);
81  if (E_NOERROR > error )
82  return error;
83 
84  for(unsigned int i=0;i<6;i++)
85  v_in_eigen(i)=v_in(i);
86 
87  for(unsigned int i=0;i<m;i++){
88  for(unsigned int j=0;j<n;j++)
89  if(transpose)
90  jac_eigen(i,j)=jac(j,i);
91  else
92  jac_eigen(i,j)=jac(i,j);
93  }
95 
96  if(transpose)
97  UY.noalias() = V.transpose() * v_in_eigen;
98  else
99  UY.noalias() = U.transpose() * v_in_eigen;
100 
101  for (unsigned int i = 0; i < n; i++){
102  double wi = UY(i);
103  double alpha = S(i);
104 
105  if (alpha != 0)
106  alpha = 1.0 / alpha;
107  else
108  alpha = 0.0;
109  SUY(i)= alpha * wi;
110  }
111  if(transpose)
112  qdot_eigen.noalias() = U * SUY;
113  else
114  qdot_eigen.noalias() = V * SUY;
115 
116  for (unsigned int j=0;j<chain.getNrOfJoints();j++)
117  qdot_out(j)=qdot_eigen(j);
118 
119  return (error = E_NOERROR);
120 
121  }
122 }
int svd_eigen_Macie(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::MatrixXd &B, Eigen::VectorXd &tempi, double threshold, bool toggle)
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:56
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
Input size does not match internal state.
Definition: solveri.hpp:99
represents both translational and rotational velocities.
Definition: frames.hpp:720
virtual void updateInternalDataStructures()
double min(double a, double b)
Definition: utility.h:212
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
double max(double a, double b)
Definition: utility.h:204
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:43