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/robot_model/robot_model.h>
00038 #include <geometric_shapes/shape_operations.h>
00039 #include <boost/math/constants/constants.hpp>
00040 #include <moveit/profiler/profiler.h>
00041 #include <algorithm>
00042 #include <limits>
00043 #include <queue>
00044 #include <cmath>
00045
00046
00047
00048 robot_model::RobotModel::RobotModel(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
00049 const boost::shared_ptr<const srdf::Model> &srdf_model)
00050 {
00051 root_joint_ = NULL;
00052 urdf_ = urdf_model;
00053 srdf_ = srdf_model;
00054 buildModel(*urdf_model, *srdf_model);
00055 }
00056
00057 robot_model::RobotModel::~RobotModel()
00058 {
00059 for (std::map<std::string, JointModelGroup*>::iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end() ; ++it)
00060 delete it->second;
00061 for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00062 delete joint_model_vector_[i];
00063 for (std::size_t i = 0 ; i < link_model_vector_.size() ; ++i)
00064 delete link_model_vector_[i];
00065 }
00066
00067 void robot_model::RobotModel::buildModel(const urdf::ModelInterface &urdf_model, const srdf::Model &srdf_model)
00068 {
00069 moveit::Profiler::ScopedStart prof_start;
00070 moveit::Profiler::ScopedBlock prof_block("RobotModel::buildModel");
00071
00072 root_joint_ = NULL;
00073 model_name_ = urdf_model.getName();
00074 if (urdf_model.getRoot())
00075 {
00076 const urdf::Link *root_link_ptr = urdf_model.getRoot().get();
00077 model_frame_ = '/' + root_link_ptr->name;
00078
00079 root_joint_ = buildRecursive(NULL, root_link_ptr, srdf_model);
00080 root_link_ = root_joint_->child_link_model_;
00081 buildMimic(urdf_model);
00082 buildJointInfo();
00083
00084 if (link_models_with_collision_geometry_vector_.empty())
00085 logWarn("No geometry is associated to any robot links");
00086
00087
00088 buildGroups(srdf_model);
00089 buildGroupStates(srdf_model);
00090
00091 std::stringstream ss;
00092 printModelInfo(ss);
00093 logDebug("%s", ss.str().c_str());
00094 }
00095 else
00096 logWarn("No root link found");
00097 }
00098
00099 void robot_model::RobotModel::buildJointInfo()
00100 {
00101
00102 variable_count_ = 0;
00103 std::vector<JointModel*> later;
00104 for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00105 {
00106 const std::vector<std::string> &name_order = joint_model_vector_[i]->getVariableNames();
00107 for (std::size_t j = 0 ; j < name_order.size() ; ++j)
00108 joint_model_vector_[i]->getVariableBounds(name_order[j], variable_bounds_[name_order[j]]);
00109 if (joint_model_vector_[i]->mimic_ == NULL)
00110 {
00111
00112 if (name_order.size() > 0)
00113 {
00114 for (std::size_t j = 0; j < name_order.size(); ++j)
00115 {
00116 joint_variables_index_map_[name_order[j]] = variable_count_ + j;
00117 active_variable_names_.push_back(name_order[j]);
00118 }
00119 joint_variables_index_map_[joint_model_vector_[i]->getName()] = variable_count_;
00120
00121
00122 variable_count_ += joint_model_vector_[i]->getVariableCount();
00123 }
00124 }
00125 else
00126 later.push_back(joint_model_vector_[i]);
00127 }
00128
00129 for (std::size_t i = 0 ; i < later.size() ; ++i)
00130 {
00131 const std::vector<std::string>& name_order = later[i]->getVariableNames();
00132 const std::vector<std::string>& mim_name_order = later[i]->mimic_->getVariableNames();
00133 for (std::size_t j = 0; j < name_order.size(); ++j)
00134 joint_variables_index_map_[name_order[j]] = joint_variables_index_map_[mim_name_order[j]];
00135 joint_variables_index_map_[later[i]->getName()] = joint_variables_index_map_[later[i]->mimic_->getName()];
00136 }
00137
00138 for (std::size_t i = 0 ; i < link_model_vector_.size() ; ++i)
00139 {
00140 LinkModelToAffine3dMap associated_transforms;
00141 computeFixedTransforms(link_model_vector_[i], Eigen::Affine3d::Identity(), associated_transforms);
00142 if (associated_transforms.size() > 1)
00143 {
00144 for (LinkModelToAffine3dMap::iterator it = associated_transforms.begin() ; it != associated_transforms.end() ; ++it)
00145 {
00146 it->first->associated_fixed_transforms_[link_model_vector_[i]] = it->second.inverse();
00147 link_model_vector_[i]->associated_fixed_transforms_[it->first] = it->second;
00148 }
00149 }
00150 }
00151 }
00152
00153 void robot_model::RobotModel::buildGroupStates(const srdf::Model &srdf_model)
00154 {
00155
00156 const std::vector<srdf::Model::GroupState> &ds = srdf_model.getGroupStates();
00157 for (std::size_t i = 0 ; i < ds.size() ; ++i)
00158 {
00159 std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.find(ds[i].group_);
00160 if (it != joint_model_group_map_.end())
00161 for (std::map<std::string, std::vector<double> >::const_iterator jt = ds[i].joint_values_.begin() ; jt != ds[i].joint_values_.end() ; ++jt)
00162 {
00163 const JointModel* jm = it->second->getJointModel(jt->first);
00164 if (jm)
00165 {
00166 const std::vector<std::string> &vn = jm->getVariableNames();
00167 if (vn.size() == jt->second.size())
00168 for (std::size_t j = 0 ; j < vn.size() ; ++j)
00169 it->second->default_states_[ds[i].name_][vn[j]] = jt->second[j];
00170 else
00171 logError("The model for joint '%s' requires %d variable values, but only %d variable values were supplied in default state '%s' for group '%s'",
00172 jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(), it->first.c_str());
00173 }
00174 else
00175 logError("Group state '%s' specifies value for joint '%s', but that joint is not part of group '%s'", ds[i].name_.c_str(),
00176 jt->first.c_str(), it->first.c_str());
00177 }
00178 else
00179 logError("Group state '%s' specified for group '%s', but that group does not exist", ds[i].name_.c_str(), ds[i].group_.c_str());
00180 }
00181 }
00182
00183 void robot_model::RobotModel::buildMimic(const urdf::ModelInterface &urdf_model)
00184 {
00185
00186 for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00187 {
00188 const urdf::Joint *jm = urdf_model.getJoint(joint_model_vector_[i]->getName()).get();
00189 if (jm)
00190 if (jm->mimic)
00191 {
00192 joint_model_vector_[i]->mimic_offset_ = jm->mimic->offset;
00193 joint_model_vector_[i]->mimic_factor_ = jm->mimic->multiplier;
00194 std::map<std::string, JointModel*>::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
00195 if (jit != joint_model_map_.end())
00196 {
00197 if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount())
00198 joint_model_vector_[i]->mimic_ = jit->second;
00199 else
00200 logError("Join '%s' cannot mimic joint '%s' because they have different number of DOF",
00201 joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
00202 }
00203 else
00204 logError("Joint '%s' cannot mimic unknown joint '%s'", joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
00205 }
00206 }
00207
00208 bool change = true;
00209 while (change)
00210 {
00211 change = false;
00212 for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00213 if (joint_model_vector_[i]->mimic_)
00214 if (joint_model_vector_[i]->mimic_->mimic_)
00215 {
00216 joint_model_vector_[i]->mimic_ = joint_model_vector_[i]->mimic_->mimic_;
00217 joint_model_vector_[i]->mimic_offset_ += joint_model_vector_[i]->mimic_factor_ * joint_model_vector_[i]->mimic_->mimic_offset_;
00218 joint_model_vector_[i]->mimic_factor_ *= joint_model_vector_[i]->mimic_->mimic_factor_;
00219 change = true;
00220 }
00221 }
00222
00223 for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00224 if (joint_model_vector_[i]->mimic_ == joint_model_vector_[i])
00225 {
00226 joint_model_vector_[i]->mimic_ = NULL;
00227 logError("Joint '%s' mimicks itself. This is not allowed.", joint_model_vector_[i]->getName().c_str());
00228 }
00229 else
00230 if (joint_model_vector_[i]->mimic_)
00231 joint_model_vector_[i]->mimic_->mimic_requests_.push_back(joint_model_vector_[i]);
00232 }
00233
00234 bool robot_model::RobotModel::hasEndEffector(const std::string& eef) const
00235 {
00236 return end_effectors_.find(eef) != end_effectors_.end();
00237 }
00238
00239 const robot_model::JointModelGroup* robot_model::RobotModel::getEndEffector(const std::string& name) const
00240 {
00241 std::map<std::string, JointModelGroup*>::const_iterator it = end_effectors_.find(name);
00242 if (it == end_effectors_.end())
00243 {
00244 it = joint_model_group_map_.find(name);
00245 if (it != joint_model_group_map_.end() && it->second->isEndEffector())
00246 return it->second;
00247 logError("End-effector '%s' not found in model %s", name.c_str(), model_name_.c_str());
00248 return NULL;
00249 }
00250 return it->second;
00251 }
00252
00253 robot_model::JointModelGroup* robot_model::RobotModel::getEndEffector(const std::string& name)
00254 {
00255 std::map<std::string, JointModelGroup*>::const_iterator it = end_effectors_.find(name);
00256 if (it == end_effectors_.end())
00257 {
00258 it = joint_model_group_map_.find(name);
00259 if (it != joint_model_group_map_.end() && it->second->isEndEffector())
00260 return it->second;
00261 logError("End-effector '%s' not found in model %s", name.c_str(), model_name_.c_str());
00262 return NULL;
00263 }
00264 return it->second;
00265 }
00266
00267 bool robot_model::RobotModel::hasJointModelGroup(const std::string &name) const
00268 {
00269 return joint_model_group_map_.find(name) != joint_model_group_map_.end();
00270 }
00271
00272 const robot_model::JointModelGroup* robot_model::RobotModel::getJointModelGroup(const std::string& name) const
00273 {
00274 std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.find(name);
00275 if (it == joint_model_group_map_.end())
00276 {
00277 logError("Group '%s' not found in model %s", name.c_str(), model_name_.c_str());
00278 return NULL;
00279 }
00280 return it->second;
00281 }
00282
00283 robot_model::JointModelGroup* robot_model::RobotModel::getJointModelGroup(const std::string& name)
00284 {
00285 std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.find(name);
00286 if (it == joint_model_group_map_.end())
00287 {
00288 logError("Group '%s' not found in model %s", name.c_str(), model_name_.c_str());
00289 return NULL;
00290 }
00291 return it->second;
00292 }
00293
00294 void robot_model::RobotModel::buildGroups(const srdf::Model &srdf_model)
00295 {
00296 const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
00297
00298
00299 std::vector<bool> processed(group_configs.size(), false);
00300
00301 bool added = true;
00302 while (added)
00303 {
00304 added = false;
00305
00306
00307 for(unsigned int i = 0 ; i < group_configs.size() ; ++i)
00308 if (!processed[i])
00309 {
00310
00311 bool all_subgroups_added = true;
00312 for(unsigned int j = 0; j < group_configs[i].subgroups_.size(); ++j)
00313 if (joint_model_group_map_.find(group_configs[i].subgroups_[j]) == joint_model_group_map_.end())
00314 {
00315 all_subgroups_added = false;
00316 break;
00317 }
00318 if (all_subgroups_added)
00319 {
00320 added = true;
00321 processed[i] = true;
00322 if (!addJointModelGroup(group_configs[i]))
00323 logWarn("Failed to add group '%s'", group_configs[i].name_.c_str());
00324 }
00325 }
00326 }
00327
00328 for (unsigned int i = 0 ; i < processed.size() ; ++i)
00329 if (!processed[i])
00330 logWarn("Could not process group '%s' due to unmet subgroup dependencies", group_configs[i].name_.c_str());
00331
00332 buildGroupsInfo_Subgroups(srdf_model);
00333 buildGroupsInfo_EndEffectors(srdf_model);
00334 }
00335
00336 void robot_model::RobotModel::buildGroupsInfo_Subgroups(const srdf::Model &srdf_model)
00337 {
00338
00339 for (std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end(); ++it)
00340 {
00341 JointModelGroup *jmg = it->second;
00342 jmg->subgroup_names_.clear();
00343 std::set<const JointModel*> joints(jmg->getJointModels().begin(), jmg->getJointModels().end());
00344 for (std::map<std::string, JointModelGroup*>::const_iterator jt = joint_model_group_map_.begin() ; jt != joint_model_group_map_.end(); ++jt)
00345 if (jt->first != it->first)
00346 {
00347 bool ok = true;
00348 JointModelGroup *sub_jmg = jt->second;
00349 const std::vector<const JointModel*> &sub_joints = sub_jmg->getJointModels();
00350 for (std::size_t k = 0 ; k < sub_joints.size() ; ++k)
00351 if (joints.find(sub_joints[k]) == joints.end())
00352 {
00353 ok = false;
00354 break;
00355 }
00356 if (ok)
00357 jmg->subgroup_names_.push_back(sub_jmg->getName());
00358 }
00359 }
00360 }
00361
00362 void robot_model::RobotModel::buildGroupsInfo_EndEffectors(const srdf::Model &srdf_model)
00363 {
00364
00365 const std::vector<srdf::Model::EndEffector> &eefs = srdf_model.getEndEffectors();
00366 for (std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end(); ++it)
00367 {
00368
00369 for (std::size_t k = 0 ; k < eefs.size() ; ++k)
00370 if (eefs[k].component_group_ == it->first)
00371 {
00372
00373 it->second->is_end_effector_ = true;
00374 it->second->end_effector_name_ = eefs[k].name_;
00375 end_effectors_[eefs[k].name_] = it->second;
00376
00377 JointModelGroup *eef_parent_group = NULL;
00378
00379
00380 if (!eefs[k].parent_group_.empty())
00381 {
00382 std::map<std::string, JointModelGroup*>::const_iterator jt = joint_model_group_map_.find(eefs[k].parent_group_);
00383 if (jt != joint_model_group_map_.end())
00384 {
00385 if (jt->second->hasLinkModel(eefs[k].parent_link_))
00386 {
00387 if (jt->second != it->second)
00388 eef_parent_group = jt->second;
00389 else
00390 logError("Group '%s' for end-effector '%s' cannot be its own parent", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
00391 }
00392 else
00393 logError("Group '%s' was specified as parent group for end-effector '%s' but it does not include the parent link '%s'",
00394 eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str());
00395 }
00396 else
00397 logError("Group name '%s' not found (specified as parent group for end-effector '%s')",
00398 eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
00399 }
00400
00401 if (eef_parent_group == NULL)
00402 {
00403
00404
00405 std::vector<JointModelGroup*> possible_parent_groups;
00406 for (std::map<std::string, JointModelGroup*>::const_iterator jt = joint_model_group_map_.begin() ; jt != joint_model_group_map_.end(); ++jt)
00407 if (jt->first != it->first)
00408 {
00409 if (jt->second->hasLinkModel(eefs[k].parent_link_))
00410 possible_parent_groups.push_back(jt->second);
00411 }
00412 if (!possible_parent_groups.empty())
00413 {
00414
00415
00416 std::size_t best = 0;
00417 for (std::size_t g = 1 ; g < possible_parent_groups.size() ; ++g)
00418 if (possible_parent_groups[g]->getJointModels().size() < possible_parent_groups[best]->getJointModels().size())
00419 best = g;
00420 eef_parent_group = possible_parent_groups[best];
00421 }
00422 }
00423 if (eef_parent_group)
00424 {
00425 eef_parent_group->attached_end_effector_names_.push_back(eefs[k].name_);
00426 it->second->end_effector_parent_.first = eef_parent_group->getName();
00427 }
00428 else
00429 logWarn("Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str());
00430 it->second->end_effector_parent_.second = eefs[k].parent_link_;
00431 break;
00432 }
00433 }
00434 }
00435
00436 bool robot_model::RobotModel::addJointModelGroup(const srdf::Model::Group& gc)
00437 {
00438 if (joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end())
00439 {
00440 logWarn("A group named '%s' already exists. Not adding.", gc.name_.c_str());
00441 return false;
00442 }
00443
00444 std::set<const JointModel*> jset;
00445
00446
00447 for (std::size_t i = 0 ; i < gc.chains_.size() ; ++i)
00448 {
00449 const LinkModel* base_link = getLinkModel(gc.chains_[i].first);
00450 const LinkModel* tip_link = getLinkModel(gc.chains_[i].second);
00451 if (base_link && tip_link)
00452 {
00453
00454 const LinkModel* lm = tip_link;
00455 std::vector<const JointModel*> cj;
00456 while (lm)
00457 {
00458 if (lm == base_link)
00459 break;
00460 cj.push_back(lm->getParentJointModel());
00461 lm = lm->getParentJointModel()->getParentLinkModel();
00462 }
00463
00464
00465 if (lm != base_link)
00466 {
00467
00468 lm = base_link;
00469 std::size_t index = 0;
00470 std::vector<const JointModel*> cj2;
00471 while (lm)
00472 {
00473 for (std::size_t j = 0 ; j < cj.size() ; ++j)
00474 if (cj[j] == lm->getParentJointModel())
00475 {
00476 index = j + 1;
00477 break;
00478 }
00479 if (index > 0)
00480 break;
00481 cj2.push_back(lm->getParentJointModel());
00482 lm = lm->getParentJointModel()->getParentLinkModel();
00483 }
00484 if (index > 0)
00485 {
00486 jset.insert(cj.begin(), cj.begin() + index);
00487 jset.insert(cj2.begin(), cj2.end());
00488 }
00489 }
00490 else
00491
00492 jset.insert(cj.begin(), cj.end());
00493 }
00494 }
00495
00496
00497 for (std::size_t i = 0 ; i < gc.joints_.size() ; ++i)
00498 {
00499 const JointModel *j = getJointModel(gc.joints_[i]);
00500 if (j)
00501 jset.insert(j);
00502 }
00503
00504
00505 for (std::size_t i = 0 ; i < gc.links_.size() ; ++i)
00506 {
00507 const LinkModel *l = getLinkModel(gc.links_[i]);
00508 if (l)
00509 jset.insert(l->getParentJointModel());
00510 }
00511
00512
00513 for (std::size_t i = 0 ; i < gc.subgroups_.size() ; ++i)
00514 {
00515 const JointModelGroup *sg = getJointModelGroup(gc.subgroups_[i]);
00516 if (sg)
00517 {
00518
00519 const std::vector<const JointModel*> &js = sg->getJointModels();
00520 for (std::size_t j = 0 ; j < js.size() ; ++j)
00521 jset.insert(js[j]);
00522
00523
00524 const std::vector<const JointModel*> &fs = sg->getFixedJointModels();
00525 for (std::size_t j = 0 ; j < fs.size() ; ++j)
00526 jset.insert(fs[j]);
00527
00528
00529 const std::vector<const JointModel*> &ms = sg->getMimicJointModels();
00530 for (std::size_t j = 0 ; j < ms.size() ; ++j)
00531 jset.insert(ms[j]);
00532 }
00533 }
00534
00535 if (jset.empty())
00536 {
00537 logWarn("Group '%s' must have at least one valid joint", gc.name_.c_str());
00538 return false;
00539 }
00540
00541 std::vector<const JointModel*> joints;
00542 for (std::set<const JointModel*>::iterator it = jset.begin() ; it != jset.end() ; ++it)
00543 joints.push_back(*it);
00544
00545 JointModelGroup *jmg = new JointModelGroup(gc.name_, joints, this);
00546 joint_model_group_map_[gc.name_] = jmg;
00547 joint_model_group_config_map_[gc.name_] = gc;
00548 joint_model_group_names_.push_back(gc.name_);
00549
00550
00551
00552 if (gc.chains_.size() == 1 && gc.joints_.empty() && gc.links_.empty() && gc.subgroups_.empty())
00553 jmg->is_chain_ = true;
00554
00555 return true;
00556 }
00557
00558 robot_model::JointModel* robot_model::RobotModel::buildRecursive(LinkModel *parent, const urdf::Link *urdf_link,
00559 const srdf::Model &srdf_model)
00560 {
00561
00562 JointModel *joint = urdf_link->parent_joint ?
00563 constructJointModel(urdf_link->parent_joint.get(), urdf_link, srdf_model) :
00564 constructJointModel(NULL, urdf_link, srdf_model);
00565 if (joint == NULL)
00566 return NULL;
00567
00568
00569 joint_model_map_[joint->name_] = joint;
00570 joint->tree_index_ = joint_model_vector_.size();
00571 joint_model_vector_.push_back(joint);
00572 joint_model_vector_const_.push_back(joint);
00573 joint_model_names_vector_.push_back(joint->getName());
00574 if (joint->getType() == JointModel::REVOLUTE && static_cast<const RevoluteJointModel*>(joint)->isContinuous())
00575 continuous_joint_model_vector_const_.push_back(joint);
00576 joint->parent_link_model_ = parent;
00577
00578
00579 LinkModel *link = constructLinkModel(urdf_link);
00580 joint->child_link_model_ = link;
00581
00582
00583 link_model_map_[joint->child_link_model_->name_] = link;
00584 link->tree_index_ = link_model_vector_.size();
00585 link_model_vector_.push_back(link);
00586 link_model_vector_const_.push_back(link);
00587 link_model_names_vector_.push_back(link->getName());
00588 if (link->shape_)
00589 {
00590 link_models_with_collision_geometry_vector_.push_back(link);
00591 link_model_names_with_collision_geometry_vector_.push_back(link->getName());
00592 }
00593 link->parent_joint_model_ = joint;
00594
00595
00596 for (std::size_t i = 0 ; i < urdf_link->child_links.size() ; ++i)
00597 {
00598 JointModel* jm = buildRecursive(link, urdf_link->child_links[i].get(), srdf_model);
00599 if (jm)
00600 link->child_joint_models_.push_back(jm);
00601 }
00602 return joint;
00603 }
00604
00605 robot_model::JointModel* robot_model::RobotModel::constructJointModel(const urdf::Joint *urdf_joint, const urdf::Link *child_link,
00606 const srdf::Model &srdf_model)
00607 {
00608 JointModel* result = NULL;
00609
00610
00611 if (urdf_joint)
00612 {
00613 switch (urdf_joint->type)
00614 {
00615 case urdf::Joint::REVOLUTE:
00616 {
00617 RevoluteJointModel *j = new RevoluteJointModel(urdf_joint->name);
00618 if (urdf_joint->safety)
00619 {
00620 j->variable_bounds_[0] = std::make_pair(urdf_joint->safety->soft_lower_limit, urdf_joint->safety->soft_upper_limit);
00621 if (urdf_joint->limits)
00622 {
00623 if (urdf_joint->limits->lower > j->variable_bounds_[0].first)
00624 j->variable_bounds_[0].first = urdf_joint->limits->lower;
00625 if (urdf_joint->limits->upper < j->variable_bounds_[0].second)
00626 j->variable_bounds_[0].second = urdf_joint->limits->upper;
00627 }
00628 }
00629 else
00630 {
00631 if (urdf_joint->limits)
00632 j->variable_bounds_[0] = std::make_pair(urdf_joint->limits->lower, urdf_joint->limits->upper);
00633 else
00634 j->variable_bounds_[0] = std::make_pair(0.0, 0.0);
00635 }
00636 if (urdf_joint->limits)
00637 j->max_velocity_ = fabs(urdf_joint->limits->velocity);
00638 j->continuous_ = false;
00639 j->axis_ = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);
00640 result = j;
00641 }
00642 break;
00643 case urdf::Joint::CONTINUOUS:
00644 {
00645 RevoluteJointModel *j = new RevoluteJointModel(urdf_joint->name);
00646 j->continuous_ = true;
00647 j->variable_bounds_[0] = std::make_pair(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
00648 if (urdf_joint->limits)
00649 j->max_velocity_ = fabs(urdf_joint->limits->velocity);
00650 j->axis_ = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);
00651 result = j;
00652 }
00653 break;
00654 case urdf::Joint::PRISMATIC:
00655 {
00656 PrismaticJointModel *j = new PrismaticJointModel(urdf_joint->name);
00657 if(urdf_joint->safety)
00658 {
00659 j->variable_bounds_[0] = std::make_pair(urdf_joint->safety->soft_lower_limit, urdf_joint->safety->soft_upper_limit);
00660 if (urdf_joint->limits)
00661 {
00662 if (urdf_joint->limits->lower > j->variable_bounds_[0].first)
00663 j->variable_bounds_[0].first = urdf_joint->limits->lower;
00664 if (urdf_joint->limits->upper < j->variable_bounds_[0].second)
00665 j->variable_bounds_[0].second = urdf_joint->limits->upper;
00666 }
00667 }
00668 else
00669 {
00670 if (urdf_joint->limits)
00671 j->variable_bounds_[0] = std::make_pair(urdf_joint->limits->lower, urdf_joint->limits->upper);
00672 else
00673 j->variable_bounds_[0] = std::make_pair(0.0, 0.0);
00674 }
00675 if (urdf_joint->limits)
00676 j->max_velocity_ = fabs(urdf_joint->limits->velocity);
00677 j->axis_ = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);
00678 result = j;
00679 }
00680 break;
00681 case urdf::Joint::FLOATING:
00682 result = new FloatingJointModel(urdf_joint->name);
00683 break;
00684 case urdf::Joint::PLANAR:
00685 result = new PlanarJointModel(urdf_joint->name);
00686 break;
00687 case urdf::Joint::FIXED:
00688 result = new FixedJointModel(urdf_joint->name);
00689 break;
00690 default:
00691 logError("Unknown joint type: %d", (int)urdf_joint->type);
00692 break;
00693 }
00694 }
00695 else
00696 {
00697 const std::vector<srdf::Model::VirtualJoint> &vjoints = srdf_model.getVirtualJoints();
00698 for (std::size_t i = 0 ; i < vjoints.size() ; ++i)
00699 if (vjoints[i].child_link_ == child_link->name && !vjoints[i].parent_frame_.empty())
00700 {
00701 if (vjoints[i].type_ == "fixed")
00702 result = new FixedJointModel(vjoints[i].name_);
00703 else if (vjoints[i].type_ == "planar")
00704 result = new PlanarJointModel(vjoints[i].name_);
00705 else if (vjoints[i].type_ == "floating")
00706 result = new FloatingJointModel(vjoints[i].name_);
00707 if (result)
00708 {
00709
00710 if (vjoints[i].type_ != "fixed")
00711 {
00712 model_frame_ = vjoints[i].parent_frame_;
00713 if (model_frame_[0] != '/')
00714 model_frame_ = '/' + model_frame_;
00715 }
00716 break;
00717 }
00718 }
00719 if (!result)
00720 {
00721 logInform("No root joint specified. Assuming fixed joint");
00722 result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT");
00723 }
00724 }
00725
00726 if (result)
00727 {
00728 for (std::size_t i = 0 ; i < result->variable_names_.size() ; ++i)
00729 result->variable_index_[result->variable_names_[i]] = i;
00730 result->setDistanceFactor(result->getStateSpaceDimension());
00731
00732 const std::vector<srdf::Model::PassiveJoint> &pjoints = srdf_model.getPassiveJoints();
00733 for (std::size_t i = 0 ; i < pjoints.size() ; ++i)
00734 {
00735 if (result->getName() == pjoints[i].name_)
00736 {
00737 result->passive_ = true;
00738 break;
00739 }
00740 }
00741 result->computeDefaultVariableLimits();
00742 }
00743 return result;
00744 }
00745
00746 namespace robot_model
00747 {
00748 static inline Eigen::Affine3d urdfPose2Affine3d(const urdf::Pose &pose)
00749 {
00750 Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
00751 Eigen::Affine3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z)*q.toRotationMatrix());
00752 return af;
00753 }
00754
00755 }
00756
00757 robot_model::LinkModel* robot_model::RobotModel::constructLinkModel(const urdf::Link *urdf_link)
00758 {
00759 LinkModel *result = new LinkModel();
00760 result->name_ = urdf_link->name;
00761 if (urdf_link->collision && urdf_link->collision->geometry)
00762 {
00763 result->collision_origin_transform_ = urdfPose2Affine3d(urdf_link->collision->origin);
00764 result->shape_ = constructShape(urdf_link->collision->geometry.get());
00765 if (result->shape_)
00766 {
00767 if (shapes::constructMsgFromShape(result->shape_.get(), result->shape_msg_))
00768 result->shape_extents_ = shapes::computeShapeExtents(result->shape_msg_);
00769 else
00770 result->shape_extents_ = Eigen::Vector3d(0.0, 0.0, 0.0);
00771 }
00772 }
00773 else if (urdf_link->visual && urdf_link->visual->geometry)
00774 {
00775 result->collision_origin_transform_ = urdfPose2Affine3d(urdf_link->visual->origin);
00776 result->shape_ = constructShape(urdf_link->visual->geometry.get());
00777 if (result->shape_)
00778 {
00779 if (shapes::constructMsgFromShape(result->shape_.get(), result->shape_msg_))
00780 result->shape_extents_ = shapes::computeShapeExtents(result->shape_msg_);
00781 else
00782 result->shape_extents_ = Eigen::Vector3d(0.0, 0.0, 0.0);
00783 }
00784 }
00785 else
00786 {
00787 result->collision_origin_transform_.setIdentity();
00788 result->shape_.reset();
00789 result->shape_extents_ = Eigen::Vector3d(0.0, 0.0, 0.0);
00790 }
00791
00792
00793 if (urdf_link->visual && urdf_link->visual->geometry)
00794 {
00795 if (urdf_link->visual->geometry->type == urdf::Geometry::MESH)
00796 {
00797 const urdf::Mesh *mesh = static_cast<const urdf::Mesh*>(urdf_link->visual->geometry.get());
00798 if (!mesh->filename.empty())
00799 {
00800 result->visual_mesh_filename_ = mesh->filename;
00801 result->visual_mesh_scale_ = Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z);
00802 }
00803 }
00804 }
00805 else
00806 if (urdf_link->collision && urdf_link->collision->geometry)
00807 {
00808 if (urdf_link->collision->geometry->type == urdf::Geometry::MESH)
00809 {
00810 const urdf::Mesh *mesh = static_cast<const urdf::Mesh*>(urdf_link->collision->geometry.get());
00811 if (!mesh->filename.empty())
00812 {
00813 result->visual_mesh_filename_ = mesh->filename;
00814 result->visual_mesh_scale_ = Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z);
00815 }
00816 }
00817 }
00818
00819 if (urdf_link->parent_joint)
00820 result->joint_origin_transform_ = urdfPose2Affine3d(urdf_link->parent_joint->parent_to_joint_origin_transform);
00821 else
00822 result->joint_origin_transform_.setIdentity();
00823
00824 return result;
00825 }
00826
00827 shapes::ShapePtr robot_model::RobotModel::constructShape(const urdf::Geometry *geom)
00828 {
00829 moveit::Profiler::ScopedBlock prof_block("RobotModel::constructShape");
00830
00831 shapes::Shape *result = NULL;
00832 switch (geom->type)
00833 {
00834 case urdf::Geometry::SPHERE:
00835 result = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
00836 break;
00837 case urdf::Geometry::BOX:
00838 {
00839 urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
00840 result = new shapes::Box(dim.x, dim.y, dim.z);
00841 }
00842 break;
00843 case urdf::Geometry::CYLINDER:
00844 result = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
00845 static_cast<const urdf::Cylinder*>(geom)->length);
00846 break;
00847 case urdf::Geometry::MESH:
00848 {
00849 const urdf::Mesh *mesh = static_cast<const urdf::Mesh*>(geom);
00850 if (!mesh->filename.empty())
00851 {
00852 Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
00853 shapes::Mesh *m = shapes::createMeshFromResource(mesh->filename, scale);
00854 result = m;
00855 }
00856 }
00857 break;
00858 default:
00859 logError("Unknown geometry type: %d", (int)geom->type);
00860 break;
00861 }
00862
00863 return shapes::ShapePtr(result);
00864 }
00865
00866 const std::string& robot_model::RobotModel::getRootJointName() const
00867 {
00868 static const std::string empty;
00869 return getRoot() ? getRoot()->getName() : empty;
00870 }
00871
00872 const std::string& robot_model::RobotModel::getRootLinkName() const
00873 {
00874 static const std::string empty;
00875 return getRootLink() ? getRootLink()->getName() : empty;
00876 }
00877
00878 bool robot_model::RobotModel::hasJointModel(const std::string &name) const
00879 {
00880 return joint_model_map_.find(name) != joint_model_map_.end();
00881 }
00882
00883 bool robot_model::RobotModel::hasLinkModel(const std::string &name) const
00884 {
00885 return link_model_map_.find(name) != link_model_map_.end();
00886 }
00887
00888 const robot_model::JointModel* robot_model::RobotModel::getJointModel(const std::string &name) const
00889 {
00890 std::map<std::string, JointModel*>::const_iterator it = joint_model_map_.find(name);
00891 if (it == joint_model_map_.end())
00892 {
00893 logError("Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
00894 return NULL;
00895 }
00896 else
00897 return it->second;
00898 }
00899
00900 const robot_model::LinkModel* robot_model::RobotModel::getLinkModel(const std::string &name) const
00901 {
00902 std::map<std::string, LinkModel*>::const_iterator it = link_model_map_.find(name);
00903 if (it == link_model_map_.end())
00904 {
00905 logError("Link '%s' not found", name.c_str());
00906 return NULL;
00907 }
00908 else
00909 return it->second;
00910 }
00911
00912 void robot_model::RobotModel::getChildLinkModels(const LinkModel *parent, std::vector<const LinkModel*> &links) const
00913 {
00914 links.clear();
00915 links.push_back(parent);
00916 std::queue<const LinkModel*> q;
00917 std::set<const LinkModel*> seen;
00918 q.push(parent);
00919 while (!q.empty())
00920 {
00921 const LinkModel* t = q.front();
00922 q.pop();
00923 if (seen.insert(t).second)
00924 for (std::size_t i = 0 ; i < t->child_joint_models_.size() ; ++i)
00925 {
00926 links.push_back(t->child_joint_models_[i]->child_link_model_);
00927 q.push(t->child_joint_models_[i]->child_link_model_);
00928 for (std::size_t j = 0 ; j < t->child_joint_models_[i]->mimic_requests_.size() ; ++j)
00929 {
00930 links.push_back(t->child_joint_models_[i]->mimic_requests_[j]->child_link_model_);
00931 q.push(t->child_joint_models_[i]->mimic_requests_[j]->child_link_model_);
00932 }
00933 }
00934 }
00935 }
00936
00937 void robot_model::RobotModel::getChildLinkModels(const JointModel *parent, std::vector<const LinkModel*> &links) const
00938 {
00939 getChildLinkModels(parent->child_link_model_, links);
00940 }
00941
00942 void robot_model::RobotModel::getChildJointModels(const LinkModel *parent, std::vector<const JointModel*> &joints) const
00943 {
00944 joints.clear();
00945 std::queue<const LinkModel*> q;
00946 std::set<const LinkModel*> seen;
00947 q.push(parent);
00948
00949 while (!q.empty())
00950 {
00951 const LinkModel* t = q.front();
00952 q.pop();
00953 if (seen.insert(t).second)
00954 for (unsigned int i = 0 ; i < t->child_joint_models_.size() ; ++i)
00955 {
00956 joints.push_back(t->child_joint_models_[i]);
00957 q.push(t->child_joint_models_[i]->child_link_model_);
00958 for (std::size_t j = 0 ; j < t->child_joint_models_[i]->mimic_requests_.size() ; ++j)
00959 {
00960 joints.push_back(t->child_joint_models_[i]->mimic_requests_[j]);
00961 q.push(t->child_joint_models_[i]->mimic_requests_[j]->child_link_model_);
00962 }
00963 }
00964 }
00965 }
00966
00967 void robot_model::RobotModel::getChildJointModels(const JointModel *parent, std::vector<const JointModel*> &joints) const
00968 {
00969 getChildJointModels(parent->child_link_model_, joints);
00970 joints.insert(joints.begin(), parent);
00971 }
00972
00973 std::vector<std::string> robot_model::RobotModel::getChildLinkModelNames(const LinkModel *parent) const
00974 {
00975 std::vector<const LinkModel*> links;
00976 getChildLinkModels(parent, links);
00977 std::vector<std::string> ret_vec(links.size());
00978 for (std::size_t i = 0; i < links.size(); ++i)
00979 ret_vec[i] = links[i]->getName();
00980 return ret_vec;
00981 }
00982
00983 std::vector<std::string> robot_model::RobotModel::getChildLinkModelNames(const JointModel *parent) const
00984 {
00985 std::vector<const LinkModel*> links;
00986 getChildLinkModels(parent, links);
00987 std::vector<std::string> ret_vec(links.size());
00988 for(unsigned int i = 0; i < links.size(); ++i)
00989 ret_vec[i] = links[i]->getName();
00990 return ret_vec;
00991 }
00992
00993 std::vector<std::string> robot_model::RobotModel::getChildJointModelNames(const LinkModel *parent) const
00994 {
00995 std::vector<const JointModel*> joints;
00996 getChildJointModels(parent, joints);
00997 std::vector<std::string> ret_vec(joints.size());
00998 for(unsigned int i = 0 ; i < joints.size() ; ++i)
00999 ret_vec[i] = joints[i]->getName();
01000 return ret_vec;
01001 }
01002
01003 std::vector<std::string> robot_model::RobotModel::getChildJointModelNames(const JointModel *parent) const
01004 {
01005 std::vector<const JointModel*> joints;
01006 getChildJointModels(parent, joints);
01007 std::vector<std::string> ret_vec(joints.size());
01008 for(unsigned int i = 0 ; i < joints.size(); ++i)
01009 ret_vec[i] = joints[i]->getName();
01010 return ret_vec;
01011 }
01012
01013
01014 void robot_model::RobotModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values) const
01015 {
01016 for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
01017 if (joint_model_vector_[i]->mimic_ == NULL)
01018 joint_model_vector_[i]->getVariableRandomValues(rng, values);
01019 }
01020
01021 void robot_model::RobotModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::map<std::string, double> &values) const
01022 {
01023 for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
01024 if (joint_model_vector_[i]->mimic_ == NULL)
01025 joint_model_vector_[i]->getVariableRandomValues(rng, values);
01026 }
01027
01028 void robot_model::RobotModel::getVariableDefaultValues(std::vector<double> &values) const
01029 {
01030 for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
01031 if (joint_model_vector_[i]->mimic_ == NULL)
01032 joint_model_vector_[i]->getVariableDefaultValues(values);
01033 }
01034
01035 void robot_model::RobotModel::getVariableDefaultValues(std::map<std::string, double> &values) const
01036 {
01037 for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
01038 if (joint_model_vector_[i]->mimic_ == NULL)
01039 joint_model_vector_[i]->getVariableDefaultValues(values);
01040 }
01041
01042 void robot_model::RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn> &allocators)
01043 {
01044 for (std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end() ; ++it)
01045 {
01046 JointModelGroup *jmg = it->second;
01047 std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
01048 std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
01049 if (jt == allocators.end())
01050 {
01051
01052 std::set<const JointModel*> joints;
01053 joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
01054
01055 std::vector<const JointModelGroup*> subs;
01056
01057
01058 for (std::map<std::string, SolverAllocatorFn>::const_iterator kt = allocators.begin() ; kt != allocators.end() ; ++kt)
01059 {
01060 const JointModelGroup *sub = getJointModelGroup(kt->first);
01061 if (!sub)
01062 {
01063 subs.clear();
01064 break;
01065 }
01066 std::set<const JointModel*> sub_joints;
01067 sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
01068
01069 if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
01070 {
01071 std::set<const JointModel*> resultj;
01072 std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
01073 std::inserter(resultj, resultj.end()));
01074 subs.push_back(sub);
01075 joints.swap(resultj);
01076 }
01077 }
01078
01079
01080 if (!subs.empty())
01081 {
01082 std::stringstream ss;
01083 for (std::size_t i = 0 ; i < subs.size() ; ++i)
01084 {
01085 ss << subs[i]->getName() << " ";
01086 result.second[subs[i]] = allocators.find(subs[i]->getName())->second;
01087 }
01088 logDebug("Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), ss.str().c_str());
01089 }
01090 }
01091 else
01092
01093 result.first = jt->second;
01094 jmg->setSolverAllocators(result);
01095 }
01096 }
01097
01098 void robot_model::RobotModel::printModelInfo(std::ostream &out) const
01099 {
01100 out << "Model " << model_name_ << " in frame " << model_frame_ << ", of dimension " << getVariableCount() << std::endl;
01101
01102 std::ios_base::fmtflags old_flags = out.flags();
01103 out.setf(std::ios::fixed, std::ios::floatfield);
01104 std::streamsize old_prec = out.precision();
01105 out.precision(5);
01106 out << "Joint values bounds: " << std::endl;
01107 for (unsigned int i = 0 ; i < joint_model_vector_.size() ; ++i)
01108 {
01109 const std::vector<std::string> &vn = joint_model_vector_[i]->getVariableNames();
01110 for (std::vector<std::string>::const_iterator it = vn.begin() ; it != vn.end() ; ++it)
01111 {
01112 out << " " << *it << " [";
01113 std::pair<double, double> b;
01114 joint_model_vector_[i]->getVariableBounds(*it, b);
01115 if (b.first <= -std::numeric_limits<double>::max())
01116 out << "DBL_MIN";
01117 else
01118 out << b.first;
01119 out << ", ";
01120 if (b.second >= std::numeric_limits<double>::max())
01121 out << "DBL_MAX";
01122 else
01123 out << b.second;
01124 out << "]";
01125 if (joint_model_vector_[i]->mimic_)
01126 out << " *";
01127 if (joint_model_vector_[i]->passive_)
01128 out << " +";
01129 out << std::endl;
01130 }
01131 }
01132 out << std::endl;
01133 out.precision(old_prec);
01134 out.flags(old_flags);
01135
01136 out << "Available groups: " << std::endl;
01137 for (std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end() ; ++it)
01138 {
01139 out << " " << it->first << " (of dimension " << it->second->getVariableCount() << "):" << std::endl;
01140 out << " joints:" << std::endl;
01141 const std::vector<std::string> &jnt = it->second->getJointModelNames();
01142 for (std::size_t k = 0 ; k < jnt.size() ; ++k)
01143 out << " " << jnt[k] << std::endl;
01144 out << " links:" << std::endl;
01145 const std::vector<std::string> &lnk = it->second->getLinkModelNames();
01146 for (std::size_t k = 0 ; k < lnk.size() ; ++k)
01147 out << " " << lnk[k] << std::endl;
01148 out << " roots:" << std::endl;
01149 const std::vector<const JointModel*> &jr = it->second->getJointRoots();
01150 for (std::size_t k = 0 ; k < jr.size() ; ++k)
01151 out << " " << jr[k]->getName() << std::endl;
01152
01153 }
01154 }
01155
01156 void robot_model::RobotModel::computeFixedTransforms(LinkModel *link, const Eigen::Affine3d &transform, LinkModelToAffine3dMap &associated_transforms)
01157 {
01158 associated_transforms[link] = transform;
01159 for (std::size_t i = 0 ; i < link->getChildJointModels().size() ; ++i)
01160 if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
01161 computeFixedTransforms(link->getChildJointModels()[i]->child_link_model_, transform * link->getJointOriginTransform(), associated_transforms);
01162 }