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 nj(chain.getNrOfJoints()),
00032 jac(nj),
00033 U(MatrixXd::Zero(6,nj)),
00034 S(VectorXd::Zero(nj)),
00035 V(MatrixXd::Zero(nj,nj)),
00036 eps(_eps),
00037 maxiter(_maxiter),
00038 tmp(VectorXd::Zero(nj)),
00039 tmp_jac(MatrixXd::Zero(6,nj)),
00040 tmp_jac_weight1(MatrixXd::Zero(6,nj)),
00041 tmp_jac_weight2(MatrixXd::Zero(6,nj)),
00042 tmp_ts(MatrixXd::Zero(6,6)),
00043 tmp_js(MatrixXd::Zero(nj,nj)),
00044 weight_ts(MatrixXd::Identity(6,6)),
00045 weight_js(MatrixXd::Identity(nj,nj)),
00046 lambda(0.0),
00047 lambda_scaled(0.0),
00048 nrZeroSigmas(0),
00049 svdResult(0),
00050 sigmaMin(0)
00051 {
00052 }
00053
00054 void ChainIkSolverVel_wdls::updateInternalDataStructures() {
00055 jnt2jac.updateInternalDataStructures();
00056 nj = chain.getNrOfJoints();
00057 jac.resize(nj);
00058 MatrixXd z6nj = MatrixXd::Zero(6,nj);
00059 VectorXd znj = VectorXd::Zero(nj);
00060 MatrixXd znjnj = MatrixXd::Zero(nj,nj);
00061 U.conservativeResizeLike(z6nj);
00062 S.conservativeResizeLike(znj);
00063 V.conservativeResizeLike(znjnj);
00064 tmp.conservativeResizeLike(znj);
00065 tmp_jac.conservativeResizeLike(z6nj);
00066 tmp_jac_weight1.conservativeResizeLike(z6nj);
00067 tmp_jac_weight2.conservativeResizeLike(z6nj);
00068 tmp_js.conservativeResizeLike(znjnj);
00069 weight_js.conservativeResizeLike(MatrixXd::Identity(nj,nj));
00070 }
00071
00072 ChainIkSolverVel_wdls::~ChainIkSolverVel_wdls()
00073 {
00074 }
00075
00076 int ChainIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq){
00077 if(nj != chain.getNrOfJoints())
00078 return (error = E_NOT_UP_TO_DATE);
00079
00080 if (Mq.size() != weight_js.size())
00081 return (error = E_SIZE_MISMATCH);
00082 weight_js = Mq;
00083 return (error = E_NOERROR);
00084 }
00085
00086 int ChainIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx){
00087 if (Mx.size() != weight_ts.size())
00088 return (error = E_SIZE_MISMATCH);
00089 weight_ts = Mx;
00090 return (error = E_NOERROR);
00091 }
00092
00093 void ChainIkSolverVel_wdls::setLambda(const double lambda_in)
00094 {
00095 lambda=lambda_in;
00096 }
00097
00098 void ChainIkSolverVel_wdls::setEps(const double eps_in)
00099 {
00100 eps=eps_in;
00101 }
00102
00103 void ChainIkSolverVel_wdls::setMaxIter(const int maxiter_in)
00104 {
00105 maxiter=maxiter_in;
00106 }
00107
00108 int ChainIkSolverVel_wdls::getSigma(Eigen::VectorXd& Sout)
00109 {
00110 if (Sout.size() != S.size())
00111 return (error = E_SIZE_MISMATCH);
00112 Sout=S;
00113 return (error = E_NOERROR);
00114 }
00115
00116 int ChainIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00117 {
00118 if(nj != chain.getNrOfJoints())
00119 return (error = E_NOT_UP_TO_DATE);
00120
00121 if(nj != q_in.rows() || nj != qdot_out.rows())
00122 return (error = E_SIZE_MISMATCH);
00123 error = jnt2jac.JntToJac(q_in,jac);
00124 if ( error < E_NOERROR) return error;
00125
00126 double sum;
00127 unsigned int i,j;
00128
00129
00130 nrZeroSigmas = 0 ;
00131 sigmaMin = 0.;
00132 lambda_scaled = 0.;
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142 tmp_jac_weight1 = jac.data.lazyProduct(weight_js);
00143 tmp_jac_weight2 = weight_ts.lazyProduct(tmp_jac_weight1);
00144
00145
00146 svdResult = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
00147 if (0 != svdResult)
00148 {
00149 qdot_out.data.setZero() ;
00150 return (error = E_SVD_FAILED);
00151 }
00152
00153
00154 tmp_ts = weight_ts.lazyProduct(U.topLeftCorner(6,6));
00155 tmp_js = weight_js.lazyProduct(V);
00156
00157
00158 if ( jac.columns() >= 6 ) {
00159 sigmaMin = S(5);
00160 }
00161 else {
00162 sigmaMin = 0.;
00163 }
00164
00165
00166 for (i=0;i<jac.columns();i++) {
00167 sum = 0.0;
00168 for (j=0;j<jac.rows();j++) {
00169 if(i<6)
00170 sum+= tmp_ts(j,i)*v_in(j);
00171 else
00172 sum+=0.0;
00173 }
00174
00175
00176
00177 if ( sigmaMin < eps )
00178 {
00179 lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ;
00180 }
00181 if(fabs(S(i))<eps) {
00182 if (i<6) {
00183
00184 tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda_scaled*lambda_scaled)));
00185 }
00186 else {
00187 tmp(i)=0.0;
00188 }
00189
00190 ++nrZeroSigmas ;
00191 }
00192 else {
00193 tmp(i) = sum/S(i);
00194 }
00195 }
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207 qdot_out.data=tmp_js.lazyProduct(tmp);
00208
00209
00210
00211 if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
00212 return (error = E_CONVERGE_PINV_SINGULAR);
00213 } else {
00214 return (error = E_NOERROR);
00215 }
00216 }
00217
00218 const char* ChainIkSolverVel_wdls::strError(const int error) const
00219 {
00220 if (E_CONVERGE_PINV_SINGULAR == error) return "Converged put pseudo inverse of jacobian is singular.";
00221 else return SolverI::strError(error);
00222 }
00223
00224 }