Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "chainiksolvervel_pinv.hpp"
00023
00024 namespace KDL
00025 {
00026 ChainIkSolverVel_pinv::ChainIkSolverVel_pinv(const Chain& _chain,double _eps,int _maxiter):
00027 chain(_chain),
00028 jnt2jac(chain),
00029 nj(chain.getNrOfJoints()),
00030 jac(nj),
00031 svd(jac),
00032 U(6,JntArray(nj)),
00033 S(nj),
00034 V(nj,JntArray(nj)),
00035 tmp(nj),
00036 eps(_eps),
00037 maxiter(_maxiter),
00038 nrZeroSigmas(0),
00039 svdResult(0)
00040 {
00041 }
00042
00043 void ChainIkSolverVel_pinv::updateInternalDataStructures() {
00044 jnt2jac.updateInternalDataStructures();
00045 nj = chain.getNrOfJoints();
00046 jac.resize(nj);
00047 svd = SVD_HH(jac);
00048 for(unsigned int i = 0 ; i < U.size(); i++)
00049 U[i].resize(nj);
00050 S.resize(nj);
00051 V.resize(nj);
00052 for(unsigned int i = 0 ; i < V.size(); i++)
00053 V[i].resize(nj);
00054 tmp.resize(nj);
00055 }
00056
00057 ChainIkSolverVel_pinv::~ChainIkSolverVel_pinv()
00058 {
00059 }
00060
00061
00062 int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00063 {
00064 if (nj != chain.getNrOfJoints())
00065 return (error = E_NOT_UP_TO_DATE);
00066
00067 if (nj != q_in.rows() || nj != qdot_out.rows())
00068 return (error = E_SIZE_MISMATCH);
00069
00070
00071
00072 error = jnt2jac.JntToJac(q_in,jac);
00073 if (error < E_NOERROR) return error;
00074
00075 double sum;
00076 unsigned int i,j;
00077
00078
00079 nrZeroSigmas = 0 ;
00080
00081
00082
00083
00084 svdResult = svd.calculate(jac,U,S,V,maxiter);
00085 if (0 != svdResult)
00086 {
00087 qdot_out.data.setZero();
00088 return (error = E_SVD_FAILED);
00089 }
00090
00091
00092
00093
00094
00095
00096 for (i=0;i<jac.columns();i++) {
00097 sum = 0.0;
00098 for (j=0;j<jac.rows();j++) {
00099 sum+= U[j](i)*v_in(j);
00100 }
00101
00102
00103 if ( fabs(S(i))<eps ) {
00104 tmp(i) = 0.0 ;
00105
00106 ++nrZeroSigmas ;
00107 }
00108 else {
00109 tmp(i) = sum/S(i) ;
00110 }
00111 }
00112
00113
00114 for (i=0;i<jac.columns();i++) {
00115 sum = 0.0;
00116 for (j=0;j<jac.columns();j++) {
00117 sum+=V[i](j)*tmp(j);
00118 }
00119
00120 qdot_out(i)=sum;
00121 }
00122
00123
00124
00125 if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
00126 return (error = E_CONVERGE_PINV_SINGULAR);
00127 } else {
00128 return (error = E_NOERROR);
00129 }
00130 }
00131
00132 const char* ChainIkSolverVel_pinv::strError(const int error) const
00133 {
00134 if (E_CONVERGE_PINV_SINGULAR == error) return "Converged put pseudo inverse of jacobian is singular.";
00135 else return SolverI::strError(error);
00136 }
00137 }