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 nj(chain.getNrOfJoints()),
00029 jnt2jac(chain),
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 ChainIkSolverVel_pinv::~ChainIkSolverVel_pinv()
00044 {
00045 }
00046
00047
00048 int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00049 {
00050 if (nj != q_in.rows() || nj != qdot_out.rows())
00051 return (error = E_SIZE_MISMATCH);
00052
00053
00054
00055 error = jnt2jac.JntToJac(q_in,jac);
00056 if (error < E_NOERROR) return error;
00057
00058 double sum;
00059 unsigned int i,j;
00060
00061
00062 nrZeroSigmas = 0 ;
00063
00064
00065
00066
00067 svdResult = svd.calculate(jac,U,S,V,maxiter);
00068 if (0 != svdResult)
00069 {
00070 qdot_out.data.setZero();
00071 return (error = E_SVD_FAILED);
00072 }
00073
00074
00075
00076
00077
00078
00079 for (i=0;i<jac.columns();i++) {
00080 sum = 0.0;
00081 for (j=0;j<jac.rows();j++) {
00082 sum+= U[j](i)*v_in(j);
00083 }
00084
00085
00086 if ( fabs(S(i))<eps ) {
00087 tmp(i) = 0.0 ;
00088
00089 ++nrZeroSigmas ;
00090 }
00091 else {
00092 tmp(i) = sum/S(i) ;
00093 }
00094 }
00095
00096
00097 for (i=0;i<jac.columns();i++) {
00098 sum = 0.0;
00099 for (j=0;j<jac.columns();j++) {
00100 sum+=V[i](j)*tmp(j);
00101 }
00102
00103 qdot_out(i)=sum;
00104 }
00105
00106
00107
00108 if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
00109 return (error = E_CONVERGE_PINV_SINGULAR);
00110 } else {
00111 return (error = E_NOERROR);
00112 }
00113 }
00114
00115 const char* ChainIkSolverVel_pinv::strError(const int error) const
00116 {
00117 if (E_CONVERGE_PINV_SINGULAR == error) return "Converged put pseudo inverse of jacobian is singular.";
00118 else return SolverI::strError(error);
00119 }
00120 }