kdl_kinematics_plugin.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */
00036 
00037 #include <moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h>
00038 #include <class_loader/class_loader.h>
00039 
00040 //#include <tf/transform_datatypes.h>
00041 #include <tf_conversions/tf_kdl.h>
00042 #include <kdl_parser/kdl_parser.hpp>
00043 
00044 // URDF, SRDF
00045 #include <urdf_model/model.h>
00046 #include <srdfdom/model.h>
00047 
00048 #include <moveit/rdf_loader/rdf_loader.h>
00049 
00050 //register KDLKinematics as a KinematicsBase implementation
00051 CLASS_LOADER_REGISTER_CLASS(kdl_kinematics_plugin::KDLKinematicsPlugin, kinematics::KinematicsBase)
00052 
00053 namespace kdl_kinematics_plugin
00054 {
00055 
00056   KDLKinematicsPlugin::KDLKinematicsPlugin():active_(false) {}
00057 
00058 void KDLKinematicsPlugin::getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
00059 {
00060   std::vector<double> jnt_array_vector(dimension_, 0.0);
00061   state_->setToRandomPositions(joint_model_group_);
00062   state_->copyJointGroupPositions(joint_model_group_, &jnt_array_vector[0]);
00063   for (std::size_t i = 0; i < dimension_; ++i)
00064   {
00065     if (lock_redundancy)
00066       if (isRedundantJoint(i))
00067         continue;
00068     jnt_array(i) = jnt_array_vector[i];
00069   }
00070 }
00071 
00072 bool KDLKinematicsPlugin::isRedundantJoint(unsigned int index) const
00073 {
00074   for (std::size_t j=0; j < redundant_joint_indices_.size(); ++j)
00075     if (redundant_joint_indices_[j] == index)
00076       return true;
00077   return false;
00078 }
00079 
00080 void KDLKinematicsPlugin::getRandomConfiguration(const KDL::JntArray &seed_state,
00081                                                  const std::vector<double> &consistency_limits,
00082                                                  KDL::JntArray &jnt_array,
00083                                                  bool lock_redundancy) const
00084 {
00085   std::vector<double> values(dimension_, 0.0);
00086   std::vector<double> near(dimension_, 0.0);
00087   for (std::size_t i = 0 ; i < dimension_; ++i)
00088     near[i] = seed_state(i);
00089 
00090   // Need to resize the consistency limits to remove mimic joints
00091   std::vector<double> consistency_limits_mimic;
00092   for(std::size_t i = 0; i < dimension_; ++i)
00093   {
00094     if(!mimic_joints_[i].active)
00095       continue;
00096     consistency_limits_mimic.push_back(consistency_limits[i]);
00097   }
00098 
00099   joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near, consistency_limits_mimic);
00100   
00101   for (std::size_t i = 0; i < dimension_; ++i)
00102   {
00103     bool skip = false;
00104     if (lock_redundancy)
00105       for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
00106         if (redundant_joint_indices_[j] == i)
00107         {
00108           skip = true;
00109           break;
00110         }
00111     if (skip)
00112       continue;
00113     jnt_array(i) = values[i];
00114   }
00115 }
00116 
00117 bool KDLKinematicsPlugin::checkConsistency(const KDL::JntArray& seed_state,
00118                                            const std::vector<double> &consistency_limits,
00119                                            const KDL::JntArray& solution) const
00120 {
00121   for (std::size_t i = 0; i < dimension_; ++i)
00122     if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
00123       return false;
00124   return true;
00125 }
00126 
00127 bool KDLKinematicsPlugin::initialize(const std::string &robot_description,
00128                                      const std::string& group_name,
00129                                      const std::string& base_frame,
00130                                      const std::string& tip_frame,
00131                                      double search_discretization)
00132 {
00133   setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
00134 
00135   ros::NodeHandle private_handle("~");
00136   rdf_loader::RDFLoader rdf_loader(robot_description_);
00137   const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
00138   const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
00139 
00140   if (!urdf_model || !srdf)
00141   {
00142     ROS_ERROR_NAMED("kdl","URDF and SRDF must be loaded for KDL kinematics solver to work.");
00143     return false;
00144   }
00145 
00146   robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
00147 
00148   robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
00149   if (!joint_model_group)
00150     return false;
00151   
00152   if(!joint_model_group->isChain())
00153   {
00154     ROS_ERROR_NAMED("kdl","Group '%s' is not a chain", group_name.c_str());
00155     return false;
00156   }
00157   if(!joint_model_group->isSingleDOFJoints())
00158   {
00159     ROS_ERROR_NAMED("kdl","Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
00160     return false;
00161   }
00162 
00163   KDL::Tree kdl_tree;
00164 
00165   if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
00166   {
00167     ROS_ERROR_NAMED("kdl","Could not initialize tree object");
00168     return false;
00169   }
00170   if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
00171   {
00172     ROS_ERROR_NAMED("kdl","Could not initialize chain object");
00173     return false;
00174   }
00175 
00176   dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
00177   for (std::size_t i=0; i < joint_model_group->getJointModels().size(); ++i)
00178   {
00179     if(joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
00180     {
00181       ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
00182       const std::vector<moveit_msgs::JointLimits> &jvec = joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
00183       ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
00184     }
00185   }
00186 
00187   fk_chain_info_.joint_names = ik_chain_info_.joint_names;
00188   fk_chain_info_.limits = ik_chain_info_.limits;
00189 
00190   if(!joint_model_group->hasLinkModel(getTipFrame()))
00191   {
00192     ROS_ERROR_NAMED("kdl","Could not find tip name in joint group '%s'", group_name.c_str());
00193     return false;
00194   }
00195   ik_chain_info_.link_names.push_back(getTipFrame());
00196   fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
00197 
00198   joint_min_.resize(ik_chain_info_.limits.size());
00199   joint_max_.resize(ik_chain_info_.limits.size());
00200 
00201   for(unsigned int i=0; i < ik_chain_info_.limits.size(); i++)
00202   {
00203     joint_min_(i) = ik_chain_info_.limits[i].min_position;
00204     joint_max_(i) = ik_chain_info_.limits[i].max_position;
00205   }
00206 
00207   // Get Solver Parameters
00208   int max_solver_iterations;
00209   double epsilon;
00210   bool position_ik;
00211 
00212   private_handle.param("max_solver_iterations", max_solver_iterations, 500);
00213   private_handle.param("epsilon", epsilon, 1e-5);
00214   private_handle.param(group_name+"/position_only_ik", position_ik, false);
00215   ROS_DEBUG_NAMED("kdl","Looking in private handle: %s for param name: %s",
00216             private_handle.getNamespace().c_str(),
00217             (group_name+"/position_only_ik").c_str());
00218 
00219   if(position_ik)
00220     ROS_INFO_NAMED("kdl","Using position only ik");
00221 
00222   num_possible_redundant_joints_ = kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik? 3:6);
00223 
00224   // Check for mimic joints
00225   bool has_mimic_joints = joint_model_group->getMimicJointModels().size() > 0;
00226   std::vector<unsigned int> redundant_joints_map_index;
00227 
00228   std::vector<JointMimic> mimic_joints;
00229   unsigned int joint_counter = 0;
00230   for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
00231   {
00232     const robot_model::JointModel *jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
00233     
00234     //first check whether it belongs to the set of active joints in the group
00235     if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
00236     {
00237       JointMimic mimic_joint;
00238       mimic_joint.reset(joint_counter);
00239       mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
00240       mimic_joint.active = true;
00241       mimic_joints.push_back(mimic_joint);
00242       ++joint_counter;
00243       continue;
00244     }
00245     if (joint_model_group->hasJointModel(jm->getName()))
00246     {
00247       if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
00248       {
00249         JointMimic mimic_joint;
00250         mimic_joint.reset(joint_counter);
00251         mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
00252         mimic_joint.offset = jm->getMimicOffset();
00253         mimic_joint.multiplier = jm->getMimicFactor();
00254         mimic_joints.push_back(mimic_joint);
00255         continue;
00256       }
00257     }
00258   }
00259   for (std::size_t i = 0; i < mimic_joints.size(); ++i)
00260   {
00261     if(!mimic_joints[i].active)
00262     {
00263       const robot_model::JointModel* joint_model = joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
00264       for(std::size_t j=0; j < mimic_joints.size(); ++j)
00265       {
00266         if(mimic_joints[j].joint_name == joint_model->getName())
00267         {
00268           mimic_joints[i].map_index = mimic_joints[j].map_index;
00269         }
00270       }
00271     }
00272   }
00273   mimic_joints_ = mimic_joints;
00274 
00275   // Setup the joint state groups that we need
00276   state_.reset(new robot_state::RobotState(robot_model_));
00277   state_2_.reset(new robot_state::RobotState(robot_model_));
00278 
00279   // Store things for when the set of redundant joints may change
00280   position_ik_ = position_ik;
00281   joint_model_group_ = joint_model_group;
00282   max_solver_iterations_ = max_solver_iterations;
00283   epsilon_ = epsilon;
00284 
00285   active_ = true;
00286   ROS_DEBUG_NAMED("kdl","KDL solver initialized");
00287   return true;
00288 }
00289 
00290 bool KDLKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int> &redundant_joints)
00291 {
00292   if(num_possible_redundant_joints_ < 0)
00293   {
00294     ROS_ERROR_NAMED("kdl","This group cannot have redundant joints");
00295     return false;
00296   }
00297   if(redundant_joints.size() > num_possible_redundant_joints_)
00298   {
00299     ROS_ERROR_NAMED("kdl","This group can only have %d redundant joints", num_possible_redundant_joints_);
00300     return false;
00301   }
00302   /*
00303     XmlRpc::XmlRpcValue joint_list;
00304     if(private_handle.getParam(group_name+"/redundant_joints", joint_list))
00305     {
00306       ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00307       std::vector<std::string> redundant_joints;
00308       for (std::size_t i = 0; i < joint_list.size(); ++i)
00309       {
00310         ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
00311         redundant_joints.push_back(static_cast<std::string>(joint_list[i]));
00312         ROS_INFO_NAMED("kdl","Designated joint: %s as redundant joint", redundant_joints.back().c_str());
00313       }
00314     }
00315   */
00316   std::vector<unsigned int> redundant_joints_map_index;
00317   unsigned int counter = 0;
00318   for(std::size_t i=0; i < dimension_; ++i)
00319   {
00320     bool is_redundant_joint = false;
00321     for(std::size_t j=0; j < redundant_joints.size(); ++j)
00322     {
00323       if(i == redundant_joints[j])
00324       {
00325         is_redundant_joint = true;
00326         counter++;
00327         break;
00328       }
00329     }
00330     if(!is_redundant_joint)
00331     {
00332       // check for mimic
00333       if(mimic_joints_[i].active) 
00334       {
00335         redundant_joints_map_index.push_back(counter);
00336         counter++;
00337       }
00338     }
00339   }
00340   for(std::size_t i=0; i < redundant_joints_map_index.size(); ++i)
00341     ROS_DEBUG_NAMED("kdl","Redundant joint map index: %d %d", (int) i, (int) redundant_joints_map_index[i]);
00342 
00343   redundant_joints_map_index_ = redundant_joints_map_index;
00344   redundant_joint_indices_ = redundant_joints;
00345   return true;
00346 }
00347 
00348 int KDLKinematicsPlugin::getJointIndex(const std::string &name) const
00349 {
00350   for (unsigned int i=0; i < ik_chain_info_.joint_names.size(); i++) {
00351     if (ik_chain_info_.joint_names[i] == name)
00352       return i;
00353   }
00354   return -1;
00355 }
00356 
00357 int KDLKinematicsPlugin::getKDLSegmentIndex(const std::string &name) const
00358 {
00359   int i=0;
00360   while (i < (int)kdl_chain_.getNrOfSegments()) {
00361     if (kdl_chain_.getSegment(i).getName() == name) {
00362       return i+1;
00363     }
00364     i++;
00365   }
00366   return -1;
00367 }
00368 
00369 bool KDLKinematicsPlugin::timedOut(const ros::WallTime &start_time, double duration) const
00370 {
00371   return ((ros::WallTime::now()-start_time).toSec() >= duration);
00372 }
00373 
00374 bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00375                                         const std::vector<double> &ik_seed_state,
00376                                         std::vector<double> &solution,
00377                                         moveit_msgs::MoveItErrorCodes &error_code,
00378                                         const kinematics::KinematicsQueryOptions &options) const
00379 {
00380   const IKCallbackFn solution_callback = 0;
00381   std::vector<double> consistency_limits;
00382 
00383   return searchPositionIK(ik_pose,
00384                           ik_seed_state,
00385                           default_timeout_,
00386                           solution,
00387                           solution_callback,
00388                           error_code,
00389                           consistency_limits,
00390                           options);
00391 }
00392 
00393 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00394                                            const std::vector<double> &ik_seed_state,
00395                                            double timeout,
00396                                            std::vector<double> &solution,
00397                                            moveit_msgs::MoveItErrorCodes &error_code,
00398                                            const kinematics::KinematicsQueryOptions &options) const
00399 {
00400   const IKCallbackFn solution_callback = 0;
00401   std::vector<double> consistency_limits;
00402 
00403   return searchPositionIK(ik_pose,
00404                           ik_seed_state,
00405                           timeout,
00406                           solution,
00407                           solution_callback,
00408                           error_code,
00409                           consistency_limits,
00410                           options);
00411 }
00412 
00413 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00414                                            const std::vector<double> &ik_seed_state,
00415                                            double timeout,
00416                                            const std::vector<double> &consistency_limits,
00417                                            std::vector<double> &solution,
00418                                            moveit_msgs::MoveItErrorCodes &error_code,
00419                                            const kinematics::KinematicsQueryOptions &options) const
00420 {
00421   const IKCallbackFn solution_callback = 0;
00422   return searchPositionIK(ik_pose,
00423                           ik_seed_state,
00424                           timeout,
00425                           solution,
00426                           solution_callback,
00427                           error_code,
00428                           consistency_limits,
00429                           options);
00430 }
00431 
00432 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00433                                            const std::vector<double> &ik_seed_state,
00434                                            double timeout,
00435                                            std::vector<double> &solution,
00436                                            const IKCallbackFn &solution_callback,
00437                                            moveit_msgs::MoveItErrorCodes &error_code,
00438                                            const kinematics::KinematicsQueryOptions &options) const
00439 {
00440   std::vector<double> consistency_limits;
00441   return searchPositionIK(ik_pose,
00442                           ik_seed_state,
00443                           timeout,
00444                           solution,
00445                           solution_callback,
00446                           error_code,
00447                           consistency_limits,
00448                           options);
00449 }
00450 
00451 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00452                                            const std::vector<double> &ik_seed_state,
00453                                            double timeout,
00454                                            const std::vector<double> &consistency_limits,
00455                                            std::vector<double> &solution,
00456                                            const IKCallbackFn &solution_callback,
00457                                            moveit_msgs::MoveItErrorCodes &error_code,
00458                                            const kinematics::KinematicsQueryOptions &options) const
00459 {
00460   return searchPositionIK(ik_pose,
00461                           ik_seed_state,
00462                           timeout,
00463                           solution,
00464                           solution_callback,
00465                           error_code,
00466                           consistency_limits,
00467                           options);
00468 }
00469 
00470 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00471                                            const std::vector<double> &ik_seed_state,
00472                                            double timeout,
00473                                            std::vector<double> &solution,
00474                                            const IKCallbackFn &solution_callback,
00475                                            moveit_msgs::MoveItErrorCodes &error_code,
00476                                            const std::vector<double> &consistency_limits,
00477                                            const kinematics::KinematicsQueryOptions &options) const
00478 {
00479   ros::WallTime n1 = ros::WallTime::now();
00480   if(!active_)
00481   {
00482     ROS_ERROR_NAMED("kdl","kinematics not active");
00483     error_code.val = error_code.NO_IK_SOLUTION;
00484     return false;
00485   }
00486 
00487   if(ik_seed_state.size() != dimension_)
00488   {
00489     ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
00490     error_code.val = error_code.NO_IK_SOLUTION;
00491     return false;
00492   }
00493 
00494   if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
00495   {
00496     ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
00497     error_code.val = error_code.NO_IK_SOLUTION;
00498     return false;
00499   }
00500 
00501   KDL::JntArray jnt_seed_state(dimension_);
00502   KDL::JntArray jnt_pos_in(dimension_);
00503   KDL::JntArray jnt_pos_out(dimension_);
00504 
00505   KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
00506   KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), redundant_joint_indices_.size(), position_ik_);
00507   KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel, max_solver_iterations_, epsilon_, position_ik_);
00508   ik_solver_vel.setMimicJoints(mimic_joints_);
00509   ik_solver_pos.setMimicJoints(mimic_joints_);
00510 
00511   if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
00512   {
00513     ROS_ERROR_NAMED("kdl","Could not set redundant joints");
00514     return false;
00515   }
00516 
00517   if(options.lock_redundant_joints)
00518   {
00519     ik_solver_vel.lockRedundantJoints();
00520   }
00521 
00522   solution.resize(dimension_);
00523 
00524   KDL::Frame pose_desired;
00525   tf::poseMsgToKDL(ik_pose, pose_desired);
00526 
00527   ROS_DEBUG_STREAM_NAMED("kdl","searchPositionIK2: Position request pose is " <<
00528                    ik_pose.position.x << " " <<
00529                    ik_pose.position.y << " " <<
00530                    ik_pose.position.z << " " <<
00531                    ik_pose.orientation.x << " " <<
00532                    ik_pose.orientation.y << " " <<
00533                    ik_pose.orientation.z << " " <<
00534                    ik_pose.orientation.w);
00535   //Do the IK
00536   for(unsigned int i=0; i < dimension_; i++)
00537     jnt_seed_state(i) = ik_seed_state[i];
00538   jnt_pos_in = jnt_seed_state;
00539 
00540   unsigned int counter(0);
00541   while(1)
00542   {
00543     //    ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
00544     counter++;
00545     if(timedOut(n1,timeout))
00546     {
00547       ROS_DEBUG_NAMED("kdl","IK timed out");
00548       error_code.val = error_code.TIMED_OUT;
00549       ik_solver_vel.unlockRedundantJoints();
00550       return false;
00551     }
00552     int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
00553     ROS_DEBUG_NAMED("kdl","IK valid: %d", ik_valid);
00554     if(!consistency_limits.empty())
00555     {
00556       getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
00557       if( (ik_valid < 0 && !options.return_approximate_solution) || !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
00558       {
00559         ROS_DEBUG_NAMED("kdl","Could not find IK solution: does not match consistency limits");
00560         continue;
00561       }
00562     }
00563     else
00564     {
00565       getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
00566       ROS_DEBUG_NAMED("kdl","New random configuration");
00567       for(unsigned int j=0; j < dimension_; j++)
00568         ROS_DEBUG_NAMED("kdl","%d %f", j, jnt_pos_in(j));
00569 
00570       if(ik_valid < 0 && !options.return_approximate_solution)
00571       {
00572         ROS_DEBUG_NAMED("kdl","Could not find IK solution");
00573         continue;
00574       }
00575     }
00576     ROS_DEBUG_NAMED("kdl","Found IK solution");
00577     for(unsigned int j=0; j < dimension_; j++)
00578       solution[j] = jnt_pos_out(j);
00579     if(!solution_callback.empty())
00580       solution_callback(ik_pose,solution,error_code);
00581     else
00582       error_code.val = error_code.SUCCESS;
00583 
00584     if(error_code.val == error_code.SUCCESS)
00585     {
00586       ROS_DEBUG_STREAM_NAMED("kdl","Solved after " << counter << " iterations");
00587       ik_solver_vel.unlockRedundantJoints();
00588       return true;
00589     }
00590   }
00591   ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
00592   error_code.val = error_code.NO_IK_SOLUTION;
00593   ik_solver_vel.unlockRedundantJoints();
00594   return false;
00595 }
00596 
00597 bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00598                                         const std::vector<double> &joint_angles,
00599                                         std::vector<geometry_msgs::Pose> &poses) const
00600 {
00601   ros::WallTime n1 = ros::WallTime::now();
00602   if(!active_)
00603   {
00604     ROS_ERROR_NAMED("kdl","kinematics not active");
00605     return false;
00606   }
00607   poses.resize(link_names.size());
00608   if(joint_angles.size() != dimension_)
00609   {
00610     ROS_ERROR_NAMED("kdl","Joint angles vector must have size: %d",dimension_);
00611     return false;
00612   }
00613 
00614   KDL::Frame p_out;
00615   geometry_msgs::PoseStamped pose;
00616   tf::Stamped<tf::Pose> tf_pose;
00617 
00618   KDL::JntArray jnt_pos_in(dimension_);
00619   for(unsigned int i=0; i < dimension_; i++)
00620   {
00621     jnt_pos_in(i) = joint_angles[i];
00622   }
00623 
00624   KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
00625 
00626   bool valid = true;
00627   for(unsigned int i=0; i < poses.size(); i++)
00628   {
00629     ROS_DEBUG_NAMED("kdl","End effector index: %d",getKDLSegmentIndex(link_names[i]));
00630     if(fk_solver.JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
00631     {
00632       tf::poseKDLToMsg(p_out,poses[i]);
00633     }
00634     else
00635     {
00636       ROS_ERROR_NAMED("kdl","Could not compute FK for %s",link_names[i].c_str());
00637       valid = false;
00638     }
00639   }
00640   return valid;
00641 }
00642 
00643 const std::vector<std::string>& KDLKinematicsPlugin::getJointNames() const
00644 {
00645   return ik_chain_info_.joint_names;
00646 }
00647 
00648 const std::vector<std::string>& KDLKinematicsPlugin::getLinkNames() const
00649 {
00650   return ik_chain_info_.link_names;
00651 }
00652 
00653 } // namespace


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:30