chainiksolvervel_wdls.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 {
27 
28  ChainIkSolverVel_wdls::ChainIkSolverVel_wdls(const Chain& _chain,double _eps,int _maxiter):
29  chain(_chain),
30  jnt2jac(chain),
31  nj(chain.getNrOfJoints()),
32  jac(nj),
33  U(Eigen::MatrixXd::Zero(6,nj)),
34  S(Eigen::VectorXd::Zero(nj)),
35  V(Eigen::MatrixXd::Zero(nj,nj)),
36  eps(_eps),
37  maxiter(_maxiter),
38  tmp(Eigen::VectorXd::Zero(nj)),
39  tmp_jac(Eigen::MatrixXd::Zero(6,nj)),
40  tmp_jac_weight1(Eigen::MatrixXd::Zero(6,nj)),
41  tmp_jac_weight2(Eigen::MatrixXd::Zero(6,nj)),
42  tmp_ts(Eigen::MatrixXd::Zero(6,6)),
43  tmp_js(Eigen::MatrixXd::Zero(nj,nj)),
44  weight_ts(Eigen::MatrixXd::Identity(6,6)),
45  weight_js(Eigen::MatrixXd::Identity(nj,nj)),
46  lambda(0.0),
47  lambda_scaled(0.0),
48  nrZeroSigmas(0),
49  svdResult(0),
50  sigmaMin(0)
51  {
52  }
53 
57  jac.resize(nj);
58  Eigen::MatrixXd z6nj = Eigen::MatrixXd::Zero(6,nj);
59  Eigen::VectorXd znj = Eigen::VectorXd::Zero(nj);
60  Eigen::MatrixXd znjnj = Eigen::MatrixXd::Zero(nj,nj);
61  U.conservativeResizeLike(z6nj);
62  S.conservativeResizeLike(znj);
63  V.conservativeResizeLike(znjnj);
64  tmp.conservativeResizeLike(znj);
65  tmp_jac.conservativeResizeLike(z6nj);
66  tmp_jac_weight1.conservativeResizeLike(z6nj);
67  tmp_jac_weight2.conservativeResizeLike(z6nj);
68  tmp_js.conservativeResizeLike(znjnj);
69  weight_js.conservativeResizeLike(Eigen::MatrixXd::Identity(nj,nj));
70  }
71 
73  {
74  }
75 
76  int ChainIkSolverVel_wdls::setWeightJS(const Eigen::MatrixXd& Mq){
77  if(nj != chain.getNrOfJoints())
78  return (error = E_NOT_UP_TO_DATE);
79 
80  if (Mq.size() != weight_js.size())
81  return (error = E_SIZE_MISMATCH);
82  weight_js = Mq;
83  return (error = E_NOERROR);
84  }
85 
86  int ChainIkSolverVel_wdls::setWeightTS(const Eigen::MatrixXd& Mx){
87  if (Mx.size() != weight_ts.size())
88  return (error = E_SIZE_MISMATCH);
89  weight_ts = Mx;
90  return (error = E_NOERROR);
91  }
92 
93  void ChainIkSolverVel_wdls::setLambda(const double lambda_in)
94  {
95  lambda=lambda_in;
96  }
97 
98  void ChainIkSolverVel_wdls::setEps(const double eps_in)
99  {
100  eps=eps_in;
101  }
102 
103  void ChainIkSolverVel_wdls::setMaxIter(const int maxiter_in)
104  {
105  maxiter=maxiter_in;
106  }
107 
108  int ChainIkSolverVel_wdls::getSigma(Eigen::VectorXd& Sout)
109  {
110  if (Sout.size() != S.size())
111  return (error = E_SIZE_MISMATCH);
112  Sout=S;
113  return (error = E_NOERROR);
114  }
115 
116  int ChainIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
117  {
118  if(nj != chain.getNrOfJoints())
119  return (error = E_NOT_UP_TO_DATE);
120 
121  if(nj != q_in.rows() || nj != qdot_out.rows())
122  return (error = E_SIZE_MISMATCH);
123  error = jnt2jac.JntToJac(q_in,jac);
124  if ( error < E_NOERROR) return error;
125 
126  double sum;
127  unsigned int i,j;
128 
129  // Initialize (internal) return values
130  nrZeroSigmas = 0 ;
131  sigmaMin = 0.;
132  lambda_scaled = 0.;
133 
134  /*
135  for (i=0;i<jac.rows();i++) {
136  for (j=0;j<jac.columns();j++)
137  tmp_jac(i,j) = jac(i,j);
138  }
139  */
140 
141  // Create the Weighted jacobian
142  tmp_jac_weight1 = jac.data.lazyProduct(weight_js);
144 
145  // Compute the SVD of the weighted jacobian
147  if (0 != svdResult)
148  {
149  qdot_out.data.setZero() ;
150  return (error = E_SVD_FAILED);
151  }
152 
153  //Pre-multiply U and V by the task space and joint space weighting matrix respectively
154  tmp_ts = weight_ts.lazyProduct(U.topLeftCorner(6,6));
155  tmp_js = weight_js.lazyProduct(V);
156 
157  // Minimum of six largest singular values of J is S(5) if number of joints >=6 and 0 for <6
158  if ( jac.columns() >= 6 ) {
159  sigmaMin = S(5);
160  }
161  else {
162  sigmaMin = 0.;
163  }
164 
165  // tmp = (Si*U'*Ly*y),
166  for (i=0;i<jac.columns();i++) {
167  sum = 0.0;
168  for (j=0;j<jac.rows();j++) {
169  if(i<6)
170  sum+= tmp_ts(j,i)*v_in(j);
171  else
172  sum+=0.0;
173  }
174  // If sigmaMin > eps, then wdls is not active and lambda_scaled = 0 (default value)
175  // If sigmaMin < eps, then wdls is active and lambda_scaled is scaled from 0 to lambda
176  // Note: singular values are always positive so sigmaMin >=0
177  if ( sigmaMin < eps )
178  {
180  }
181  if(fabs(S(i))<eps) {
182  if (i<6) {
183  // Scale lambda to size of singular value sigmaMin
184  tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda_scaled*lambda_scaled)));
185  }
186  else {
187  tmp(i)=0.0; // S(i)=0 for i>=6 due to cols>rows
188  }
189  // Count number of singular values near zero
190  ++nrZeroSigmas ;
191  }
192  else {
193  tmp(i) = sum/S(i);
194  }
195  }
196 
197  /*
198  // x = Lx^-1*V*tmp + x
199  for (i=0;i<jac.columns();i++) {
200  sum = 0.0;
201  for (j=0;j<jac.columns();j++) {
202  sum+=tmp_js(i,j)*tmp(j);
203  }
204  qdot_out(i)=sum;
205  }
206  */
207  qdot_out.data=tmp_js.lazyProduct(tmp);
208 
209  // If number of near zero singular values is greater than the full rank
210  // of jac, then wdls is active
211  if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
212  return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular
213  } else {
214  return (error = E_NOERROR); // have converged
215  }
216  }
217 
218  const char* ChainIkSolverVel_wdls::strError(const int error) const
219  {
220  if (E_CONVERGE_PINV_SINGULAR == error) return "Converged put pseudo inverse of jacobian is singular.";
221  else return SolverI::strError(error);
222  }
223 
224 }
ChainIkSolverVel_wdls(const Chain &chain, double eps=0.00001, int maxiter=150)
int setWeightJS(const Eigen::MatrixXd &Mq)
unsigned int rows() const
Definition: jacobian.cpp:69
unsigned int columns() const
Definition: jacobian.cpp:74
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:54
int setWeightTS(const Eigen::MatrixXd &Mx)
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
virtual const char * strError(const int error) const
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
unsigned int rows() const
Definition: jntarray.cpp:70
static const int E_CONVERGE_PINV_SINGULAR
solution converged but (pseudo)inverse is singular
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
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:723
virtual void updateInternalDataStructures()
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
Internal svd calculation failed.
Definition: solveri.hpp:107
Eigen::VectorXd data
Definition: jntarray.hpp:72
int svd_eigen_HH(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::VectorXd &tmp, int maxiter, double epsilon)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
void setEps(const double eps_in)
int getSigma(Eigen::VectorXd &Sout)
void setLambda(const double lambda)
virtual const char * strError(const int error) const
Definition: solveri.hpp:125
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
void setMaxIter(const int maxiter_in)


orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14