aubo_moveit_plugin.h
Go to the documentation of this file.
00001 #ifndef AUBO_KINEMATICS_PLUGIN_
00002 #define AUBO_KINEMATICS_PLUGIN_
00003 
00004 // ROS
00005 #include <ros/ros.h>
00006 #include <random_numbers/random_numbers.h>
00007 
00008 // System
00009 #include <boost/shared_ptr.hpp>
00010 
00011 // ROS msgs
00012 #include <geometry_msgs/PoseStamped.h>
00013 #include <moveit_msgs/GetPositionFK.h>
00014 #include <moveit_msgs/GetPositionIK.h>
00015 #include <moveit_msgs/GetKinematicSolverInfo.h>
00016 #include <moveit_msgs/MoveItErrorCodes.h>
00017 
00018 // KDL
00019 #include <kdl/jntarray.hpp>
00020 #include <kdl/chainiksolvervel_pinv.hpp>
00021 #include <kdl/chainiksolverpos_nr_jl.hpp>
00022 #include <kdl/chainfksolverpos_recursive.hpp>
00023 #include <moveit/kdl_kinematics_plugin/chainiksolver_pos_nr_jl_mimic.hpp>
00024 #include <moveit/kdl_kinematics_plugin/chainiksolver_vel_pinv_mimic.hpp>
00025 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
00026 
00027 // MoveIt!
00028 #include <moveit/kinematics_base/kinematics_base.h>
00029 #include <moveit/robot_model/robot_model.h>
00030 #include <moveit/robot_state/robot_state.h>
00031 
00032 namespace aubo_kinematics
00033 {
00037   class AuboKinematicsPlugin : public kinematics::KinematicsBase
00038   {
00039     public:
00040 
00044     AuboKinematicsPlugin();
00045 
00046     virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00047                                const std::vector<double> &ik_seed_state,
00048                                std::vector<double> &solution,
00049                                moveit_msgs::MoveItErrorCodes &error_code,
00050                                const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00051 
00052     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00053                                   const std::vector<double> &ik_seed_state,
00054                                   double timeout,
00055                                   std::vector<double> &solution,
00056                                   moveit_msgs::MoveItErrorCodes &error_code,
00057                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00058 
00059     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00060                                   const std::vector<double> &ik_seed_state,
00061                                   double timeout,
00062                                   const std::vector<double> &consistency_limits,
00063                                   std::vector<double> &solution,
00064                                   moveit_msgs::MoveItErrorCodes &error_code,
00065                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00066 
00067     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00068                                   const std::vector<double> &ik_seed_state,
00069                                   double timeout,
00070                                   std::vector<double> &solution,
00071                                   const IKCallbackFn &solution_callback,
00072                                   moveit_msgs::MoveItErrorCodes &error_code,
00073                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00074 
00075     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00076                                   const std::vector<double> &ik_seed_state,
00077                                   double timeout,
00078                                   const std::vector<double> &consistency_limits,
00079                                   std::vector<double> &solution,
00080                                   const IKCallbackFn &solution_callback,
00081                                   moveit_msgs::MoveItErrorCodes &error_code,
00082                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00083 
00084     virtual bool getPositionFK(const std::vector<std::string> &link_names,
00085                                const std::vector<double> &joint_angles,
00086                                std::vector<geometry_msgs::Pose> &poses) const;
00087 
00088     virtual bool initialize(const std::string &robot_description,
00089                             const std::string &group_name,
00090                             const std::string &base_name,
00091                             const std::string &tip_name,
00092                             double search_discretization);
00093 
00097     const std::vector<std::string>& getJointNames() const;
00098 
00102     const std::vector<std::string>& getLinkNames() const;
00103 
00104   protected:
00105 
00121     bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00122                           const std::vector<double> &ik_seed_state,
00123                           double timeout,
00124                           std::vector<double> &solution,
00125                           const IKCallbackFn &solution_callback,
00126                           moveit_msgs::MoveItErrorCodes &error_code,
00127                           const std::vector<double> &consistency_limits,
00128                           const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00129 
00130     virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
00131 
00132   private:
00133 
00134     bool timedOut(const ros::WallTime &start_time, double duration) const;
00135 
00136 
00144     bool checkConsistency(const KDL::JntArray& seed_state,
00145                           const std::vector<double> &consistency_limit,
00146                           const KDL::JntArray& solution) const;
00147 
00148     int getJointIndex(const std::string &name) const;
00149 
00150     int getKDLSegmentIndex(const std::string &name) const;
00151 
00152     void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const;
00153 
00160     void getRandomConfiguration(const KDL::JntArray& seed_state,
00161                                 const std::vector<double> &consistency_limits,
00162                                 KDL::JntArray &jnt_array,
00163                                 bool lock_redundancy) const;
00164 
00165     bool isRedundantJoint(unsigned int index) const;
00166 
00167     bool active_; 
00169     moveit_msgs::KinematicSolverInfo ik_chain_info_; 
00171     moveit_msgs::KinematicSolverInfo fk_chain_info_; 
00173     KDL::Chain kdl_chain_;
00174 
00175     unsigned int dimension_; 
00177     KDL::JntArray joint_min_, joint_max_; 
00179     mutable random_numbers::RandomNumberGenerator random_number_generator_;
00180 
00181     robot_model::RobotModelPtr robot_model_;
00182 
00183     robot_state::RobotStatePtr state_, state_2_;
00184 
00185     int num_possible_redundant_joints_;
00186     std::vector<unsigned int> redundant_joints_map_index_;
00187 
00188     // Storage required for when the set of redundant joints is reset
00189     bool position_ik_; //whether this solver is only being used for position ik
00190     robot_model::JointModelGroup* joint_model_group_;
00191     double max_solver_iterations_;
00192     double epsilon_;
00193     std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints_;
00194 
00195     std::vector<double> ik_weights_;
00196     std::vector<std::string> aubo_joint_names_;
00197     std::vector<std::string> aubo_link_names_;
00198     int aubo_joint_inds_start_;
00199     std::string arm_prefix_;
00200 
00201     // kinematic chains representing the chain from the group base link to the
00202     // Aubo base link, and the UR tip link to the group tip link
00203     KDL::Chain kdl_base_chain_;
00204     KDL::Chain kdl_tip_chain_;
00205 
00206   };
00207 }
00208 
00209 #endif


aubo_kinematics
Author(s): liuxin
autogenerated on Sat Jun 8 2019 19:05:58