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