iri_wam_ik: kuka_ikac.h Source File

Go to the documentation of this file.
00001 #ifndef _KUKA_IKAC_H_
00002 #define _KUKA_IKAC_H_
00003 
00004 #include <math.h>
00005 #include <cstddef>
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 #include <tf/transform_listener.h>
00014 
00015 // [publisher subscriber headers]
00016 #include "sensor_msgs/JointState.h"
00017 
00018 // [service client headers]
00019 #include "iri_wam_common_msgs/pose_move.h"
00020 #include "iri_wam_common_msgs/joints_move.h"
00021 #include "iri_wam_common_msgs/wamInverseKinematics.h"
00022 #include "iri_wam_common_msgs/wamInverseKinematicsFromPose.h"
00023 
00024 // [action server client headers]
00025 
00026 class KukaIKAC {
00027   private:
00028     double step_, d1, d3, d5, d7;
00029     Eigen::MatrixXd DHparam_;
00030     Eigen::MatrixXd Qlim_;
00031     int IMAX_,checksol_;
00032     std::vector<double> currentjoints;
00033 
00034     ros::NodeHandle nh_;
00035     
00036     tf::TransformListener listener_; 
00037     tf::StampedTransform tcp_H_wam7_;
00038 
00039     // [publisher attributes]
00040     sensor_msgs::JointState ik_joints_msg;
00041     ros::Publisher ik_joints_publisher_;
00042     Eigen::VectorXd joints_;
00043 
00044     // [subscriber attributes]
00045     ros::Subscriber joint_states_subscriber;
00046     void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg);
00047     std::vector<double> currentjoints_;
00048 
00049     // [service attributes]
00050     ros::ServiceServer kukaik_server_fromPose;
00051     bool kukaikCallbackFromPose(iri_wam_common_msgs::wamInverseKinematicsFromPose::Request &req, iri_wam_common_msgs::wamInverseKinematicsFromPose::Response &res);
00052     ros::ServiceServer kukaik_server;
00053     bool kukaikCallback(iri_wam_common_msgs::wamInverseKinematics::Request &req, iri_wam_common_msgs::wamInverseKinematics::Response &res);
00054 
00055     // [client attributes]
00056     ros::ServiceClient joint_move_client;
00057     iri_wam_common_msgs::joints_move joint_move_srv;
00058     // capçaleres
00059     bool ik(std::vector<double> pose, std::vector<double> currentjoints, std::vector<double>& joints);
00060           void filtersols(Eigen::MatrixXd qsol,Eigen::VectorXd& sol);
00061           void DHmatrix(double alpha,double a, double d,double& theta, Eigen::MatrixXd& T);
00062           void basicsols(Eigen::MatrixXd qsol,Eigen::MatrixXd& qaux,Eigen::VectorXd sol);
00063           void optimizesol(Eigen::MatrixXd q,Eigen::MatrixXd T0, Eigen::VectorXd& qbest,double& potq,Eigen::VectorXd qref);
00064           void skewop(Eigen::Matrix3d& M,Eigen::Vector3d v);
00065           void getq1(Eigen::VectorXd cjoints, float i, double& q10);
00066           double pi2pi(double x);
00067           void exactikine(Eigen::MatrixXd T0, double q10,Eigen::VectorXd cjoints,  Eigen::MatrixXd& qsol,Eigen::VectorXd&sol);
00068           void bestsol(Eigen::MatrixXd qsol,Eigen::VectorXd sol,Eigen::VectorXd qref,Eigen::VectorXd& q,double& potq);
00069           void potentialfunction(Eigen::VectorXd qref,Eigen::VectorXd& q, double& potq);
00070           void sphericalikine(Eigen::MatrixXd T0,Eigen::MatrixXd T4, Eigen::VectorXd cjoints, Eigen::Vector2d& q5,Eigen::Vector2d& q6,Eigen::Vector2d& q7);
00071           void initialelbows(Eigen::VectorXd q,Eigen::MatrixXd T0,Eigen::VectorXd& PJ);
00072           void soltrig(double a, double b, double c, Eigen::MatrixXd& q2sol2);
00073     // [action server attributes]
00074 
00075     // [action client attributes]
00076 
00077   protected:
00078   public:
00079     KukaIKAC();
00080     void ikPub(void);
00081   
00082 };
00083 #endif