aubo_moveit_plugin.cpp
Go to the documentation of this file.
00001 #include <class_loader/class_loader.h>
00002 
00003 //#include <tf/transform_datatypes.h>
00004 #include <tf_conversions/tf_kdl.h>
00005 #include <kdl_parser/kdl_parser.hpp>
00006 
00007 // URDF, SRDF
00008 #include <urdf_model/model.h>
00009 #include <srdfdom/model.h>
00010 
00011 #include <moveit/rdf_loader/rdf_loader.h>
00012 
00013 
00014 #include <aubo_kinematics/aubo_moveit_plugin.h>
00015 #include <aubo_kinematics/aubo_kin.h>
00016 
00017 //register KDLKinematics as a KinematicsBase implementation
00018 CLASS_LOADER_REGISTER_CLASS(aubo_kinematics::AuboKinematicsPlugin, kinematics::KinematicsBase)
00019 
00020 namespace aubo_kinematics
00021 {
00022 
00023   AuboKinematicsPlugin::AuboKinematicsPlugin():active_(false) {}
00024 
00025 void AuboKinematicsPlugin::getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
00026 {
00027   std::vector<double> jnt_array_vector(dimension_, 0.0);
00028   state_->setToRandomPositions(joint_model_group_);
00029   state_->copyJointGroupPositions(joint_model_group_, &jnt_array_vector[0]);
00030   for (std::size_t i = 0; i < dimension_; ++i)
00031   {
00032     if (lock_redundancy)
00033       if (isRedundantJoint(i))
00034         continue;
00035     jnt_array(i) = jnt_array_vector[i];
00036   }
00037 }
00038 
00039 bool AuboKinematicsPlugin::isRedundantJoint(unsigned int index) const
00040 {
00041   for (std::size_t j=0; j < redundant_joint_indices_.size(); ++j)
00042     if (redundant_joint_indices_[j] == index)
00043       return true;
00044   return false;
00045 }
00046 
00047 void AuboKinematicsPlugin::getRandomConfiguration(const KDL::JntArray &seed_state,
00048                                                  const std::vector<double> &consistency_limits,
00049                                                  KDL::JntArray &jnt_array,
00050                                                  bool lock_redundancy) const
00051 {
00052   std::vector<double> values(dimension_, 0.0);
00053   std::vector<double> near(dimension_, 0.0);
00054   for (std::size_t i = 0 ; i < dimension_; ++i)
00055     near[i] = seed_state(i);
00056 
00057   // Need to resize the consistency limits to remove mimic joints
00058   std::vector<double> consistency_limits_mimic;
00059   for(std::size_t i = 0; i < dimension_; ++i)
00060   {
00061     if(!mimic_joints_[i].active)
00062       continue;
00063     consistency_limits_mimic.push_back(consistency_limits[i]);
00064   }
00065 
00066   joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near, consistency_limits_mimic);
00067   
00068   for (std::size_t i = 0; i < dimension_; ++i)
00069   {
00070     bool skip = false;
00071     if (lock_redundancy)
00072       for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
00073         if (redundant_joint_indices_[j] == i)
00074         {
00075           skip = true;
00076           break;
00077         }
00078     if (skip)
00079       continue;
00080     jnt_array(i) = values[i];
00081   }
00082 }
00083 
00084 bool AuboKinematicsPlugin::checkConsistency(const KDL::JntArray& seed_state,
00085                                            const std::vector<double> &consistency_limits,
00086                                            const KDL::JntArray& solution) const
00087 {
00088   for (std::size_t i = 0; i < dimension_; ++i)
00089     if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
00090       return false;
00091   return true;
00092 }
00093 
00094 bool AuboKinematicsPlugin::initialize(const std::string &robot_description,
00095                                      const std::string& group_name,
00096                                      const std::string& base_frame,
00097                                      const std::string& tip_frame,
00098                                      double search_discretization)
00099 {
00100   setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
00101 
00102   ros::NodeHandle private_handle("~");
00103   rdf_loader::RDFLoader rdf_loader(robot_description_);
00104   const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
00105   const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
00106 
00107   if (!urdf_model || !srdf)
00108   {
00109     ROS_ERROR_NAMED("kdl","URDF and SRDF must be loaded for KDL kinematics solver to work.");
00110     return false;
00111   }
00112 
00113   robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
00114 
00115   robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
00116   if (!joint_model_group)
00117     return false;
00118   
00119   if(!joint_model_group->isChain())
00120   {
00121     ROS_ERROR_NAMED("kdl","Group '%s' is not a chain", group_name.c_str());
00122     return false;
00123   }
00124   if(!joint_model_group->isSingleDOFJoints())
00125   {
00126     ROS_ERROR_NAMED("kdl","Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
00127     return false;
00128   }
00129 
00130   KDL::Tree kdl_tree;
00131 
00132   if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
00133   {
00134     ROS_ERROR_NAMED("kdl","Could not initialize tree object");
00135     return false;
00136   }
00137   if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
00138   {
00139     ROS_ERROR_NAMED("kdl","Could not initialize chain object");
00140     return false;
00141   }
00142 
00143   dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
00144   for (std::size_t i=0; i < joint_model_group->getJointModels().size(); ++i)
00145   {
00146     if(joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
00147     {
00148       ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
00149       const std::vector<moveit_msgs::JointLimits> &jvec = joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
00150       ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
00151     }
00152   }
00153 
00154   fk_chain_info_.joint_names = ik_chain_info_.joint_names;
00155   fk_chain_info_.limits = ik_chain_info_.limits;
00156 
00157   if(!joint_model_group->hasLinkModel(getTipFrame()))
00158   {
00159     ROS_ERROR_NAMED("kdl","Could not find tip name in joint group '%s'", group_name.c_str());
00160     return false;
00161   }
00162   ik_chain_info_.link_names.push_back(getTipFrame());
00163   fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
00164 
00165   joint_min_.resize(ik_chain_info_.limits.size());
00166   joint_max_.resize(ik_chain_info_.limits.size());
00167 
00168   for(unsigned int i=0; i < ik_chain_info_.limits.size(); i++)
00169   {
00170     joint_min_(i) = ik_chain_info_.limits[i].min_position;
00171     joint_max_(i) = ik_chain_info_.limits[i].max_position;
00172   }
00173 
00174   // Get Solver Parameters
00175   int max_solver_iterations;
00176   double epsilon;
00177   bool position_ik;
00178 
00179   private_handle.param("max_solver_iterations", max_solver_iterations, 500);
00180   private_handle.param("epsilon", epsilon, 1e-5);
00181   private_handle.param(group_name+"/position_only_ik", position_ik, false);
00182   ROS_DEBUG_NAMED("kdl","Looking in private handle: %s for param name: %s",
00183             private_handle.getNamespace().c_str(),
00184             (group_name+"/position_only_ik").c_str());
00185 
00186   if(position_ik)
00187     ROS_ERROR_NAMED("kdl","Using position only ik");
00188 
00189   num_possible_redundant_joints_ = kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik? 3:6);
00190 
00191   // Check for mimic joints
00192   bool has_mimic_joints = joint_model_group->getMimicJointModels().size() > 0;
00193   std::vector<unsigned int> redundant_joints_map_index;
00194 
00195   std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints;
00196   unsigned int joint_counter = 0;
00197   for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
00198   {
00199     const robot_model::JointModel *jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
00200     
00201     //first check whether it belongs to the set of active joints in the group
00202     if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
00203     {
00204       kdl_kinematics_plugin::JointMimic mimic_joint;
00205       mimic_joint.reset(joint_counter);
00206       mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
00207       mimic_joint.active = true;
00208       mimic_joints.push_back(mimic_joint);
00209       ++joint_counter;
00210       continue;
00211     }
00212     if (joint_model_group->hasJointModel(jm->getName()))
00213     {
00214       if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
00215       {
00216         kdl_kinematics_plugin::JointMimic mimic_joint;
00217         mimic_joint.reset(joint_counter);
00218         mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
00219         mimic_joint.offset = jm->getMimicOffset();
00220         mimic_joint.multiplier = jm->getMimicFactor();
00221         mimic_joints.push_back(mimic_joint);
00222         continue;
00223       }
00224     }
00225   }
00226   for (std::size_t i = 0; i < mimic_joints.size(); ++i)
00227   {
00228     if(!mimic_joints[i].active)
00229     {
00230       const robot_model::JointModel* joint_model = joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
00231       for(std::size_t j=0; j < mimic_joints.size(); ++j)
00232       {
00233         if(mimic_joints[j].joint_name == joint_model->getName())
00234         {
00235           mimic_joints[i].map_index = mimic_joints[j].map_index;
00236         }
00237       }
00238     }
00239   }
00240   mimic_joints_ = mimic_joints;
00241 
00242   // Setup the joint state groups that we need
00243   state_.reset(new robot_state::RobotState(robot_model_));
00244   state_2_.reset(new robot_state::RobotState(robot_model_));
00245 
00246   // Store things for when the set of redundant joints may change
00247   position_ik_ = position_ik;
00248   joint_model_group_ = joint_model_group;
00249   max_solver_iterations_ = max_solver_iterations;
00250   epsilon_ = epsilon;
00251 
00252   private_handle.param<std::string>("arm_prefix", arm_prefix_, "");
00253 
00254   aubo_joint_names_.push_back(arm_prefix_ + "shoulder_joint");
00255   aubo_joint_names_.push_back(arm_prefix_ + "upperArm_joint");
00256   aubo_joint_names_.push_back(arm_prefix_ + "foreArm_joint");
00257   aubo_joint_names_.push_back(arm_prefix_ + "wrist1_joint");
00258   aubo_joint_names_.push_back(arm_prefix_ + "wrist2_joint");
00259   aubo_joint_names_.push_back(arm_prefix_ + "wrist3_joint");
00260 
00261   aubo_link_names_.push_back(arm_prefix_ + "base_Link");       // 0
00262   aubo_link_names_.push_back(arm_prefix_ + "shoulder_Link");   // 1
00263   aubo_link_names_.push_back(arm_prefix_ + "upperArm_Link");  // 2
00264   aubo_link_names_.push_back(arm_prefix_ + "foreArm_Link");    // 3
00265   aubo_link_names_.push_back(arm_prefix_ + "wrist1_Link");    // 4
00266   aubo_link_names_.push_back(arm_prefix_ + "wrist2_Link");    // 5
00267   aubo_link_names_.push_back(arm_prefix_ + "wrist3_Link");    // 6
00268   aubo_link_names_.push_back(arm_prefix_ + "ee_Link");    // 7
00269 
00270   aubo_joint_inds_start_ = getJointIndex(aubo_joint_names_[0]);
00271 
00272   // check to make sure the serial chain is properly defined in the model
00273   int cur_aubo_joint_ind, last_aubo_joint_ind = aubo_joint_inds_start_;
00274   for(int i=1; i<6; i++) {
00275     cur_aubo_joint_ind = getJointIndex(aubo_joint_names_[i]);
00276     if(cur_aubo_joint_ind < 0) {
00277       ROS_ERROR_NAMED("kdl", 
00278         "Kin chain provided in model doesn't contain standard Aubo joint '%s'.", 
00279         aubo_joint_names_[i].c_str());
00280       return false;
00281     }
00282     if(cur_aubo_joint_ind != last_aubo_joint_ind + 1) {
00283       ROS_ERROR_NAMED("kdl", 
00284         "Kin chain provided in model doesn't have proper serial joint order: '%s'.", 
00285         aubo_joint_names_[i].c_str());
00286       return false;
00287     }
00288     last_aubo_joint_ind = cur_aubo_joint_ind;
00289   }
00290   // if successful, the kinematic chain includes a serial chain of the AUBO joints
00291 
00292   kdl_tree.getChain(getBaseFrame(), aubo_link_names_.front(), kdl_base_chain_);
00293   kdl_tree.getChain(aubo_link_names_.back(), getTipFrame(), kdl_tip_chain_);
00294 
00295   // weights for redundant solution selection
00296   ik_weights_.resize(6);
00297   if(private_handle.hasParam("ik_weights")) {
00298     private_handle.getParam("ik_weights", ik_weights_);
00299   } else {
00300     ik_weights_[0] = 1.0;
00301     ik_weights_[1] = 1.0;
00302     ik_weights_[2] = 0.1;
00303     ik_weights_[3] = 0.1;
00304     ik_weights_[4] = 0.3;
00305     ik_weights_[5] = 0.3;
00306   }
00307 
00308   active_ = true;
00309   ROS_DEBUG_NAMED("kdl","KDL solver initialized");
00310   return true;
00311 }
00312 
00313 bool AuboKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int> &redundant_joints)
00314 {
00315   if(num_possible_redundant_joints_ < 0)
00316   {
00317     ROS_ERROR_NAMED("kdl","This group cannot have redundant joints");
00318     return false;
00319   }
00320   if(redundant_joints.size() > num_possible_redundant_joints_)
00321   {
00322     ROS_ERROR_NAMED("kdl","This group can only have %d redundant joints", num_possible_redundant_joints_);
00323     return false;
00324   }
00325   std::vector<unsigned int> redundant_joints_map_index;
00326   unsigned int counter = 0;
00327   for(std::size_t i=0; i < dimension_; ++i)
00328   {
00329     bool is_redundant_joint = false;
00330     for(std::size_t j=0; j < redundant_joints.size(); ++j)
00331     {
00332       if(i == redundant_joints[j])
00333       {
00334         is_redundant_joint = true;
00335 counter++;
00336         break;
00337       }
00338     }
00339     if(!is_redundant_joint)
00340     {
00341       // check for mimic
00342       if(mimic_joints_[i].active)
00343       {
00344 redundant_joints_map_index.push_back(counter);
00345 counter++;
00346       }
00347     }
00348   }
00349   for(std::size_t i=0; i < redundant_joints_map_index.size(); ++i)
00350     ROS_DEBUG_NAMED("kdl","Redundant joint map index: %d %d", (int) i, (int) redundant_joints_map_index[i]);
00351 
00352   redundant_joints_map_index_ = redundant_joints_map_index;
00353   redundant_joint_indices_ = redundant_joints;
00354   return true;
00355 }
00356 
00357 int AuboKinematicsPlugin::getJointIndex(const std::string &name) const
00358 {
00359   for (unsigned int i=0; i < ik_chain_info_.joint_names.size(); i++) {
00360     if (ik_chain_info_.joint_names[i] == name)
00361       return i;
00362   }
00363   return -1;
00364 }
00365 
00366 int AuboKinematicsPlugin::getKDLSegmentIndex(const std::string &name) const
00367 {
00368   int i=0;
00369   while (i < (int)kdl_chain_.getNrOfSegments()) {
00370     if (kdl_chain_.getSegment(i).getName() == name) {
00371       return i+1;
00372     }
00373     i++;
00374   }
00375   return -1;
00376 }
00377 
00378 bool AuboKinematicsPlugin::timedOut(const ros::WallTime &start_time, double duration) const
00379 {
00380   return ((ros::WallTime::now()-start_time).toSec() >= duration);
00381 }
00382 
00383 bool AuboKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00384                                         const std::vector<double> &ik_seed_state,
00385                                         std::vector<double> &solution,
00386                                         moveit_msgs::MoveItErrorCodes &error_code,
00387                                         const kinematics::KinematicsQueryOptions &options) const
00388 {
00389   const IKCallbackFn solution_callback = 0;
00390   std::vector<double> consistency_limits;
00391 
00392   return searchPositionIK(ik_pose,
00393                           ik_seed_state,
00394                           default_timeout_,
00395                           solution,
00396                           solution_callback,
00397                           error_code,
00398                           consistency_limits,
00399                           options);
00400 }
00401 
00402 bool AuboKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00403                                            const std::vector<double> &ik_seed_state,
00404                                            double timeout,
00405                                            std::vector<double> &solution,
00406                                            moveit_msgs::MoveItErrorCodes &error_code,
00407                                            const kinematics::KinematicsQueryOptions &options) const
00408 {
00409   const IKCallbackFn solution_callback = 0;
00410   std::vector<double> consistency_limits;
00411 
00412   return searchPositionIK(ik_pose,
00413                           ik_seed_state,
00414                           timeout,
00415                           solution,
00416                           solution_callback,
00417                           error_code,
00418                           consistency_limits,
00419                           options);
00420 }
00421 
00422 bool AuboKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00423                                            const std::vector<double> &ik_seed_state,
00424                                            double timeout,
00425                                            const std::vector<double> &consistency_limits,
00426                                            std::vector<double> &solution,
00427                                            moveit_msgs::MoveItErrorCodes &error_code,
00428                                            const kinematics::KinematicsQueryOptions &options) const
00429 {
00430   const IKCallbackFn solution_callback = 0;
00431   return searchPositionIK(ik_pose,
00432                           ik_seed_state,
00433                           timeout,
00434                           solution,
00435                           solution_callback,
00436                           error_code,
00437                           consistency_limits,
00438                           options);
00439 }
00440 
00441 bool AuboKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00442                                            const std::vector<double> &ik_seed_state,
00443                                            double timeout,
00444                                            std::vector<double> &solution,
00445                                            const IKCallbackFn &solution_callback,
00446                                            moveit_msgs::MoveItErrorCodes &error_code,
00447                                            const kinematics::KinematicsQueryOptions &options) const
00448 {
00449   std::vector<double> consistency_limits;
00450   return searchPositionIK(ik_pose,
00451                           ik_seed_state,
00452                           timeout,
00453                           solution,
00454                           solution_callback,
00455                           error_code,
00456                           consistency_limits,
00457                           options);
00458 }
00459 
00460 bool AuboKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00461                                            const std::vector<double> &ik_seed_state,
00462                                            double timeout,
00463                                            const std::vector<double> &consistency_limits,
00464                                            std::vector<double> &solution,
00465                                            const IKCallbackFn &solution_callback,
00466                                            moveit_msgs::MoveItErrorCodes &error_code,
00467                                            const kinematics::KinematicsQueryOptions &options) const
00468 {
00469   return searchPositionIK(ik_pose,
00470                           ik_seed_state,
00471                           timeout,
00472                           solution,
00473                           solution_callback,
00474                           error_code,
00475                           consistency_limits,
00476                           options);
00477 }
00478 
00479 typedef std::pair<int, double> idx_double;
00480 bool comparator(const idx_double& l, const idx_double& r)
00481 { return l.second < r.second; }
00482 
00483 bool AuboKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00484                                            const std::vector<double> &ik_seed_state,
00485                                            double timeout,
00486                                            std::vector<double> &solution,
00487                                            const IKCallbackFn &solution_callback,
00488                                            moveit_msgs::MoveItErrorCodes &error_code,
00489                                            const std::vector<double> &consistency_limits,
00490                                            const kinematics::KinematicsQueryOptions &options) const
00491 {
00492   ros::WallTime n1 = ros::WallTime::now();
00493   if(!active_) {
00494     ROS_ERROR_NAMED("kdl","kinematics not active");
00495     error_code.val = error_code.NO_IK_SOLUTION;
00496     return false;
00497   }
00498 
00499   if(ik_seed_state.size() != dimension_) {
00500     ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
00501     error_code.val = error_code.NO_IK_SOLUTION;
00502     return false;
00503   }
00504 
00505   if(!consistency_limits.empty() && consistency_limits.size() != dimension_) {
00506     ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
00507     error_code.val = error_code.NO_IK_SOLUTION;
00508     return false;
00509   }
00510 
00511   KDL::JntArray jnt_seed_state(dimension_);
00512   for(int i=0; i<dimension_; i++)
00513     jnt_seed_state(i) = ik_seed_state[i];
00514 
00515   solution.resize(dimension_);
00516 
00517   KDL::ChainFkSolverPos_recursive fk_solver_base(kdl_base_chain_);
00518   KDL::ChainFkSolverPos_recursive fk_solver_tip(kdl_tip_chain_);
00519 
00520   KDL::JntArray jnt_pos_test(jnt_seed_state);
00521   KDL::JntArray jnt_pos_base(aubo_joint_inds_start_);
00522   KDL::JntArray jnt_pos_tip(dimension_ - 6 - aubo_joint_inds_start_);
00523   KDL::Frame pose_base, pose_tip;
00524 
00525   KDL::Frame kdl_ik_pose;
00526   KDL::Frame kdl_ik_pose_aubo_chain;
00527   double homo_ik_pose[4][4];
00528   double q_ik_sols[8][6]; // maximum of 8 IK solutions
00529   uint16_t num_sols;
00530 
00531   while(1) {
00532     if(timedOut(n1, timeout)) {
00533       ROS_DEBUG_NAMED("kdl","IK timed out");
00534       error_code.val = error_code.TIMED_OUT;
00535       return false;
00536     }
00537 
00539     // find transformation from robot base to AUBO base and AUBO tip to robot tip
00540     for(uint32_t i=0; i<jnt_pos_base.rows(); i++)
00541       jnt_pos_base(i) = jnt_pos_test(i);
00542     for(uint32_t i=0; i<jnt_pos_tip.rows(); i++)
00543       jnt_pos_tip(i) = jnt_pos_test(i + aubo_joint_inds_start_ + 6);
00544     for(uint32_t i=0; i<jnt_seed_state.rows(); i++)
00545       solution[i] = jnt_pos_test(i);
00546 
00547     if(fk_solver_base.JntToCart(jnt_pos_base, pose_base) < 0) {
00548       ROS_ERROR_NAMED("kdl", "Could not compute FK for base chain");
00549       return false;
00550     }
00551 
00552     if(fk_solver_tip.JntToCart(jnt_pos_tip, pose_tip) < 0) {
00553       ROS_ERROR_NAMED("kdl", "Could not compute FK for tip chain");
00554       return false;
00555     }
00557 
00559     // Convert into query for analytic solver
00560     tf::poseMsgToKDL(ik_pose, kdl_ik_pose);
00561     kdl_ik_pose_aubo_chain = pose_base.Inverse() * kdl_ik_pose * pose_tip.Inverse();
00562     
00563     kdl_ik_pose_aubo_chain.Make4x4((double*) homo_ik_pose);
00564 #if KDL_OLD_BUG_FIX
00565     // in older versions of KDL, setting this flag might be necessary
00566     for(int i=0; i<3; i++) homo_ik_pose[i][3] *= 1000; // strange KDL fix
00567 #endif
00568 
00569 
00570 
00571 
00572     // Do the analytic IK
00573     num_sols = inverse((double*) homo_ik_pose, (double*) q_ik_sols);
00574 
00575     /*printf("Inverse\n");
00576     for(int i=0;i<num_sols;i++)
00577       printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n",
00578          q_ik_sols[i][0], q_ik_sols[i][1], q_ik_sols[i][2], q_ik_sols[i][3], q_ik_sols[i][4], q_ik_sols[i][5]);
00579     */
00580     
00581     uint16_t num_valid_sols;
00582     std::vector< std::vector<double> > q_ik_valid_sols;
00583     for(uint16_t i=0; i<num_sols; i++)
00584     {
00585       bool valid = true;
00586       std::vector< double > valid_solution;
00587       valid_solution.assign(6,0.0);
00588       
00589       for(uint16_t j=0; j<6; j++)
00590       {
00591         if((q_ik_sols[i][j] <= ik_chain_info_.limits[j].max_position) && (q_ik_sols[i][j] >= ik_chain_info_.limits[j].min_position))
00592         { 
00593           valid_solution[j] = q_ik_sols[i][j];
00594           valid = true;
00595           continue;
00596         }
00597         else if ((q_ik_sols[i][j] > ik_chain_info_.limits[j].max_position) && (q_ik_sols[i][j]-2*M_PI > ik_chain_info_.limits[j].min_position))
00598         {
00599           valid_solution[j] = q_ik_sols[i][j]-2*M_PI;
00600           valid = true;
00601           continue;
00602         }
00603         else if ((q_ik_sols[i][j] < ik_chain_info_.limits[j].min_position) && (q_ik_sols[i][j]+2*M_PI < ik_chain_info_.limits[j].max_position))
00604         {
00605           valid_solution[j] = q_ik_sols[i][j]+2*M_PI;
00606           valid = true;
00607           continue;
00608         }
00609         else
00610         {
00611           valid = false;
00612           break;
00613         }
00614       }
00615       
00616       if(valid)
00617       {
00618         q_ik_valid_sols.push_back(valid_solution);
00619       }
00620     }
00621      
00622      
00623     // use weighted absolute deviations to determine the solution closest the seed state
00624     std::vector<idx_double> weighted_diffs;
00625     for(uint16_t i=0; i<q_ik_valid_sols.size(); i++) {
00626       double cur_weighted_diff = 0;
00627       for(uint16_t j=0; j<6; j++) {
00628         // solution violates the consistency_limits, throw it out
00629         double abs_diff = std::fabs(ik_seed_state[aubo_joint_inds_start_+j] - q_ik_valid_sols[i][j]);
00630         if(!consistency_limits.empty() && abs_diff > consistency_limits[aubo_joint_inds_start_+j]) {
00631           cur_weighted_diff = std::numeric_limits<double>::infinity();
00632           break;
00633         }
00634         cur_weighted_diff += ik_weights_[j] * abs_diff;
00635       }
00636       weighted_diffs.push_back(idx_double(i, cur_weighted_diff));
00637     }
00638 
00639     std::sort(weighted_diffs.begin(), weighted_diffs.end(), comparator);
00640 
00641 #if 0
00642     printf("start\n");
00643     printf("                     q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", ik_seed_state[1], ik_seed_state[2], ik_seed_state[3], ik_seed_state[4], ik_seed_state[5], ik_seed_state[6]);
00644     for(uint16_t i=0; i<weighted_diffs.size(); i++) {
00645       int cur_idx = weighted_diffs[i].first;
00646       printf("diff %f, i %d, q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", weighted_diffs[i].second, cur_idx, q_ik_valid_sols[cur_idx][0], q_ik_valid_sols[cur_idx][1], q_ik_valid_sols[cur_idx][2], q_ik_valid_sols[cur_idx][3], q_ik_valid_sols[cur_idx][4], q_ik_valid_sols[cur_idx][5]);
00647     }
00648     printf("end\n");
00649 #endif
00650 
00651     for(uint16_t i=0; i<weighted_diffs.size(); i++) {
00652       if(weighted_diffs[i].second == std::numeric_limits<double>::infinity()) {
00653         // rest are infinity, no more feasible solutions
00654         break;
00655       }
00656 
00657       // copy the best solution to the output
00658       int cur_idx = weighted_diffs[i].first;
00659       solution = q_ik_valid_sols[cur_idx];
00660 
00661       // see if this solution passes the callback function test
00662       if(!solution_callback.empty())
00663         solution_callback(ik_pose, solution, error_code);
00664       else
00665         error_code.val = error_code.SUCCESS;
00666 
00667       if(error_code.val == error_code.SUCCESS) {
00668 #if 0
00669         std::vector<std::string> fk_link_names;
00670         fk_link_names.push_back(aubo_link_names_.back());
00671         std::vector<geometry_msgs::Pose> fk_poses;
00672         getPositionFK(fk_link_names, solution, fk_poses);
00673         KDL::Frame kdl_fk_pose;
00674         tf::poseMsgToKDL(fk_poses[0], kdl_fk_pose);
00675         printf("FK(solution) - pose \n");
00676         for(int i=0; i<4; i++) {
00677           for(int j=0; j<4; j++)
00678             printf("%1.6f ", kdl_fk_pose(i, j)-kdl_ik_pose(i, j));
00679           printf("\n");
00680         }
00681 #endif
00682         return true;
00683       }
00684     }
00685     // none of the solutions were both consistent and passed the solution callback
00686 
00687     if(options.lock_redundant_joints) {
00688       ROS_DEBUG_NAMED("kdl","Will not pertubate redundant joints to find solution");
00689       break;
00690     }
00691 
00692     if(dimension_ == 6) {
00693       ROS_DEBUG_NAMED("kdl","No other joints to pertubate, cannot find solution");
00694       break;
00695     }
00696 
00697     // randomly pertubate other joints and try again
00698     if(!consistency_limits.empty()) {
00699       getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_test, false);
00700     } else {
00701       getRandomConfiguration(jnt_pos_test, false);
00702     }
00703   }
00704 
00705   ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
00706   error_code.val = error_code.NO_IK_SOLUTION;
00707   return false;
00708 }
00709 
00710 bool AuboKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00711                                         const std::vector<double> &joint_angles,
00712                                         std::vector<geometry_msgs::Pose> &poses) const
00713 {
00714   ros::WallTime n1 = ros::WallTime::now();
00715   if(!active_)
00716   {
00717     ROS_ERROR_NAMED("kdl","kinematics not active");
00718     return false;
00719   }
00720   poses.resize(link_names.size());
00721   if(joint_angles.size() != dimension_)
00722   {
00723     ROS_ERROR_NAMED("kdl","Joint angles vector must have size: %d",dimension_);
00724     return false;
00725   }
00726 
00727   KDL::Frame p_out;
00728   geometry_msgs::PoseStamped pose;
00729   tf::Stamped<tf::Pose> tf_pose;
00730 
00731   KDL::JntArray jnt_pos_in(dimension_);
00732   for(unsigned int i=0; i < dimension_; i++)
00733   {
00734     jnt_pos_in(i) = joint_angles[i];
00735   }
00736 
00737   KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
00738 
00739   bool valid = true;
00740   for(unsigned int i=0; i < poses.size(); i++)
00741   {
00742     ROS_DEBUG_NAMED("kdl","End effector index: %d",getKDLSegmentIndex(link_names[i]));
00743     if(fk_solver.JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
00744     {
00745       tf::poseKDLToMsg(p_out,poses[i]);
00746     }
00747     else
00748     {
00749       ROS_ERROR_NAMED("kdl","Could not compute FK for %s",link_names[i].c_str());
00750       valid = false;
00751     }
00752   }
00753   return valid;
00754 }
00755 
00756 const std::vector<std::string>& AuboKinematicsPlugin::getJointNames() const
00757 {
00758   return ik_chain_info_.joint_names;
00759 }
00760 
00761 const std::vector<std::string>& AuboKinematicsPlugin::getLinkNames() const
00762 {
00763   return ik_chain_info_.link_names;
00764 }
00765 
00766 } // namespace


aubo_kinematics
Author(s): liuxin
autogenerated on Wed Sep 6 2017 03:06:35