iri_wam_ik: wam_ikj.h Source File

Go to the documentation of this file.
00001 #ifndef _WAM_IKJ_H_
00002 #define _WAM_IKJ_H_
00003 
00004 #include <Eigen/Geometry>
00005 #include <math.h>
00006 #include <Eigen/StdVector>
00007 #include <Eigen/Dense>
00008 #include <string>
00009 #include "ros/this_node.h"
00010 #include "ros/names.h"
00011 #include "ros/node_handle.h"
00012 #include "ros/rate.h"
00013 
00014 // [publisher subscriber headers]
00015 #include "sensor_msgs/JointState.h"
00016 
00017 // [service client headers]
00018 #include "iri_wam_common_msgs/pose_move.h"
00019 #include "iri_wam_common_msgs/joints_move.h"
00020 #include "iri_wam_common_msgs/wamInverseKinematics.h"
00021 
00022 // [action server client headers]
00023 
00024 class WamIKJ {
00025   private:
00026     ros::NodeHandle nh_;
00027     // [publisher attributes]
00028     sensor_msgs::JointState ik_joints_msg;
00029     ros::Publisher ik_joints_publisher_;
00030     Eigen::VectorXd joints_;
00031 
00032 
00033     // [subscriber attributes]
00034     ros::Subscriber joint_states_subscriber;
00035     void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg);
00036     std::vector<double> currentjoints;
00037 
00038     // [service attributes]
00039     ros::ServiceServer pose_move_server;
00040     bool pose_moveCallback(iri_wam_common_msgs::pose_move::Request &req, iri_wam_common_msgs::pose_move::Response &res);
00041     ros::ServiceServer wamik_server;
00042     bool wamikCallback(iri_wam_common_msgs::wamInverseKinematics::Request &req, iri_wam_common_msgs::wamInverseKinematics::Response &res);
00043 
00044     // [client attributes]
00045     ros::ServiceClient joint_move_client;
00046     iri_wam_common_msgs::joints_move joint_move_srv;
00047     static bool ik(std::vector<double> pose, std::vector<double> currentjoints, std::vector<double>& joints);
00048         // capçaleres
00049 
00050         static void fkine(Eigen::VectorXd theta,Eigen::MatrixXd& Tf,Eigen::MatrixXd& Jn);
00051         static void errortrans(Eigen::MatrixXd Tcurrent, Eigen::MatrixXd Tdesired, Eigen::VectorXd& errc);
00052         static void filtersols(Eigen::MatrixXd qsol,Eigen::VectorXd& sol);
00053         static void DHmatrix(double alpha, double a, double d,double& theta, Eigen::MatrixXd& T);
00054         static void skewop(Eigen::Matrix3d& M,Eigen::Vector3d v);
00055         static void potentialfunction(Eigen::VectorXd qref,Eigen::VectorXd& q, double& potq);
00056         static void sphericalikine(Eigen::MatrixXd T0,Eigen::MatrixXd T4, Eigen::Vector2d& q5,Eigen::Vector2d& q6,Eigen::Vector2d& q7);
00057         static void soltrig(double a, double b, double c, Eigen::MatrixXd& q2sol2);
00058         static void dotprod(Eigen::MatrixXd u,Eigen::MatrixXd v,double& p);
00059         static Eigen::MatrixXd crossprod(Eigen::MatrixXd u,Eigen::MatrixXd v);
00060         static void svdJ(Eigen::MatrixXd J,Eigen::MatrixXd& U,Eigen::MatrixXd& V,Eigen::MatrixXd& S);
00061         static void pseudoinverse(Eigen::MatrixXd J,Eigen::MatrixXd& Ji,Eigen::MatrixXd& U,Eigen::MatrixXd& V,Eigen::MatrixXd& S);
00062         static void Dpseudoinverse(Eigen::MatrixXd J,Eigen::MatrixXd& Ji,double lambda);
00063         static void Fpseudoinverse(Eigen::MatrixXd J,Eigen::MatrixXd& Ji,double lambda, double eps);
00064         static void CFpseudoinverse(Eigen::MatrixXd J,Eigen::MatrixXd& Ji,double nu,double s0);
00065 
00066         static Eigen::VectorXd pi2piwam(Eigen::VectorXd q);
00067         static Eigen::MatrixXd rcpinv(Eigen::MatrixXd Q,Eigen::MatrixXd W);
00068         static Eigen::MatrixXd lcpinv(Eigen::MatrixXd Q,Eigen::MatrixXd W);
00069         static Eigen::MatrixXd cpinv(Eigen::MatrixXd J,Eigen::MatrixXd H);
00070         static void HjlCONT(Eigen::VectorXd q,Eigen::MatrixXd& H,double b);
00071         static double fubeta(double x,double beta);
00072         static void clampjoints(Eigen::VectorXd q,Eigen::MatrixXd& H);
00073         static  void dhfunct(Eigen::VectorXd q,Eigen::VectorXd& dH);
00074         static  void manipgrad(Eigen::VectorXd q,double& man,Eigen::VectorXd& gradman);
00075         static  void manipulability(Eigen::MatrixXd J,double& man);
00076     // [action server attributes]
00077 
00078     // [action client attributes]
00079 
00080   protected:
00081   public:
00082     WamIKJ();
00083     void ikPub(void);
00084   
00085 };
00086 #endif