iri_wam_ik: wam_ikac.h Source File

Go to the documentation of this file.
00001 #ifndef _WAM_IKAC_H_
00002 #define _WAM_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 WamIKAC {
00027   private:
00028     ros::NodeHandle nh_;
00029     
00030     tf::TransformListener listener_; 
00031     tf::StampedTransform tcp_H_wam7_;
00032 
00033 
00034     // [publisher attributes]
00035     sensor_msgs::JointState ik_joints_msg;
00036     ros::Publisher ik_joints_publisher_;
00037     Eigen::VectorXd joints_;
00038 
00039     // [subscriber attributes]
00040     ros::Subscriber joint_states_subscriber;
00041     void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg);
00042     std::vector<double> currentjoints_;
00043 
00044     // [service attributes]
00045     ros::ServiceServer wamik_server_fromPose;
00046     bool wamikCallbackFromPose(iri_wam_common_msgs::wamInverseKinematicsFromPose::Request &req, iri_wam_common_msgs::wamInverseKinematicsFromPose::Response &res);
00047     ros::ServiceServer wamik_server;
00048     bool wamikCallback(iri_wam_common_msgs::wamInverseKinematics::Request &req, iri_wam_common_msgs::wamInverseKinematics::Response &res);
00049 
00050     // [client attributes]
00051     ros::ServiceClient joint_move_client;
00052     iri_wam_common_msgs::joints_move joint_move_srv;
00053     static bool ik(std::vector<double> pose, std::vector<double> currentjoints, std::vector<double>& joints);
00054     // capçaleres
00055     static void filtersols(Eigen::MatrixXd qsol,Eigen::VectorXd& sol);
00056     static void DHmatrix(double alpha,double a, double d,double& theta, Eigen::MatrixXd& T);
00057     static void basicsols(Eigen::MatrixXd qsol,Eigen::MatrixXd& qaux,Eigen::VectorXd sol);
00058     static void optimizesol(Eigen::MatrixXd q,Eigen::MatrixXd T0, Eigen::VectorXd& qbest,double& potq,Eigen::VectorXd qref);
00059     static void skewop(Eigen::Matrix3d& M,Eigen::Vector3d v);
00060     static void getq1(Eigen::VectorXd cjoints,float i,double& q10,double step);
00061     static void exactikine(Eigen::MatrixXd T0, double q10, Eigen::MatrixXd& qsol,Eigen::VectorXd&sol);
00062     static void bestsol(Eigen::MatrixXd qsol,Eigen::VectorXd sol,Eigen::VectorXd qref,Eigen::VectorXd& q,double& potq);
00063     static void potentialfunction(Eigen::VectorXd qref,Eigen::VectorXd& q, double& potq);
00064     static void sphericalikine(Eigen::MatrixXd T0,Eigen::MatrixXd T4, Eigen::Vector2d& q5,Eigen::Vector2d& q6,Eigen::Vector2d& q7);
00065     static void initialelbows(Eigen::VectorXd q,Eigen::MatrixXd T0,Eigen::VectorXd& PL,Eigen::VectorXd& PJ);
00066     static void soltrig(double a, double b, double c, Eigen::MatrixXd& q2sol2);
00067     // [action server attributes]
00068 
00069     // [action client attributes]
00070 
00071   protected:
00072   public:
00073     WamIKAC();
00074     void ikPub(void);
00075   
00076 };
00077 #endif