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_nso.hpp"
00023 #include "utilities/svd_eigen_HH.hpp"
00024
00025 namespace KDL
00026 {
00027 ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, JntArray _opt_pos, JntArray _weights, double _eps, int _maxiter, double _alpha):
00028 chain(_chain),
00029 jnt2jac(chain),
00030 nj(chain.getNrOfJoints()),
00031 jac(nj),
00032 U(MatrixXd::Zero(6,nj)),
00033 S(VectorXd::Zero(nj)),
00034 Sinv(VectorXd::Zero(nj)),
00035 V(MatrixXd::Zero(nj,nj)),
00036 tmp(VectorXd::Zero(nj)),
00037 tmp2(VectorXd::Zero(nj)),
00038 eps(_eps),
00039 maxiter(_maxiter),
00040 alpha(_alpha),
00041 weights(_weights),
00042 opt_pos(_opt_pos)
00043 {
00044 }
00045
00046 ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, double _alpha):
00047 chain(_chain),
00048 jnt2jac(chain),
00049 nj(chain.getNrOfJoints()),
00050 jac(nj),
00051 U(MatrixXd::Zero(6,nj)),
00052 S(VectorXd::Zero(nj)),
00053 Sinv(VectorXd::Zero(nj)),
00054 V(MatrixXd::Zero(nj,nj)),
00055 tmp(VectorXd::Zero(nj)),
00056 tmp2(VectorXd::Zero(nj)),
00057 eps(_eps),
00058 maxiter(_maxiter),
00059 alpha(_alpha)
00060 {
00061 }
00062
00063 ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso()
00064 {
00065 }
00066
00067
00068 int ChainIkSolverVel_pinv_nso::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00069 {
00070
00071
00072 jnt2jac.JntToJac(q_in,jac);
00073
00074
00075
00076
00077 int svdResult = svd_eigen_HH(jac.data,U,S,V,tmp,maxiter);
00078 if (0 != svdResult)
00079 {
00080 qdot_out.data.setZero() ;
00081 return svdResult;
00082 }
00083
00084 unsigned int i;
00085
00086
00087
00088
00089
00090
00091 for (i = 0; i < nj; ++i) {
00092 Sinv(i) = fabs(S(i))<eps ? 0.0 : 1.0/S(i);
00093 }
00094 for (i = 0; i < 6; ++i) {
00095 tmp(i) = v_in(i);
00096 }
00097
00098 qdot_out.data = V * Sinv.asDiagonal() * U.transpose() * tmp.head(6);
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118 double g = 0;
00119 double A = 0;
00120 for (i = 0; i < nj; ++i) {
00121 double qd = q_in(i) - opt_pos(i);
00122 g += 0.5 * qd*qd * weights(i);
00123 A += qd*qd * weights(i)*weights(i);
00124 }
00125
00126 if (A > 1e-9) {
00127
00128 for (i = 0; i < nj; ++i) {
00129 tmp(i) = weights(i)*(q_in(i) - opt_pos(i)) / A;
00130 }
00131
00132
00133 tmp2 = V * Sinv.asDiagonal() * U.transpose() * U * S.asDiagonal() * V.transpose() * tmp;
00134
00135 for (i = 0; i < nj; ++i) {
00136
00137 qdot_out(i) += -2*alpha*g * (tmp(i) - tmp2(i));
00138 }
00139 }
00140
00141 return svdResult;
00142 }
00143
00144 int ChainIkSolverVel_pinv_nso::setWeights(const JntArray & _weights)
00145 {
00146 weights = _weights;
00147 return 0;
00148 }
00149
00150 int ChainIkSolverVel_pinv_nso::setOptPos(const JntArray & _opt_pos)
00151 {
00152 opt_pos = _opt_pos;
00153 return 0;
00154 }
00155
00156 int ChainIkSolverVel_pinv_nso::setAlpha(const double _alpha)
00157 {
00158 alpha = _alpha;
00159 return 0;
00160 }
00161
00162
00163 }