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_givens.hpp"
00023 #include "utilities/svd_eigen_Macie.hpp"
00024
00025 namespace KDL
00026 {
00027 ChainIkSolverVel_pinv_givens::ChainIkSolverVel_pinv_givens(const Chain& _chain):
00028 chain(_chain),
00029 jnt2jac(chain),
00030 jac(chain.getNrOfJoints()),
00031 transpose(chain.getNrOfJoints()>6),toggle(true),
00032 m(max(6,chain.getNrOfJoints())),
00033 n(min(6,chain.getNrOfJoints())),
00034 jac_eigen(m,n),
00035 U(MatrixXd::Identity(m,m)),
00036 V(MatrixXd::Identity(n,n)),
00037 B(m,n),
00038 S(n),
00039 tempi(m),
00040 tempj(m),
00041 UY(VectorXd::Zero(6)),
00042 SUY(VectorXd::Zero(chain.getNrOfJoints())),
00043 qdot_eigen(chain.getNrOfJoints()),
00044 v_in_eigen(6)
00045 {
00046 }
00047
00048 ChainIkSolverVel_pinv_givens::~ChainIkSolverVel_pinv_givens()
00049 {
00050 }
00051
00052
00053 int ChainIkSolverVel_pinv_givens::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00054 {
00055 toggle=!toggle;
00056
00057 jnt2jac.JntToJac(q_in,jac);
00058
00059 for(unsigned int i=0;i<6;i++)
00060 v_in_eigen(i)=v_in(i);
00061
00062 for(unsigned int i=0;i<m;i++){
00063 for(unsigned int j=0;j<n;j++)
00064 if(transpose)
00065 jac_eigen(i,j)=jac(j,i);
00066 else
00067 jac_eigen(i,j)=jac(i,j);
00068 }
00069 int ret = svd_eigen_Macie(jac_eigen,U,S,V,B,tempi,1e-15,toggle);
00070
00071
00072 if(transpose)
00073 UY.noalias() = V.transpose() * v_in_eigen;
00074 else
00075 UY.noalias() = U.transpose() * v_in_eigen;
00076
00077 for (unsigned int i = 0; i < n; i++){
00078 double wi = UY(i);
00079 double alpha = S(i);
00080
00081 if (alpha != 0)
00082 alpha = 1.0 / alpha;
00083 else
00084 alpha = 0.0;
00085 SUY(i)= alpha * wi;
00086 }
00087 if(transpose)
00088 qdot_eigen.noalias() = U * SUY;
00089 else
00090 qdot_eigen.noalias() = V * SUY;
00091
00092 for (unsigned int j=0;j<chain.getNrOfJoints();j++)
00093 qdot_out(j)=qdot_eigen(j);
00094
00095 return ret;
00096
00097 }
00098
00099 }