chainiksolvervel_pinv_nso.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  ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, const JntArray& _opt_pos, const JntArray& _weights, double _eps, int _maxiter, double _alpha):
28  chain(_chain),
29  jnt2jac(chain),
30  nj(chain.getNrOfJoints()),
31  jac(nj),
32  U(MatrixXd::Zero(6,nj)),
33  S(VectorXd::Zero(nj)),
34  Sinv(VectorXd::Zero(nj)),
35  V(MatrixXd::Zero(nj,nj)),
36  tmp(VectorXd::Zero(nj)),
37  tmp2(VectorXd::Zero(nj)),
38  eps(_eps),
39  maxiter(_maxiter),
40  svdResult(0),
41  alpha(_alpha),
42  weights(_weights),
43  opt_pos(_opt_pos)
44  {
45  }
46 
47  ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, double _alpha):
48  chain(_chain),
49  jnt2jac(chain),
50  nj(chain.getNrOfJoints()),
51  jac(nj),
52  U(MatrixXd::Zero(6,nj)),
53  S(VectorXd::Zero(nj)),
54  Sinv(VectorXd::Zero(nj)),
55  V(MatrixXd::Zero(nj,nj)),
56  tmp(VectorXd::Zero(nj)),
57  tmp2(VectorXd::Zero(nj)),
58  eps(_eps),
59  maxiter(_maxiter),
60  svdResult(0),
61  alpha(_alpha)
62  {
63  }
64 
68  jac.resize(nj);
69  U.conservativeResizeLike(MatrixXd::Zero(6,nj));
70  S.conservativeResizeLike(VectorXd::Zero(nj));
71  Sinv.conservativeResizeLike(VectorXd::Zero(nj));
72  V.conservativeResizeLike(MatrixXd::Zero(nj,nj));
73  tmp.conservativeResizeLike(VectorXd::Zero(nj));
74  tmp2.conservativeResizeLike(VectorXd::Zero(nj));
75  opt_pos.data.conservativeResizeLike(VectorXd::Zero(nj));
76  weights.data.conservativeResizeLike(VectorXd::Ones(nj));
77  }
78 
80  {
81  }
82 
83 
84  int ChainIkSolverVel_pinv_nso::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
85  {
86  if (nj != chain.getNrOfJoints())
87  return (error = E_NOT_UP_TO_DATE);
88 
89  if (nj != q_in.rows() || nj != qdot_out.rows() || nj != opt_pos.rows() || nj != weights.rows())
90  return (error = E_SIZE_MISMATCH);
91  //Let the ChainJntToJacSolver calculate the jacobian "jac" for
92  //the current joint positions "q_in"
93  error = jnt2jac.JntToJac(q_in,jac);
94  if (error < E_NOERROR) return error;
95 
96  //Do a singular value decomposition of "jac" with maximum
97  //iterations "maxiter", put the results in "U", "S" and "V"
98  //jac = U*S*Vt
100  if (0 != svdResult)
101  {
102  qdot_out.data.setZero() ;
103  return error = E_SVD_FAILED;
104  }
105 
106  unsigned int i;
107 
108  // We have to calculate qdot_out = jac_pinv*v_in
109  // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
110  // qdot_out = V*S_pinv*Ut*v_in
111 
112  // S^-1
113  for (i = 0; i < nj; ++i) {
114  Sinv(i) = fabs(S(i))<eps ? 0.0 : 1.0/S(i);
115  }
116  for (i = 0; i < 6; ++i) {
117  tmp(i) = v_in(i);
118  }
119 
120  qdot_out.data = V * Sinv.asDiagonal() * U.transpose() * tmp.head(6);
121 
122  // Now onto NULL space
123  // Given the cost function g, and the current joints q, desired joints qd, and weights w:
124  // t = g(q) = 1/2 * Sum( w_i * (q_i - qd_i)^2 )
125  //
126  // The jacobian Jc is:
127  // t_dot = Jc(q) * q_dot
128  // Jc = dt/dq = w_j * (q_i - qd_i) [1 x nj vector]
129  //
130  // The pseudo inverse (^-1) is
131  // Jc^-1 = w_j * (q_i - qd_i) / A [nj x 1 vector]
132  // A = Sum( w_i^2 * (q_i - qd_i)^2 )
133  //
134  // We can set the change as the step needed to remove the error times a value alpha:
135  // t_dot = -2 * alpha * t
136  //
137  // When we put it together and project into the nullspace, the final result is
138  // q'_out += (I_n - J^-1 * J) * Jc^-1 * (-2 * alpha * g(q))
139 
140  double g = 0; // g(q)
141  double A = 0; // normalizing term
142  for (i = 0; i < nj; ++i) {
143  double qd = q_in(i) - opt_pos(i);
144  g += 0.5 * qd*qd * weights(i);
145  A += qd*qd * weights(i)*weights(i);
146  }
147 
148  if (A > 1e-9) {
149  // Calculate inverse Jacobian Jc^-1
150  for (i = 0; i < nj; ++i) {
151  tmp(i) = weights(i)*(q_in(i) - opt_pos(i)) / A;
152  }
153 
154  // Calculate J^-1 * J * Jc^-1 = V*S^-1*U' * U*S*V' * tmp
155  tmp2 = V * Sinv.asDiagonal() * U.transpose() * U * S.asDiagonal() * V.transpose() * tmp;
156 
157  for (i = 0; i < nj; ++i) {
158  //std::cerr << i <<": "<< qdot_out(i) <<", "<< -2*alpha*g * (tmp(i) - tmp2(i)) << std::endl;
159  qdot_out(i) += -2*alpha*g * (tmp(i) - tmp2(i));
160  }
161  }
162  //return the return value of the svd decomposition
163  return (error = E_NOERROR);
164  }
165 
167  {
168  if (nj != _weights.rows())
169  return (error = E_SIZE_MISMATCH);
170  weights = _weights;
171  return (error = E_NOERROR);
172  }
173 
175  {
176  if (nj != _opt_pos.rows())
177  return (error = E_SIZE_MISMATCH);
178  opt_pos = _opt_pos;
179  return (error = E_NOERROR);
180  }
181 
182  int ChainIkSolverVel_pinv_nso::setAlpha(const double _alpha)
183  {
184  alpha = _alpha;
185  return 0;
186  }
187 
188 
189 }
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
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
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
int svd_eigen_HH(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, VectorXd &tmp, int maxiter, double epsilon)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
Definition: jacobian.hpp:41
ChainIkSolverVel_pinv_nso(const Chain &chain, const JntArray &opt_pos, const JntArray &weights, double eps=0.00001, int maxiter=150, double alpha=0.25)
represents both translational and rotational velocities.
Definition: frames.hpp:720
virtual void updateInternalDataStructures()
virtual int setAlpha(const double alpha)
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
Internal svd calculation failed.
Definition: solveri.hpp:107
Eigen::VectorXd data
Definition: jntarray.hpp:72
virtual int setOptPos(const JntArray &opt_pos)
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
virtual int setWeights(const JntArray &weights)
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149


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