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