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_wdls.hpp"
00023 #include "utilities/svd_eigen_HH.hpp"
00024
00025 namespace KDL
00026 {
00027
00028 ChainIkSolverVel_wdls::ChainIkSolverVel_wdls(const Chain& _chain,double _eps,int _maxiter):
00029 chain(_chain),
00030 jnt2jac(chain),
00031 jac(chain.getNrOfJoints()),
00032 U(MatrixXd::Zero(6,chain.getNrOfJoints())),
00033 S(VectorXd::Zero(chain.getNrOfJoints())),
00034 V(MatrixXd::Zero(chain.getNrOfJoints(),chain.getNrOfJoints())),
00035 eps(_eps),
00036 maxiter(_maxiter),
00037 tmp(VectorXd::Zero(chain.getNrOfJoints())),
00038 tmp_jac(MatrixXd::Zero(6,chain.getNrOfJoints())),
00039 tmp_jac_weight1(MatrixXd::Zero(6,chain.getNrOfJoints())),
00040 tmp_jac_weight2(MatrixXd::Zero(6,chain.getNrOfJoints())),
00041 tmp_ts(MatrixXd::Zero(6,6)),
00042 tmp_js(MatrixXd::Zero(chain.getNrOfJoints(),chain.getNrOfJoints())),
00043 weight_ts(MatrixXd::Identity(6,6)),
00044 weight_js(MatrixXd::Identity(chain.getNrOfJoints(),chain.getNrOfJoints())),
00045 lambda(0.0),
00046 lambda_scaled(0.0),
00047 nrZeroSigmas(0),
00048 svdResult(0),
00049 sigmaMin(0)
00050 {
00051 }
00052
00053 ChainIkSolverVel_wdls::~ChainIkSolverVel_wdls()
00054 {
00055 }
00056
00057 void ChainIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq){
00058 weight_js = Mq;
00059 }
00060
00061 void ChainIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx){
00062 weight_ts = Mx;
00063 }
00064
00065 void ChainIkSolverVel_wdls::setLambda(const double lambda_in)
00066 {
00067 lambda=lambda_in;
00068 }
00069
00070 void ChainIkSolverVel_wdls::setEps(const double eps_in)
00071 {
00072 eps=eps_in;
00073 }
00074
00075 void ChainIkSolverVel_wdls::setMaxIter(const int maxiter_in)
00076 {
00077 maxiter=maxiter_in;
00078 }
00079
00080 int ChainIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00081 {
00082 jnt2jac.JntToJac(q_in,jac);
00083
00084 double sum;
00085 unsigned int i,j;
00086
00087
00088 nrZeroSigmas = 0 ;
00089 sigmaMin = 0.;
00090 lambda_scaled = 0.;
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100 tmp_jac_weight1 = jac.data.lazyProduct(weight_js);
00101 tmp_jac_weight2 = weight_ts.lazyProduct(tmp_jac_weight1);
00102
00103
00104 svdResult = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
00105 if (0 != svdResult)
00106 {
00107 qdot_out.data.setZero() ;
00108 return (error = E_SVD_FAILED);
00109 }
00110
00111
00112 tmp_ts = weight_ts.lazyProduct(U.topLeftCorner(6,6));
00113 tmp_js = weight_js.lazyProduct(V);
00114
00115
00116 if ( jac.columns() >= 6 ) {
00117 sigmaMin = S(5);
00118 }
00119 else {
00120 sigmaMin = 0.;
00121 }
00122
00123
00124 for (i=0;i<jac.columns();i++) {
00125 sum = 0.0;
00126 for (j=0;j<jac.rows();j++) {
00127 if(i<6)
00128 sum+= tmp_ts(j,i)*v_in(j);
00129 else
00130 sum+=0.0;
00131 }
00132
00133
00134
00135 if ( sigmaMin < eps )
00136 {
00137 lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ;
00138 }
00139 if(fabs(S(i))<eps) {
00140 if (i<6) {
00141
00142 tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda_scaled*lambda_scaled)));
00143 }
00144 else {
00145 tmp(i)=0.0;
00146 }
00147
00148 ++nrZeroSigmas ;
00149 }
00150 else {
00151 tmp(i) = sum/S(i);
00152 }
00153 }
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165 qdot_out.data=tmp_js.lazyProduct(tmp);
00166
00167
00168
00169 if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
00170 return (error = E_CONVERGE_PINV_SINGULAR);
00171 } else {
00172 return (error = E_NOERROR);
00173 }
00174 }
00175
00176 const char* ChainIkSolverVel_wdls::strError(const int error) const
00177 {
00178 if (E_SVD_FAILED == error) return "SVD failed";
00179 else return SolverI::strError(error);
00180 }
00181
00182 }