r2_gaze_ik.cpp
Go to the documentation of this file.
00001 #include "r2_gaze_ik.hpp"
00002 
00003 // KDL stuff
00004 #include <kdl/frames.hpp>
00005 
00006 // Eigen stuff
00007 #include <Eigen/SVD>
00008 
00009 // Standard libraries
00010 #include <cmath>
00011 #include <iostream>
00012 
00013 namespace r2_gaze_controller 
00014 {
00015 
00016 /*template <typename Derived>
00017 bool pinv(const Eigen::MatrixBase<Derived>& a, Eigen::MatrixBase<Derived> const & a_pinv)
00018 {
00019         typedef typename Derived::Scalar Scalar;
00020         typedef typename Eigen::JacobiSVD<Derived>::SingularValuesType SingularValuesType;
00021         typedef typename Eigen::JacobiSVD<Derived>::MatrixUType MatrixUType;
00022 
00023     // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
00024         Eigen::MatrixBase<Derived> m(a);
00025         Eigen::MatrixBase<Derived> m_pinv(a);
00026 
00027     // transpose so SVD decomp can work...
00028     //if (a.rows() < a.cols())
00029     //{
00030     //  m.resize(a.cols(),a.rows());
00031     //  m = a.transpose();
00032     //  m_pinv.resize(a.cols(),a.rows());
00033     //}
00034     std::cout << "Got to here." << std::endl;
00035     // SVD
00036     Eigen::JacobiSVD<Derived> svd = m.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
00037     std::cout << "Got to here2." << std::endl;
00038     const SingularValuesType& vSingular = svd.singularValues();
00039     // Build a diagonal matrix with the Inverted Singular values
00040     // The pseudo inverted singular matrix is easy to compute :
00041     // is formed by replacing every nonzero entry by its reciprocal (inversing).
00042     Eigen::Matrix<Scalar, Eigen::Dynamic, 1> vPseudoInvertedSingular(svd.matrixV().cols(),1);
00043     for (int iRow =0; iRow < vSingular.rows(); iRow++)
00044     {
00045         if ( fabs(vSingular(iRow))<=1e-10 ) { // Todo : Put epsilon in parameter
00046             vPseudoInvertedSingular(iRow,0)=0.;
00047         } else {
00048             vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
00049         }
00050     }
00051 
00052     // A little optimization here 
00053     //MatrixUType mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
00054     // Pseudo-Inversion : V * S * U'
00055     //m_pinv = (svd.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * mAdjointU;
00056     m_pinv = (svd.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * svd.matrixU().transpose();
00057 
00058     // transpose back if necessary
00059     //if (a.rows() < a.cols())
00060     //{
00061     //  const_cast< Eigen::MatrixBase<Derived>& >(a_pinv) = m_pinv.transpose();
00062     //} else {
00063         const_cast< Eigen::MatrixBase<Derived>& >(a_pinv) = m_pinv;
00064     //}
00065 
00066     return true;
00067 }*/
00068 
00069 template<typename _Matrix_Type_>
00070 bool pseudoInverse(const _Matrix_Type_ &a, _Matrix_Type_ &result, double
00071                 epsilon = std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon())
00072 {
00073         if(a.rows()<a.cols())
00074                 return false;
00075 
00076         Eigen::JacobiSVD< _Matrix_Type_ > svd = a.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
00077 
00078         typename _Matrix_Type_::Scalar tolerance = epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff();
00079 
00080         result = svd.matrixV() * _Matrix_Type_( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) ).asDiagonal() * svd.matrixU().adjoint();
00081         return true;
00082 }
00083 
00084 struct Convert
00085 {
00086     static void toEigenVector(const KDL::Frame& in, Eigen::Vector3d& out)
00087     {
00088         for (int r=0; r < 3; r++) out(r) = in.p.data[r];
00089     }
00090     
00091     static void toJntArray(const Eigen::VectorXd& in, KDL::JntArray& out)
00092     {
00093         out.data = in;
00094     }
00095 };
00096 
00097 R2GazeIK::R2GazeIK(const KDL::Chain& chain) :
00098     chain(chain),
00099     fk(chain),
00100     jacobian_solver(chain),
00101     N(chain.getNrOfJoints()),
00102     Qinv(Eigen::VectorXd::Ones(N)),
00103     maxSolverAttempts(200),
00104     dt(0.1),
00105     p_d(Eigen::Vector3d::Zero()),
00106     p(Eigen::Vector3d::Zero()),
00107     e(Eigen::Vector3d::Zero()),
00108     J_partial(Jacobian::Zero(3,N)),
00109     Jw(Jacobian::Zero(3,N)),
00110     Jwt(Jacobian::Zero(N,3)),
00111     Jwt_inv(Jacobian::Zero(N,3)),
00112     q_kdl(N),
00113     J_kdl(N),
00114     del_q(N), q_local(N)
00115 {
00116 }
00117 
00118 R2GazeIK::~R2GazeIK()
00119 {
00120 
00121 }
00122 
00123 void R2GazeIK::setWeightMatrix(const Eigen::VectorXd& w)
00124 {
00125     Qinv.diagonal() = w;
00126 }
00127 
00128 void R2GazeIK::setWeightMatrix(const Eigen::MatrixXd& W)
00129 {
00130         Eigen::VectorXd w(W.diagonal());
00131         this->setWeightMatrix(w);
00132 }
00133 
00134 double R2GazeIK::computeDelta(const Eigen::VectorXd& q, const KDL::Frame& desired_frame, Eigen::VectorXd& del_q)
00135 {
00136     // Get desired position to look at
00137     Convert::toEigenVector(desired_frame, p_d);
00138 
00139     // Create a joint array from q from actual neck angles
00140     Convert::toJntArray(q, q_kdl);
00141 
00142     // Use forward kinematics to determine where we are currently at
00143     KDL::Frame frame_actual;
00144     if (fk.JntToCart(q_kdl, frame_actual) < 0)
00145     {
00146         return -1;
00147     }
00148     Convert::toEigenVector(frame_actual, p);
00149                     
00150     // Determine error - where we are currently and where we want to be
00151     e = p_d - p;
00152 
00153     // Compute chain jacobian at q
00154     jacobian_solver.JntToJac(q_kdl, J_kdl);
00155 
00156     // Only interested in the position submatrix of J
00157     J_partial = J_kdl.data.topRows(3);
00158 
00159     // Weighted jacobian and its inverse
00160     Jw = J_partial * Qinv * J_partial.transpose();
00161     Jwt = Jw.transpose();
00162     pseudoInverse(Jwt, Jwt_inv);
00163 
00164     // Compute delta_q step
00165     del_q = Qinv * J_partial.transpose() * Jwt_inv.transpose() * e;
00166     return e.transpose() * e;
00167 }
00168 
00169 int R2GazeIK::computeSolution(const KDL::JntArray& q_act, const KDL::Frame& desired_frame, KDL::JntArray& q)
00170 {
00171         Eigen::VectorXd q_in(q_act.data), q_out(q_act.data);
00172         int ret = computeSolution(q_in, desired_frame, q_out);
00173         q.data = q_out;
00174         return ret;
00175 }
00176 
00177 int R2GazeIK::computeSolution(const Eigen::VectorXd& q_act, const KDL::Frame& desired_frame, Eigen::VectorXd& q)
00178 {
00179     // Out starting point
00180     q_local = q_act;
00181 
00182     // Find out where we are at
00183     double err = computeDelta(q_local, desired_frame, del_q);
00184 
00185     if (err < 0) return -1;
00186     
00187     // Iterate until position error is small
00188     int attempts = 0;
00189     while ((err > 0.01) & (attempts < maxSolverAttempts))
00190     {
00191         q_local += dt * del_q;
00192         err = computeDelta(q_local, desired_frame, del_q);
00193         if (err < 0) return -1;
00194         attempts = attempts + 1;
00195     } 
00196 
00197     q = q_local;
00198                  
00199     if (attempts >= maxSolverAttempts) return -1;
00200     return 0;
00201 }
00202    
00203 } // end namespace


r2_controllers_gazebo
Author(s): Stephen Hart
autogenerated on Mon Oct 6 2014 02:48:44