lma_kinematics_plugin.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2016, CRI group, NTU, Singapore
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 CRI group 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: Francisco Suarez-Ruiz */
00036 
00037 #include <moveit/lma_kinematics_plugin/lma_kinematics_plugin.h>
00038 #include <class_loader/class_loader.h>
00039 
00040 #include <tf_conversions/tf_kdl.h>
00041 #include <kdl_parser/kdl_parser.hpp>
00042 
00043 // URDF, SRDF
00044 #include <urdf_model/model.h>
00045 #include <srdfdom/model.h>
00046 
00047 #include <moveit/rdf_loader/rdf_loader.h>
00048 
00049 // register KDLKinematics as a KinematicsBase implementation
00050 CLASS_LOADER_REGISTER_CLASS(lma_kinematics_plugin::LMAKinematicsPlugin, kinematics::KinematicsBase)
00051 
00052 namespace lma_kinematics_plugin
00053 {
00054 LMAKinematicsPlugin::LMAKinematicsPlugin() : active_(false)
00055 {
00056 }
00057 
00058 void LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::getRandomConfiguration(const KDL::JntArray& seed_state,
00081                                                  const std::vector<double>& consistency_limits,
00082                                                  KDL::JntArray& jnt_array, bool lock_redundancy) const
00083 {
00084   std::vector<double> values(dimension_, 0.0);
00085   std::vector<double> near(dimension_, 0.0);
00086   for (std::size_t i = 0; i < dimension_; ++i)
00087     near[i] = seed_state(i);
00088 
00089   // Need to resize the consistency limits to remove mimic joints
00090   std::vector<double> consistency_limits_mimic;
00091   for (std::size_t i = 0; i < dimension_; ++i)
00092   {
00093     if (!mimic_joints_[i].active)
00094       continue;
00095     consistency_limits_mimic.push_back(consistency_limits[i]);
00096   }
00097 
00098   joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near,
00099                                                        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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
00128                                      const std::string& base_frame, const std::string& tip_frame,
00129                                      double search_discretization)
00130 {
00131   setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
00132 
00133   rdf_loader::RDFLoader rdf_loader(robot_description_);
00134   const boost::shared_ptr<srdf::Model>& srdf = rdf_loader.getSRDF();
00135   const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
00136 
00137   if (!urdf_model || !srdf)
00138   {
00139     ROS_ERROR_NAMED("lma", "URDF and SRDF must be loaded for KDL kinematics solver to work.");
00140     return false;
00141   }
00142 
00143   robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
00144 
00145   robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
00146   if (!joint_model_group)
00147     return false;
00148 
00149   if (!joint_model_group->isChain())
00150   {
00151     ROS_ERROR_NAMED("lma", "Group '%s' is not a chain", group_name.c_str());
00152     return false;
00153   }
00154   if (!joint_model_group->isSingleDOFJoints())
00155   {
00156     ROS_ERROR_NAMED("lma", "Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
00157     return false;
00158   }
00159 
00160   KDL::Tree kdl_tree;
00161 
00162   if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
00163   {
00164     ROS_ERROR_NAMED("lma", "Could not initialize tree object");
00165     return false;
00166   }
00167   if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
00168   {
00169     ROS_ERROR_NAMED("lma", "Could not initialize chain object");
00170     return false;
00171   }
00172 
00173   dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
00174   for (std::size_t i = 0; i < joint_model_group->getJointModels().size(); ++i)
00175   {
00176     if (joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE ||
00177         joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
00178     {
00179       ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
00180       const std::vector<moveit_msgs::JointLimits>& jvec =
00181           joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
00182       ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
00183     }
00184   }
00185 
00186   fk_chain_info_.joint_names = ik_chain_info_.joint_names;
00187   fk_chain_info_.limits = ik_chain_info_.limits;
00188 
00189   if (!joint_model_group->hasLinkModel(getTipFrame()))
00190   {
00191     ROS_ERROR_NAMED("lma", "Could not find tip name in joint group '%s'", group_name.c_str());
00192     return false;
00193   }
00194   ik_chain_info_.link_names.push_back(getTipFrame());
00195   fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
00196 
00197   joint_min_.resize(ik_chain_info_.limits.size());
00198   joint_max_.resize(ik_chain_info_.limits.size());
00199 
00200   for (unsigned int i = 0; i < ik_chain_info_.limits.size(); i++)
00201   {
00202     joint_min_(i) = ik_chain_info_.limits[i].min_position;
00203     joint_max_(i) = ik_chain_info_.limits[i].max_position;
00204   }
00205 
00206   // Get Solver Parameters
00207   int max_solver_iterations;
00208   double epsilon;
00209   bool position_ik;
00210 
00211   lookupParam("max_solver_iterations", max_solver_iterations, 500);
00212   lookupParam("epsilon", epsilon, 1e-5);
00213   lookupParam("position_only_ik", position_ik, false);
00214   ROS_DEBUG_NAMED("lma", "Looking for param name: position_only_ik");
00215 
00216   if (position_ik)
00217     ROS_INFO_NAMED("lma", "Using position only ik");
00218 
00219   num_possible_redundant_joints_ =
00220       kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik ? 3 : 6);
00221 
00222   // Check for mimic joints
00223   bool has_mimic_joints = joint_model_group->getMimicJointModels().size() > 0;
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("lma", "KDL solver initialized");
00286   return true;
00287 }
00288 
00289 bool LMAKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joints)
00290 {
00291   if (num_possible_redundant_joints_ < 0)
00292   {
00293     ROS_ERROR_NAMED("lma", "This group cannot have redundant joints");
00294     return false;
00295   }
00296   if (redundant_joints.size() > num_possible_redundant_joints_)
00297   {
00298     ROS_ERROR_NAMED("lma", "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("lma","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("lma", "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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const
00372 {
00373   return ((ros::WallTime::now() - start_time).toSec() >= duration);
00374 }
00375 
00376 bool LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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("lma", "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("lma", "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("lma", "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   // Build Solvers
00466   Eigen::Matrix<double, 6, 1> L;
00467   L(0) = 1;
00468   L(1) = 1;
00469   L(2) = 1;
00470   L(3) = 0.01;
00471   L(4) = 0.01;
00472   L(5) = 0.01;
00473 
00474   KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
00475   KDL::ChainIkSolverPos_LMA ik_solver(kdl_chain_, L, epsilon_, max_solver_iterations_);
00476   KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(),
00477                                                  redundant_joint_indices_.size(), position_ik_);
00478   KDL::ChainIkSolverPos_LMA_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver,
00479                                                    max_solver_iterations_, epsilon_, position_ik_);
00480   ik_solver_vel.setMimicJoints(mimic_joints_);
00481   ik_solver_pos.setMimicJoints(mimic_joints_);
00482 
00483   if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
00484   {
00485     ROS_ERROR_NAMED("lma", "Could not set redundant joints");
00486     return false;
00487   }
00488 
00489   if (options.lock_redundant_joints)
00490   {
00491     ik_solver_vel.lockRedundantJoints();
00492   }
00493 
00494   solution.resize(dimension_);
00495 
00496   KDL::Frame pose_desired;
00497   tf::poseMsgToKDL(ik_pose, pose_desired);
00498 
00499   ROS_DEBUG_STREAM_NAMED("lma", "searchPositionIK2: Position request pose is "
00500                                     << ik_pose.position.x << " " << ik_pose.position.y << " " << ik_pose.position.z
00501                                     << " " << ik_pose.orientation.x << " " << ik_pose.orientation.y << " "
00502                                     << ik_pose.orientation.z << " " << ik_pose.orientation.w);
00503   // Do the IK
00504   for (unsigned int i = 0; i < dimension_; i++)
00505     jnt_seed_state(i) = ik_seed_state[i];
00506   jnt_pos_in = jnt_seed_state;
00507 
00508   unsigned int counter(0);
00509   while (1)
00510   {
00511     //    ROS_DEBUG_NAMED("lma","Iteration: %d, time: %f, Timeout:
00512     //    %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
00513     counter++;
00514     if (timedOut(n1, timeout))
00515     {
00516       ROS_DEBUG_NAMED("lma", "IK timed out");
00517       error_code.val = error_code.TIMED_OUT;
00518       ik_solver_vel.unlockRedundantJoints();
00519       return false;
00520     }
00521     int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
00522     ROS_DEBUG_NAMED("lma", "IK valid: %d", ik_valid);
00523     if (!consistency_limits.empty())
00524     {
00525       getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
00526       if ((ik_valid < 0 && !options.return_approximate_solution) ||
00527           !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
00528       {
00529         ROS_DEBUG_NAMED("lma", "Could not find IK solution: does not match consistency limits");
00530         continue;
00531       }
00532     }
00533     else
00534     {
00535       getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
00536       ROS_DEBUG_NAMED("lma", "New random configuration");
00537       for (unsigned int j = 0; j < dimension_; j++)
00538         ROS_DEBUG_NAMED("lma", "%d %f", j, jnt_pos_in(j));
00539 
00540       if (ik_valid < 0 && !options.return_approximate_solution)
00541       {
00542         ROS_DEBUG_NAMED("lma", "Could not find IK solution");
00543         continue;
00544       }
00545     }
00546     ROS_DEBUG_NAMED("lma", "Found IK solution");
00547     for (unsigned int j = 0; j < dimension_; j++)
00548       solution[j] = jnt_pos_out(j);
00549     if (!solution_callback.empty())
00550       solution_callback(ik_pose, solution, error_code);
00551     else
00552       error_code.val = error_code.SUCCESS;
00553 
00554     if (error_code.val == error_code.SUCCESS)
00555     {
00556       ROS_DEBUG_STREAM_NAMED("lma", "Solved after " << counter << " iterations");
00557       ik_solver_vel.unlockRedundantJoints();
00558       return true;
00559     }
00560   }
00561   ROS_DEBUG_NAMED("lma", "An IK that satisifes the constraints and is collision free could not be found");
00562   error_code.val = error_code.NO_IK_SOLUTION;
00563   ik_solver_vel.unlockRedundantJoints();
00564   return false;
00565 }
00566 
00567 bool LMAKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
00568                                         const std::vector<double>& joint_angles,
00569                                         std::vector<geometry_msgs::Pose>& poses) const
00570 {
00571   ros::WallTime n1 = ros::WallTime::now();
00572   if (!active_)
00573   {
00574     ROS_ERROR_NAMED("lma", "kinematics not active");
00575     return false;
00576   }
00577   poses.resize(link_names.size());
00578   if (joint_angles.size() != dimension_)
00579   {
00580     ROS_ERROR_NAMED("lma", "Joint angles vector must have size: %d", dimension_);
00581     return false;
00582   }
00583 
00584   KDL::Frame p_out;
00585   geometry_msgs::PoseStamped pose;
00586   tf::Stamped<tf::Pose> tf_pose;
00587 
00588   KDL::JntArray jnt_pos_in(dimension_);
00589   for (unsigned int i = 0; i < dimension_; i++)
00590   {
00591     jnt_pos_in(i) = joint_angles[i];
00592   }
00593 
00594   KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
00595 
00596   bool valid = true;
00597   for (unsigned int i = 0; i < poses.size(); i++)
00598   {
00599     ROS_DEBUG_NAMED("lma", "End effector index: %d", getKDLSegmentIndex(link_names[i]));
00600     if (fk_solver.JntToCart(jnt_pos_in, p_out, getKDLSegmentIndex(link_names[i])) >= 0)
00601     {
00602       tf::poseKDLToMsg(p_out, poses[i]);
00603     }
00604     else
00605     {
00606       ROS_ERROR_NAMED("lma", "Could not compute FK for %s", link_names[i].c_str());
00607       valid = false;
00608     }
00609   }
00610   return valid;
00611 }
00612 
00613 const std::vector<std::string>& LMAKinematicsPlugin::getJointNames() const
00614 {
00615   return ik_chain_info_.joint_names;
00616 }
00617 
00618 const std::vector<std::string>& LMAKinematicsPlugin::getLinkNames() const
00619 {
00620   return ik_chain_info_.link_names;
00621 }
00622 
00623 }  // namespace


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