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 KDLKinematicsPlugin::KDLKinematicsPlugin() : active_(false)
00056 {
00057 }
00058 
00059 void KDLKinematicsPlugin::getRandomConfiguration(KDL::JntArray& jnt_array, bool lock_redundancy) const
00060 {
00061   std::vector<double> jnt_array_vector(dimension_, 0.0);
00062   state_->setToRandomPositions(joint_model_group_);
00063   state_->copyJointGroupPositions(joint_model_group_, &jnt_array_vector[0]);
00064   for (std::size_t i = 0; i < dimension_; ++i)
00065   {
00066     if (lock_redundancy)
00067       if (isRedundantJoint(i))
00068         continue;
00069     jnt_array(i) = jnt_array_vector[i];
00070   }
00071 }
00072 
00073 bool KDLKinematicsPlugin::isRedundantJoint(unsigned int index) const
00074 {
00075   for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
00076     if (redundant_joint_indices_[j] == index)
00077       return true;
00078   return false;
00079 }
00080 
00081 void KDLKinematicsPlugin::getRandomConfiguration(const KDL::JntArray& seed_state,
00082                                                  const std::vector<double>& consistency_limits,
00083                                                  KDL::JntArray& jnt_array, 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,
00100                                                        consistency_limits_mimic);
00101 
00102   for (std::size_t i = 0; i < dimension_; ++i)
00103   {
00104     bool skip = false;
00105     if (lock_redundancy)
00106       for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
00107         if (redundant_joint_indices_[j] == i)
00108         {
00109           skip = true;
00110           break;
00111         }
00112     if (skip)
00113       continue;
00114     jnt_array(i) = values[i];
00115   }
00116 }
00117 
00118 bool KDLKinematicsPlugin::checkConsistency(const KDL::JntArray& seed_state,
00119                                            const std::vector<double>& consistency_limits,
00120                                            const KDL::JntArray& solution) const
00121 {
00122   for (std::size_t i = 0; i < dimension_; ++i)
00123     if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
00124       return false;
00125   return true;
00126 }
00127 
00128 bool KDLKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
00129                                      const std::string& base_frame, const std::string& tip_frame,
00130                                      double search_discretization)
00131 {
00132   setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
00133 
00134   rdf_loader::RDFLoader rdf_loader(robot_description_);
00135   const boost::shared_ptr<srdf::Model>& srdf = rdf_loader.getSRDF();
00136   const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
00137 
00138   if (!urdf_model || !srdf)
00139   {
00140     ROS_ERROR_NAMED("kdl", "URDF and SRDF must be loaded for KDL kinematics solver to work.");
00141     return false;
00142   }
00143 
00144   robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
00145 
00146   robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
00147   if (!joint_model_group)
00148     return false;
00149 
00150   if (!joint_model_group->isChain())
00151   {
00152     ROS_ERROR_NAMED("kdl", "Group '%s' is not a chain", group_name.c_str());
00153     return false;
00154   }
00155   if (!joint_model_group->isSingleDOFJoints())
00156   {
00157     ROS_ERROR_NAMED("kdl", "Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
00158     return false;
00159   }
00160 
00161   KDL::Tree kdl_tree;
00162 
00163   if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
00164   {
00165     ROS_ERROR_NAMED("kdl", "Could not initialize tree object");
00166     return false;
00167   }
00168   if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
00169   {
00170     ROS_ERROR_NAMED("kdl", "Could not initialize chain object");
00171     return false;
00172   }
00173 
00174   dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
00175   for (std::size_t i = 0; i < joint_model_group->getJointModels().size(); ++i)
00176   {
00177     if (joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE ||
00178         joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
00179     {
00180       ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
00181       const std::vector<moveit_msgs::JointLimits>& jvec =
00182           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   lookupParam("max_solver_iterations", max_solver_iterations, 500);
00213   lookupParam("epsilon", epsilon, 1e-5);
00214   lookupParam("position_only_ik", position_ik, false);
00215   ROS_DEBUG_NAMED("kdl", "Looking for param name: position_only_ik");
00216 
00217   if (position_ik)
00218     ROS_INFO_NAMED("kdl", "Using position only ik");
00219 
00220   num_possible_redundant_joints_ =
00221       kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik ? 3 : 6);
00222 
00223   // Check for mimic joints
00224   std::vector<unsigned int> redundant_joints_map_index;
00225 
00226   std::vector<JointMimic> mimic_joints;
00227   unsigned int joint_counter = 0;
00228   for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
00229   {
00230     const robot_model::JointModel* jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
00231 
00232     // first check whether it belongs to the set of active joints in the group
00233     if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
00234     {
00235       JointMimic mimic_joint;
00236       mimic_joint.reset(joint_counter);
00237       mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
00238       mimic_joint.active = true;
00239       mimic_joints.push_back(mimic_joint);
00240       ++joint_counter;
00241       continue;
00242     }
00243     if (joint_model_group->hasJointModel(jm->getName()))
00244     {
00245       if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
00246       {
00247         JointMimic mimic_joint;
00248         mimic_joint.reset(joint_counter);
00249         mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
00250         mimic_joint.offset = jm->getMimicOffset();
00251         mimic_joint.multiplier = jm->getMimicFactor();
00252         mimic_joints.push_back(mimic_joint);
00253         continue;
00254       }
00255     }
00256   }
00257   for (std::size_t i = 0; i < mimic_joints.size(); ++i)
00258   {
00259     if (!mimic_joints[i].active)
00260     {
00261       const robot_model::JointModel* joint_model =
00262           joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
00263       for (std::size_t j = 0; j < mimic_joints.size(); ++j)
00264       {
00265         if (mimic_joints[j].joint_name == joint_model->getName())
00266         {
00267           mimic_joints[i].map_index = mimic_joints[j].map_index;
00268         }
00269       }
00270     }
00271   }
00272   mimic_joints_ = mimic_joints;
00273 
00274   // Setup the joint state groups that we need
00275   state_.reset(new robot_state::RobotState(robot_model_));
00276   state_2_.reset(new robot_state::RobotState(robot_model_));
00277 
00278   // Store things for when the set of redundant joints may change
00279   position_ik_ = position_ik;
00280   joint_model_group_ = joint_model_group;
00281   max_solver_iterations_ = max_solver_iterations;
00282   epsilon_ = epsilon;
00283 
00284   active_ = true;
00285   ROS_DEBUG_NAMED("kdl", "KDL solver initialized");
00286   return true;
00287 }
00288 
00289 bool KDLKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joints)
00290 {
00291   if (num_possible_redundant_joints_ < 0)
00292   {
00293     ROS_ERROR_NAMED("kdl", "This group cannot have redundant joints");
00294     return false;
00295   }
00296   if (static_cast<int>(redundant_joints.size()) > num_possible_redundant_joints_)
00297   {
00298     ROS_ERROR_NAMED("kdl", "This group can only have %d redundant joints", num_possible_redundant_joints_);
00299     return false;
00300   }
00301   /*
00302     XmlRpc::XmlRpcValue joint_list;
00303     if(private_handle.getParam(group_name+"/redundant_joints", joint_list))
00304     {
00305       ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00306       std::vector<std::string> redundant_joints;
00307       for (std::size_t i = 0; i < joint_list.size(); ++i)
00308       {
00309         ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
00310         redundant_joints.push_back(static_cast<std::string>(joint_list[i]));
00311         ROS_INFO_NAMED("kdl","Designated joint: %s as redundant joint", redundant_joints.back().c_str());
00312       }
00313     }
00314   */
00315   std::vector<unsigned int> redundant_joints_map_index;
00316   unsigned int counter = 0;
00317   for (std::size_t i = 0; i < dimension_; ++i)
00318   {
00319     bool is_redundant_joint = false;
00320     for (std::size_t j = 0; j < redundant_joints.size(); ++j)
00321     {
00322       if (i == redundant_joints[j])
00323       {
00324         is_redundant_joint = true;
00325         counter++;
00326         break;
00327       }
00328     }
00329     if (!is_redundant_joint)
00330     {
00331       // check for mimic
00332       if (mimic_joints_[i].active)
00333       {
00334         redundant_joints_map_index.push_back(counter);
00335         counter++;
00336       }
00337     }
00338   }
00339   for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
00340     ROS_DEBUG_NAMED("kdl", "Redundant joint map index: %d %d", (int)i, (int)redundant_joints_map_index[i]);
00341 
00342   redundant_joints_map_index_ = redundant_joints_map_index;
00343   redundant_joint_indices_ = redundant_joints;
00344   return true;
00345 }
00346 
00347 int KDLKinematicsPlugin::getJointIndex(const std::string& name) const
00348 {
00349   for (unsigned int i = 0; i < ik_chain_info_.joint_names.size(); i++)
00350   {
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   {
00362     if (kdl_chain_.getSegment(i).getName() == name)
00363     {
00364       return i + 1;
00365     }
00366     i++;
00367   }
00368   return -1;
00369 }
00370 
00371 bool KDLKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const
00372 {
00373   return ((ros::WallTime::now() - start_time).toSec() >= duration);
00374 }
00375 
00376 bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00377                                         std::vector<double>& solution, 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, ik_seed_state, default_timeout_, solution, solution_callback, error_code,
00384                           consistency_limits, options);
00385 }
00386 
00387 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00388                                            double timeout, std::vector<double>& solution,
00389                                            moveit_msgs::MoveItErrorCodes& error_code,
00390                                            const kinematics::KinematicsQueryOptions& options) const
00391 {
00392   const IKCallbackFn solution_callback = 0;
00393   std::vector<double> consistency_limits;
00394 
00395   return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
00396                           options);
00397 }
00398 
00399 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00400                                            double timeout, const std::vector<double>& consistency_limits,
00401                                            std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00402                                            const kinematics::KinematicsQueryOptions& options) const
00403 {
00404   const IKCallbackFn solution_callback = 0;
00405   return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
00406                           options);
00407 }
00408 
00409 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00410                                            double timeout, std::vector<double>& solution,
00411                                            const IKCallbackFn& solution_callback,
00412                                            moveit_msgs::MoveItErrorCodes& error_code,
00413                                            const kinematics::KinematicsQueryOptions& options) const
00414 {
00415   std::vector<double> consistency_limits;
00416   return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
00417                           options);
00418 }
00419 
00420 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00421                                            double timeout, const std::vector<double>& consistency_limits,
00422                                            std::vector<double>& solution, const IKCallbackFn& solution_callback,
00423                                            moveit_msgs::MoveItErrorCodes& error_code,
00424                                            const kinematics::KinematicsQueryOptions& options) const
00425 {
00426   return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
00427                           options);
00428 }
00429 
00430 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00431                                            double timeout, std::vector<double>& solution,
00432                                            const IKCallbackFn& solution_callback,
00433                                            moveit_msgs::MoveItErrorCodes& error_code,
00434                                            const std::vector<double>& consistency_limits,
00435                                            const kinematics::KinematicsQueryOptions& options) const
00436 {
00437   ros::WallTime n1 = ros::WallTime::now();
00438   if (!active_)
00439   {
00440     ROS_ERROR_NAMED("kdl", "kinematics not active");
00441     error_code.val = error_code.NO_IK_SOLUTION;
00442     return false;
00443   }
00444 
00445   if (ik_seed_state.size() != dimension_)
00446   {
00447     ROS_ERROR_STREAM_NAMED("kdl", "Seed state must have size " << dimension_ << " instead of size "
00448                                                                << ik_seed_state.size());
00449     error_code.val = error_code.NO_IK_SOLUTION;
00450     return false;
00451   }
00452 
00453   if (!consistency_limits.empty() && consistency_limits.size() != dimension_)
00454   {
00455     ROS_ERROR_STREAM_NAMED("kdl", "Consistency limits be empty or must have size " << dimension_ << " instead of size "
00456                                                                                    << consistency_limits.size());
00457     error_code.val = error_code.NO_IK_SOLUTION;
00458     return false;
00459   }
00460 
00461   KDL::JntArray jnt_seed_state(dimension_);
00462   KDL::JntArray jnt_pos_in(dimension_);
00463   KDL::JntArray jnt_pos_out(dimension_);
00464 
00465   KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
00466   KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(),
00467                                                  redundant_joint_indices_.size(), position_ik_);
00468   KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel,
00469                                                   max_solver_iterations_, epsilon_, position_ik_);
00470   ik_solver_vel.setMimicJoints(mimic_joints_);
00471   ik_solver_pos.setMimicJoints(mimic_joints_);
00472 
00473   if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
00474   {
00475     ROS_ERROR_NAMED("kdl", "Could not set redundant joints");
00476     return false;
00477   }
00478 
00479   if (options.lock_redundant_joints)
00480   {
00481     ik_solver_vel.lockRedundantJoints();
00482   }
00483 
00484   solution.resize(dimension_);
00485 
00486   KDL::Frame pose_desired;
00487   tf::poseMsgToKDL(ik_pose, pose_desired);
00488 
00489   ROS_DEBUG_STREAM_NAMED("kdl", "searchPositionIK2: Position request pose is "
00490                                     << ik_pose.position.x << " " << ik_pose.position.y << " " << ik_pose.position.z
00491                                     << " " << ik_pose.orientation.x << " " << ik_pose.orientation.y << " "
00492                                     << ik_pose.orientation.z << " " << ik_pose.orientation.w);
00493   // Do the IK
00494   for (unsigned int i = 0; i < dimension_; i++)
00495     jnt_seed_state(i) = ik_seed_state[i];
00496   jnt_pos_in = jnt_seed_state;
00497 
00498   unsigned int counter(0);
00499   while (1)
00500   {
00501     //    ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout:
00502     //    %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
00503     counter++;
00504     if (timedOut(n1, timeout))
00505     {
00506       ROS_DEBUG_NAMED("kdl", "IK timed out");
00507       error_code.val = error_code.TIMED_OUT;
00508       ik_solver_vel.unlockRedundantJoints();
00509       return false;
00510     }
00511     int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
00512     ROS_DEBUG_NAMED("kdl", "IK valid: %d", ik_valid);
00513     if (!consistency_limits.empty())
00514     {
00515       getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
00516       if ((ik_valid < 0 && !options.return_approximate_solution) ||
00517           !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
00518       {
00519         ROS_DEBUG_NAMED("kdl", "Could not find IK solution: does not match consistency limits");
00520         continue;
00521       }
00522     }
00523     else
00524     {
00525       getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
00526       ROS_DEBUG_NAMED("kdl", "New random configuration");
00527       for (unsigned int j = 0; j < dimension_; j++)
00528         ROS_DEBUG_NAMED("kdl", "%d %f", j, jnt_pos_in(j));
00529 
00530       if (ik_valid < 0 && !options.return_approximate_solution)
00531       {
00532         ROS_DEBUG_NAMED("kdl", "Could not find IK solution");
00533         continue;
00534       }
00535     }
00536     ROS_DEBUG_NAMED("kdl", "Found IK solution");
00537     for (unsigned int j = 0; j < dimension_; j++)
00538       solution[j] = jnt_pos_out(j);
00539     if (!solution_callback.empty())
00540       solution_callback(ik_pose, solution, error_code);
00541     else
00542       error_code.val = error_code.SUCCESS;
00543 
00544     if (error_code.val == error_code.SUCCESS)
00545     {
00546       ROS_DEBUG_STREAM_NAMED("kdl", "Solved after " << counter << " iterations");
00547       ik_solver_vel.unlockRedundantJoints();
00548       return true;
00549     }
00550   }
00551   ROS_DEBUG_NAMED("kdl", "An IK that satisifes the constraints and is collision free could not be found");
00552   error_code.val = error_code.NO_IK_SOLUTION;
00553   ik_solver_vel.unlockRedundantJoints();
00554   return false;
00555 }
00556 
00557 bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
00558                                         const std::vector<double>& joint_angles,
00559                                         std::vector<geometry_msgs::Pose>& poses) const
00560 {
00561   ros::WallTime n1 = ros::WallTime::now();
00562   if (!active_)
00563   {
00564     ROS_ERROR_NAMED("kdl", "kinematics not active");
00565     return false;
00566   }
00567   poses.resize(link_names.size());
00568   if (joint_angles.size() != dimension_)
00569   {
00570     ROS_ERROR_NAMED("kdl", "Joint angles vector must have size: %d", dimension_);
00571     return false;
00572   }
00573 
00574   KDL::Frame p_out;
00575   geometry_msgs::PoseStamped pose;
00576   tf::Stamped<tf::Pose> tf_pose;
00577 
00578   KDL::JntArray jnt_pos_in(dimension_);
00579   for (unsigned int i = 0; i < dimension_; i++)
00580   {
00581     jnt_pos_in(i) = joint_angles[i];
00582   }
00583 
00584   KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
00585 
00586   bool valid = true;
00587   for (unsigned int i = 0; i < poses.size(); i++)
00588   {
00589     ROS_DEBUG_NAMED("kdl", "End effector index: %d", getKDLSegmentIndex(link_names[i]));
00590     if (fk_solver.JntToCart(jnt_pos_in, p_out, getKDLSegmentIndex(link_names[i])) >= 0)
00591     {
00592       tf::poseKDLToMsg(p_out, poses[i]);
00593     }
00594     else
00595     {
00596       ROS_ERROR_NAMED("kdl", "Could not compute FK for %s", link_names[i].c_str());
00597       valid = false;
00598     }
00599   }
00600   return valid;
00601 }
00602 
00603 const std::vector<std::string>& KDLKinematicsPlugin::getJointNames() const
00604 {
00605   return ik_chain_info_.joint_names;
00606 }
00607 
00608 const std::vector<std::string>& KDLKinematicsPlugin::getLinkNames() const
00609 {
00610   return ik_chain_info_.link_names;
00611 }
00612 
00613 }  // namespace


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Mon Apr 23 2018 10:25:23