00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h>
00038 #include <class_loader/class_loader.h>
00039
00040
00041 #include <tf_conversions/tf_kdl.h>
00042 #include <kdl_parser/kdl_parser.hpp>
00043
00044
00045 #include <urdf_model/model.h>
00046 #include <srdfdom/model.h>
00047
00048 #include <moveit/rdf_loader/rdf_loader.h>
00049
00050
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
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
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
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
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
00276 state_.reset(new robot_state::RobotState(robot_model_));
00277 state_2_.reset(new robot_state::RobotState(robot_model_));
00278
00279
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
00304
00305
00306
00307
00308
00309
00310
00311
00312
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
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
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
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 }