r2_gaze_ik.hpp
Go to the documentation of this file.
00001 #ifndef _R2_GAZE_IK_HPP_
00002 #define _R2_GAZE_IK_HPP_
00003 
00004 // KDL stuff
00005 #include <kdl/chain.hpp>
00006 #include <kdl/chainfksolverpos_recursive.hpp>
00007 #include <kdl/chainjnttojacsolver.hpp>
00008 
00009 // Eigen stuff
00010 #include <Eigen/Core>
00011 
00012 namespace r2_gaze_controller
00013 {
00014 
00015 class R2GazeIK
00016 {
00017     typedef Eigen::DiagonalMatrix<double,Eigen::Dynamic> DiagMatrix;
00018     typedef Eigen::MatrixXd Jacobian;
00019     
00020 public:
00021     R2GazeIK(const KDL::Chain& chain);
00022     ~R2GazeIK();
00023     
00024     void setWeightMatrix(const Eigen::VectorXd& w);
00025     void setWeightMatrix(const Eigen::MatrixXd& W);
00026     int computeSolution(const Eigen::VectorXd& q_act, const KDL::Frame& desired_frame, Eigen::VectorXd& q);
00027     int computeSolution(const KDL::JntArray& q_act, const KDL::Frame& desired_frame, KDL::JntArray& q);
00028     
00029 private:
00030 
00031     double computeDelta(const Eigen::VectorXd& q, const KDL::Frame& desired_frame, Eigen::VectorXd& del_q);
00032 
00033     // KDL chain, forward kinematics and a jacobian solver
00034     KDL::Chain chain;
00035     KDL::ChainFkSolverPos_recursive fk;
00036     KDL::ChainJntToJacSolver jacobian_solver;
00037     
00038     // Stores the number of degrees of freedom in chain
00039     int N;
00040     
00041     // Weight matrix and its inverse
00042     DiagMatrix Q;
00043     DiagMatrix Qinv;
00044 
00045     // Parameters which control the ik solver
00046     int maxSolverAttempts;
00047     double dt;
00048     
00049     // Temporaries
00050     Eigen::Vector3d p_d, p, e;
00051     Jacobian J_partial, Jw, Jwt, Jwt_inv;
00052     Eigen::VectorXd del_q, q_local;
00053     KDL::JntArray q_kdl;
00054     KDL::Jacobian J_kdl;
00055 };
00056 
00057 }
00058 
00059 #endif
00060 


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