orocos_kdl
chainiksolvervel_pinv.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 
23 
24 namespace KDL
25 {
26  ChainIkSolverVel_pinv::ChainIkSolverVel_pinv(const Chain& _chain,double _eps,int _maxiter):
27  chain(_chain),
28  jnt2jac(chain),
29  nj(chain.getNrOfJoints()),
30  jac(nj),
31  svd(jac),
32  U(6,JntArray(nj)),
33  S(nj),
34  V(nj,JntArray(nj)),
35  tmp(nj),
36  eps(_eps),
37  maxiter(_maxiter),
38  nrZeroSigmas(0),
39  svdResult(0)
40  {
41  }
42 
46  jac.resize(nj);
47  svd = SVD_HH(jac);
48  for(unsigned int i = 0 ; i < U.size(); i++)
49  U[i].resize(nj);
50  S.resize(nj);
51  V.resize(nj);
52  for(unsigned int i = 0 ; i < V.size(); i++)
53  V[i].resize(nj);
54  tmp.resize(nj);
55  }
56 
58  {
59  }
60 
61 
62  int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
63  {
64  if (nj != chain.getNrOfJoints())
65  return (error = E_NOT_UP_TO_DATE);
66 
67  if (nj != q_in.rows() || nj != qdot_out.rows())
68  return (error = E_SIZE_MISMATCH);
69 
70  //Let the ChainJntToJacSolver calculate the jacobian "jac" for
71  //the current joint positions "q_in"
72  error = jnt2jac.JntToJac(q_in,jac);
73  if (error < E_NOERROR) return error;
74 
75  double sum;
76  unsigned int i,j;
77 
78  // Initialize near zero singular value counter
79  nrZeroSigmas = 0 ;
80 
81  //Do a singular value decomposition of "jac" with maximum
82  //iterations "maxiter", put the results in "U", "S" and "V"
83  //jac = U*S*Vt
85  if (0 != svdResult)
86  {
87  qdot_out.data.setZero();
88  return (error = E_SVD_FAILED);
89  }
90 
91  // We have to calculate qdot_out = jac_pinv*v_in
92  // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
93  // qdot_out = V*S_pinv*Ut*v_in
94 
95  //first we calculate Ut*v_in
96  for (i=0;i<jac.columns();i++) {
97  sum = 0.0;
98  for (j=0;j<jac.rows();j++) {
99  sum+= U[j](i)*v_in(j);
100  }
101  //If the singular value is too small (<eps), don't invert it but
102  //set the inverted singular value to zero (truncated svd)
103  if ( fabs(S(i))<eps ) {
104  tmp(i) = 0.0 ;
105  // Count number of singular values near zero
106  ++nrZeroSigmas ;
107  }
108  else {
109  tmp(i) = sum/S(i) ;
110  }
111  }
112  //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
113  //it with V to get qdot_out
114  for (i=0;i<jac.columns();i++) {
115  sum = 0.0;
116  for (j=0;j<jac.columns();j++) {
117  sum+=V[i](j)*tmp(j);
118  }
119  //Put the result in qdot_out
120  qdot_out(i)=sum;
121  }
122 
123  // Note if the solution is singular, i.e. if number of near zero
124  // singular values is greater than the full rank of jac
125  if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
126  return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular
127  } else {
128  return (error = E_NOERROR); // have converged
129  }
130  }
131 
132  const char* ChainIkSolverVel_pinv::strError(const int error) const
133  {
134  if (E_CONVERGE_PINV_SINGULAR == error) return "Converged put pseudo inverse of jacobian is singular.";
135  else return SolverI::strError(error);
136  }
137 }
KDL::ChainIkSolverVel_pinv::svd
SVD_HH svd
Definition: chainiksolvervel_pinv.hpp:105
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:74
KDL::SolverI::strError
virtual const char * strError(const int error) const
Definition: solveri.hpp:125
KDL::ChainIkSolverVel_pinv::nrZeroSigmas
unsigned int nrZeroSigmas
Definition: chainiksolvervel_pinv.hpp:112
KDL::ChainIkSolverVel_pinv::jac
Jacobian jac
Definition: chainiksolvervel_pinv.hpp:104
KDL::JntArray
Definition: jntarray.hpp:69
KDL::ChainIkSolverVel_pinv::strError
virtual const char * strError(const int error) const
Definition: chainiksolvervel_pinv.cpp:132
KDL::SVD_HH
Definition: svd_HH.hpp:32
KDL::JntArray::resize
void resize(unsigned int newSize)
Definition: jntarray.cpp:53
KDL::ChainIkSolverVel_pinv::eps
double eps
Definition: chainiksolvervel_pinv.hpp:110
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::ChainIkSolverVel_pinv::svdResult
int svdResult
Definition: chainiksolvervel_pinv.hpp:113
KDL::ChainIkSolverVel_pinv::maxiter
int maxiter
Definition: chainiksolvervel_pinv.hpp:111
KDL::ChainIkSolverVel_pinv::V
std::vector< JntArray > V
Definition: chainiksolvervel_pinv.hpp:108
KDL::ChainJntToJacSolver::JntToJac
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
Definition: chainjnttojacsolver.cpp:48
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::ChainIkSolverVel_pinv::jnt2jac
ChainJntToJacSolver jnt2jac
Definition: chainiksolvervel_pinv.hpp:102
KDL::ChainIkSolverVel_pinv::nj
unsigned int nj
Definition: chainiksolvervel_pinv.hpp:103
KDL::ChainIkSolverVel_pinv::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolvervel_pinv.cpp:43
KDL::Jacobian::resize
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:54
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
KDL::ChainIkSolverVel_pinv::U
std::vector< JntArray > U
Definition: chainiksolvervel_pinv.hpp:106
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::SolverI::E_SVD_FAILED
@ E_SVD_FAILED
Internal svd calculation failed.
Definition: solveri.hpp:107
KDL::Jacobian::rows
unsigned int rows() const
Definition: jacobian.cpp:69
KDL::ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR
static const int E_CONVERGE_PINV_SINGULAR
solution converged but (pseudo)inverse is singular
Definition: chainiksolvervel_pinv.hpp:44
KDL::JntArray::data
Eigen::VectorXd data
Definition: jntarray.hpp:72
KDL::ChainIkSolverVel_pinv::chain
const Chain & chain
Definition: chainiksolvervel_pinv.hpp:101
KDL::ChainJntToJacSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainjnttojacsolver.cpp:31
KDL::ChainIkSolverVel_pinv::CartToJnt
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
Definition: chainiksolvervel_pinv.cpp:62
KDL::ChainIkSolverVel_pinv::tmp
JntArray tmp
Definition: chainiksolvervel_pinv.hpp:109
chainiksolvervel_pinv.hpp
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::SVD_HH::calculate
int calculate(const Jacobian &jac, std::vector< JntArray > &U, JntArray &w, std::vector< JntArray > &v, int maxiter)
Definition: svd_HH.cpp:60
KDL::ChainIkSolverVel_pinv::ChainIkSolverVel_pinv
ChainIkSolverVel_pinv(const Chain &chain, double eps=0.00001, int maxiter=150)
Definition: chainiksolvervel_pinv.cpp:26
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
KDL::ChainIkSolverVel_pinv::S
JntArray S
Definition: chainiksolvervel_pinv.hpp:107
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::ChainIkSolverVel_pinv::~ChainIkSolverVel_pinv
~ChainIkSolverVel_pinv()
Definition: chainiksolvervel_pinv.cpp:57