orocos_kdl
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(static_cast<unsigned int>(max(6,nj))),
34  n(static_cast<unsigned int>(min(6,nj))),
35  jac_eigen(m,n),
36  U(Eigen::MatrixXd::Identity(m,m)),
37  V(Eigen::MatrixXd::Identity(n,n)),
38  B(m,n),
39  S(n),
40  tempi(m),
41  UY(Eigen::VectorXd::Zero(6)),
42  SUY(Eigen::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 = static_cast<unsigned int>(max(6,nj));
54  n = static_cast<unsigned int>(min(6,nj));
55  jac_eigen.conservativeResize(m,n);
56  U.conservativeResizeLike(Eigen::MatrixXd::Identity(m,m));
57  V.conservativeResizeLike(Eigen::MatrixXd::Identity(n,n));
58  B.conservativeResize(m,n);
59  S.conservativeResize(n);
60  tempi.conservativeResize(m);
61  SUY.conservativeResizeLike(Eigen::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 
112  if(transpose)
113  qdot_eigen.noalias() = U * SUY;
114  else
115  qdot_eigen.noalias() = V * SUY;
116 
117  for (unsigned int j=0;j<chain.getNrOfJoints();j++)
118  qdot_out(j)=qdot_eigen(j);
119 
120  return (error = E_NOERROR);
121  }
122 }
KDL::JntArray
Definition: jntarray.hpp:69
chainiksolvervel_pinv_givens.hpp
KDL::ChainIkSolverVel_pinv_givens::jac_eigen
Eigen::MatrixXd jac_eigen
Definition: chainiksolvervel_pinv_givens.hpp:53
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::min
double min(double a, double b)
Definition: utility.h:212
KDL::ChainIkSolverVel_pinv_givens::transpose
bool transpose
Definition: chainiksolvervel_pinv_givens.hpp:51
KDL::ChainIkSolverVel_pinv_givens::CartToJnt
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
Definition: chainiksolvervel_pinv_givens.cpp:70
KDL::ChainIkSolverVel_pinv_givens::toggle
bool toggle
Definition: chainiksolvervel_pinv_givens.hpp:51
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_givens::n
unsigned int n
Definition: chainiksolvervel_pinv_givens.hpp:52
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_givens::U
Eigen::MatrixXd U
Definition: chainiksolvervel_pinv_givens.hpp:53
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::ChainIkSolverVel_pinv_givens::qdot_eigen
Eigen::VectorXd qdot_eigen
Definition: chainiksolvervel_pinv_givens.hpp:54
KDL::ChainIkSolverVel_pinv_givens::m
unsigned int m
Definition: chainiksolvervel_pinv_givens.hpp:52
KDL::svd_eigen_Macie
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)
Definition: svd_eigen_Macie.cpp:25
KDL::max
double max(double a, double b)
Definition: utility.h:204
KDL::ChainIkSolverVel_pinv_givens::tempi
Eigen::VectorXd tempi
Definition: chainiksolvervel_pinv_givens.hpp:54
KDL::ChainIkSolverVel_pinv_givens::jac
Jacobian jac
Definition: chainiksolvervel_pinv_givens.hpp:50
KDL::ChainJntToJacSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainjnttojacsolver.cpp:31
KDL::ChainIkSolverVel_pinv_givens::V
Eigen::MatrixXd V
Definition: chainiksolvervel_pinv_givens.hpp:53
KDL::ChainIkSolverVel_pinv_givens::UY
Eigen::VectorXd UY
Definition: chainiksolvervel_pinv_givens.hpp:54
KDL::ChainIkSolverVel_pinv_givens::B
Eigen::MatrixXd B
Definition: chainiksolvervel_pinv_givens.hpp:53
KDL::ChainIkSolverVel_pinv_givens::chain
const Chain & chain
Definition: chainiksolvervel_pinv_givens.hpp:47
svd_eigen_Macie.hpp
KDL::ChainIkSolverVel_pinv_givens::S
Eigen::VectorXd S
Definition: chainiksolvervel_pinv_givens.hpp:54
KDL::ChainIkSolverVel_pinv_givens::nj
unsigned int nj
Definition: chainiksolvervel_pinv_givens.hpp:48
KDL::ChainIkSolverVel_pinv_givens::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainiksolvervel_pinv_givens.cpp:48
KDL::ChainIkSolverVel_pinv_givens::v_in_eigen
Eigen::VectorXd v_in_eigen
Definition: chainiksolvervel_pinv_givens.hpp:54
KDL::ChainIkSolverVel_pinv_givens::jnt2jac
ChainJntToJacSolver jnt2jac
Definition: chainiksolvervel_pinv_givens.hpp:49
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
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_givens::ChainIkSolverVel_pinv_givens
ChainIkSolverVel_pinv_givens(const Chain &chain)
Definition: chainiksolvervel_pinv_givens.cpp:27
KDL::ChainIkSolverVel_pinv_givens::SUY
Eigen::VectorXd SUY
Definition: chainiksolvervel_pinv_givens.hpp:54
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::ChainIkSolverVel_pinv_givens::~ChainIkSolverVel_pinv_givens
~ChainIkSolverVel_pinv_givens()
Definition: chainiksolvervel_pinv_givens.cpp:65