$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, 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 the 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 00037 #include <queue> 00038 #include <cmath> 00039 #include <set> 00040 00041 #include <ros/ros.h> 00042 #include <planning_models/kinematic_model.h> 00043 #include <geometric_shapes/shape_operations.h> 00044 #include <ros/console.h> 00045 #include <angles/angles.h> 00046 00047 /* ------------------------ KinematicModel ------------------------ */ 00048 planning_models::KinematicModel::KinematicModel(const urdf::Model &model, 00049 const std::vector<GroupConfig>& group_configs, 00050 const std::vector<MultiDofConfig>& multi_dof_configs) 00051 { 00052 model_name_ = model.getName(); 00053 if (model.getRoot()) 00054 { 00055 const urdf::Link *root = model.getRoot().get(); 00056 root_ = buildRecursive(NULL, root, multi_dof_configs); 00057 buildGroups(group_configs); 00058 } 00059 else 00060 { 00061 root_ = NULL; 00062 ROS_WARN("No root link found"); 00063 } 00064 } 00065 00066 planning_models::KinematicModel::KinematicModel(const KinematicModel &source) 00067 { 00068 copyFrom(source); 00069 } 00070 00071 planning_models::KinematicModel::~KinematicModel(void) 00072 { 00073 for (std::map<std::string, JointModelGroup*>::iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end() ; ++it) 00074 { 00075 delete it->second; 00076 } 00077 //this destroys the whole tree 00078 if(root_) { 00079 delete root_; 00080 } 00081 } 00082 00083 void planning_models::KinematicModel::exclusiveLock(void) const 00084 { 00085 lock_.lock(); 00086 } 00087 00088 void planning_models::KinematicModel::exclusiveUnlock(void) const 00089 { 00090 lock_.unlock(); 00091 } 00092 00093 void planning_models::KinematicModel::sharedLock(void) const 00094 { 00095 lock_.lock_shared(); 00096 } 00097 00098 void planning_models::KinematicModel::sharedUnlock(void) const 00099 { 00100 lock_.unlock_shared(); 00101 } 00102 00103 const std::string& planning_models::KinematicModel::getName() const { 00104 return model_name_; 00105 } 00106 00107 void planning_models::KinematicModel::copyFrom(const KinematicModel &source) 00108 { 00109 model_name_ = source.model_name_; 00110 00111 if (source.root_) 00112 { 00113 root_ = copyRecursive(NULL, source.root_->child_link_model_); 00114 00115 00116 const std::map<std::string, JointModelGroup*>& source_group_map = source.getJointModelGroupMap(); 00117 std::map< std::string, std::vector<std::string> > groupContent; 00118 std::vector<GroupConfig> group_configs; 00119 std::vector<std::string> empty_strings; 00120 for(std::map<std::string, JointModelGroup*>::const_iterator it = source_group_map.begin(); 00121 it != source_group_map.end(); 00122 it++) { 00123 group_configs.push_back(GroupConfig(it->first, it->second->getJointModelNames(), empty_strings)); 00124 } 00125 buildGroups(group_configs); 00126 } else 00127 { 00128 root_ = NULL; 00129 } 00130 } 00131 00132 void planning_models::KinematicModel::buildGroups(const std::vector<GroupConfig>& group_configs) 00133 { 00134 //the only thing tricky is dealing with subgroups 00135 std::vector<bool> processed(group_configs.size(), false); 00136 while(true) { 00137 //going to make passes until we can't do anything else 00138 bool added = false; 00139 for(unsigned int i = 0; i < group_configs.size(); i++) { 00140 if(!processed[i]) { 00141 //if we haven't processed, check and see if the dependencies are met yet 00142 bool all_subgroups_added = true; 00143 for(unsigned int j = 0; j < group_configs[i].subgroups_.size(); j++) { 00144 if(joint_model_group_map_.find(group_configs[i].subgroups_[j]) == 00145 joint_model_group_map_.end()) { 00146 all_subgroups_added = false; 00147 break; 00148 } 00149 } 00150 if(all_subgroups_added) { 00151 //only get one chance to do it right 00152 if(addModelGroup(group_configs[i])) { 00153 processed[i] = true; 00154 added = true; 00155 } else { 00156 ROS_WARN_STREAM("Failed to add group " << group_configs[i].name_); 00157 } 00158 } 00159 } 00160 } 00161 if(!added) { 00162 for(unsigned int i = 0; i < processed.size(); i++) { 00163 if(!processed[i]) { 00164 ROS_WARN_STREAM("Could not process group " << group_configs[i].name_ << " due to unmet subgroup dependencies"); 00165 } 00166 } 00167 return; 00168 } 00169 } 00170 } 00171 00172 void planning_models::KinematicModel::removeModelGroup(const std::string& group) { 00173 if(!hasModelGroup(group)) return; 00174 delete joint_model_group_map_[group]; 00175 joint_model_group_map_.erase(group); 00176 joint_model_group_config_map_.erase(group); 00177 } 00178 00179 bool planning_models::KinematicModel::addModelGroup(const planning_models::KinematicModel::GroupConfig& gc) 00180 { 00181 if(joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end()) { 00182 ROS_WARN_STREAM("Already have a model group named " << gc.name_ <<". Not adding."); 00183 return false; 00184 } 00185 std::vector<const JointModel*> jointv; 00186 std::vector<const JointModel*> fixed_jointv; 00187 if(!gc.tip_link_.empty() && !gc.base_link_.empty()) { 00188 if(!gc.subgroups_.empty()) { 00189 ROS_WARN_STREAM("Ignoring subgroups as tip and base are defined for group " << gc.name_); 00190 } 00191 //if this is not a physical robot link but is the world link 00192 bool base_link_is_world_link = (gc.base_link_ == getRoot()->getParentFrameId() && 00193 getLinkModel(gc.base_link_) == NULL); 00194 const LinkModel* base_link = NULL; 00195 if(!base_link_is_world_link) { 00196 base_link = getLinkModel(gc.base_link_); 00197 if(base_link == NULL) { 00198 ROS_WARN_STREAM("Group config " << gc.name_ << " has invalid base link " << gc.base_link_); 00199 return false; 00200 } 00201 } 00202 const LinkModel* tip_link = getLinkModel(gc.tip_link_); 00203 if(tip_link == NULL) { 00204 ROS_WARN_STREAM("Group config " << gc.name_ << " has invalid tip link " << gc.tip_link_); 00205 return false; 00206 } 00207 const LinkModel* lm = tip_link; 00208 bool ok = false; 00209 while(true) { 00210 if(lm == NULL) { 00211 if(base_link_is_world_link) { 00212 ok = true; 00213 } 00214 break; 00215 } 00216 if(lm == base_link) { 00217 ok = true; 00218 break; 00219 } 00220 if(lm->getParentJointModel() == NULL) { 00221 break; 00222 } 00223 //only adding non-fixed joint models 00224 const FixedJointModel* fjm = dynamic_cast<const FixedJointModel*>(lm->getParentJointModel()); 00225 if(fjm == NULL) { 00226 jointv.push_back(lm->getParentJointModel()); 00227 } else { 00228 fixed_jointv.push_back(fjm); 00229 } 00230 lm = lm->getParentJointModel()->getParentLinkModel(); 00231 } 00232 if(!ok) { 00233 ROS_WARN_STREAM("For group " << gc.name_ 00234 << " base link " << gc.base_link_ 00235 << " does not appear to be a direct descendent of " << gc.tip_link_); 00236 return false; 00237 } 00238 //need to reverse jointv to get things in right order 00239 std::reverse(jointv.begin(), jointv.end()); 00240 } else { 00241 if(!gc.subgroups_.empty()) { 00242 std::set<const JointModel*> joint_set; 00243 for(unsigned int i = 0; i < gc.subgroups_.size(); i++) { 00244 if(joint_model_group_map_.find(gc.subgroups_[i]) == joint_model_group_map_.end()) { 00245 ROS_INFO_STREAM("Subgroup " << gc.subgroups_[i] << " not defined so can't add group " << gc.name_); 00246 return false; 00247 } 00248 const JointModelGroup* jmg = joint_model_group_map_.find(gc.subgroups_[i])->second; 00249 for(unsigned int j = 0; j < jmg->getJointModels().size(); j++) { 00250 joint_set.insert(jmg->getJointModels()[j]); 00251 } 00252 } 00253 for(std::set<const JointModel*>::iterator it = joint_set.begin(); 00254 it != joint_set.end(); 00255 it++) { 00256 jointv.push_back((*it)); 00257 } 00258 } 00259 if(gc.joints_.size() == 0 && gc.subgroups_.empty()) { 00260 ROS_WARN_STREAM("Group " << gc.name_ << " must have tip/base links, subgroups, or one or more joints"); 00261 return false; 00262 } 00263 for(unsigned int j = 0; j < gc.joints_.size(); j++) { 00264 const JointModel* joint = getJointModel(gc.joints_[j]); 00265 if(joint == NULL) { 00266 ROS_ERROR_STREAM("Group " << gc.name_ << " has invalid joint " << gc.joints_[j]); 00267 return false; 00268 } 00269 jointv.push_back(joint); 00270 } 00271 } 00272 if(jointv.size() == 0) { 00273 ROS_WARN_STREAM("Group " << gc.name_ << " must have at least one valid joint"); 00274 return false; 00275 } 00276 joint_model_group_map_[gc.name_] = new JointModelGroup(gc.name_, jointv, fixed_jointv, this); 00277 joint_model_group_config_map_[gc.name_] = gc; 00278 return true; 00279 } 00280 00281 planning_models::KinematicModel::JointModel* planning_models::KinematicModel::buildRecursive(LinkModel *parent, const urdf::Link *link, 00282 const std::vector<MultiDofConfig>& multi_dof_configs) 00283 { 00284 JointModel *joint = constructJointModel(link->parent_joint.get(), link, multi_dof_configs); 00285 if(joint == NULL) { 00286 return NULL; 00287 } 00288 joint_model_map_[joint->name_] = joint; 00289 for(JointModel::js_type::const_iterator it = joint->getJointStateEquivalents().begin(); 00290 it != joint->getJointStateEquivalents().end(); 00291 it++) { 00292 joint_model_map_[it->right] = joint; 00293 } 00294 joint_model_vector_.push_back(joint); 00295 joint->parent_link_model_ = parent; 00296 joint->child_link_model_ = constructLinkModel(link); 00297 if (parent == NULL) 00298 joint->child_link_model_->joint_origin_transform_.setIdentity(); 00299 link_model_map_[joint->child_link_model_->name_] = joint->child_link_model_; 00300 link_model_vector_.push_back(joint->child_link_model_); 00301 if(joint->child_link_model_->shape_ != NULL) { 00302 link_models_with_collision_geometry_vector_.push_back(joint->child_link_model_); 00303 } 00304 joint->child_link_model_->parent_joint_model_ = joint; 00305 00306 for (unsigned int i = 0 ; i < link->child_links.size() ; ++i) { 00307 JointModel* jm = buildRecursive(joint->child_link_model_, link->child_links[i].get(), multi_dof_configs); 00308 if(jm != NULL) { 00309 joint->child_link_model_->child_joint_models_.push_back(jm); 00310 } 00311 } 00312 00313 return joint; 00314 } 00315 00316 planning_models::KinematicModel::JointModel* planning_models::KinematicModel::constructJointModel(const urdf::Joint *urdf_joint, 00317 const urdf::Link *child_link, 00318 const std::vector<MultiDofConfig>& multi_dof_configs) 00319 { 00320 const MultiDofConfig* joint_config = NULL; 00321 bool found = false; 00322 for(std::vector<MultiDofConfig>::const_iterator it = multi_dof_configs.begin(); 00323 it != multi_dof_configs.end(); 00324 it++) { 00325 if(it->child_frame_id == child_link->name) { 00326 if(found == true) { 00327 ROS_WARN_STREAM("KinematicModel - two multi-dof joints with same " << it->child_frame_id << " child frame"); 00328 } else { 00329 found = true; 00330 joint_config = &(*it); 00331 } 00332 } 00333 } 00334 00335 planning_models::KinematicModel::JointModel* result = NULL; 00336 00337 //must be the root link transform 00338 if(urdf_joint == NULL) { 00339 if(!found) { 00340 ROS_ERROR("Root transform has no multi dof joint config"); 00341 return NULL; 00342 } 00343 if(joint_config->type == "Planar") { 00344 result = new PlanarJointModel(joint_config->name, joint_config); 00345 } else if(joint_config->type == "Floating") { 00346 result = new FloatingJointModel(joint_config->name, joint_config); 00347 } else if(joint_config->type == "Fixed"){ 00348 result = new FixedJointModel(joint_config->name, joint_config); 00349 } else { 00350 ROS_ERROR_STREAM("Unrecognized type of multi dof joint " << joint_config->type); 00351 return NULL; 00352 } 00353 } else { 00354 switch (urdf_joint->type) 00355 { 00356 case urdf::Joint::REVOLUTE: 00357 { 00358 RevoluteJointModel *j = new RevoluteJointModel(urdf_joint->name, joint_config); 00359 if(urdf_joint->safety) 00360 { 00361 j->setVariableBounds(j->name_, urdf_joint->safety->soft_lower_limit, urdf_joint->safety->soft_upper_limit); 00362 } 00363 else 00364 { 00365 j->setVariableBounds(j->name_, urdf_joint->limits->lower, urdf_joint->limits->upper); 00366 } 00367 j->continuous_ = false; 00368 j->axis_.setValue(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z); 00369 result = j; 00370 } 00371 break; 00372 case urdf::Joint::CONTINUOUS: 00373 { 00374 RevoluteJointModel *j = new RevoluteJointModel(urdf_joint->name, joint_config); 00375 j->continuous_ = true; 00376 j->setVariableBounds(j->name_, -M_PI, M_PI); 00377 j->axis_.setValue(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z); 00378 result = j; 00379 } 00380 break; 00381 case urdf::Joint::PRISMATIC: 00382 { 00383 PrismaticJointModel *j = new PrismaticJointModel(urdf_joint->name, joint_config); 00384 if(urdf_joint->safety) 00385 { 00386 j->setVariableBounds(j->name_, urdf_joint->safety->soft_lower_limit, urdf_joint->safety->soft_upper_limit); 00387 } 00388 else 00389 { 00390 j->setVariableBounds(j->name_, urdf_joint->limits->lower, urdf_joint->limits->upper); 00391 } 00392 j->axis_.setValue(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z); 00393 result = j; 00394 } 00395 break; 00396 case urdf::Joint::FLOATING: 00397 result = new FloatingJointModel(urdf_joint->name, joint_config); 00398 break; 00399 case urdf::Joint::PLANAR: 00400 result = new PlanarJointModel(urdf_joint->name, joint_config); 00401 break; 00402 case urdf::Joint::FIXED: 00403 result = new FixedJointModel(urdf_joint->name, joint_config); 00404 break; 00405 default: 00406 ROS_ERROR("Unknown joint type: %d", (int)urdf_joint->type); 00407 break; 00408 } 00409 } 00410 return result; 00411 } 00412 00413 namespace planning_models 00414 { 00415 static inline btTransform urdfPose2btTransform(const urdf::Pose &pose) 00416 { 00417 return btTransform(btQuaternion(pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w), 00418 btVector3(pose.position.x, pose.position.y, pose.position.z)); 00419 } 00420 } 00421 00422 planning_models::KinematicModel::LinkModel* planning_models::KinematicModel::constructLinkModel(const urdf::Link *urdf_link) 00423 { 00424 ROS_ASSERT(urdf_link); 00425 00426 LinkModel *result = new LinkModel(this); 00427 result->name_ = urdf_link->name; 00428 00429 if(urdf_link->collision && urdf_link->collision->geometry) { 00430 result->collision_origin_transform_ = urdfPose2btTransform(urdf_link->collision->origin); 00431 result->shape_ = constructShape(urdf_link->collision->geometry.get()); 00432 } else if(urdf_link->visual && urdf_link->visual->geometry){ 00433 result->collision_origin_transform_ = urdfPose2btTransform(urdf_link->visual->origin); 00434 result->shape_ = constructShape(urdf_link->visual->geometry.get()); 00435 } else { 00436 result->collision_origin_transform_.setIdentity(); 00437 result->shape_ = NULL; 00438 } 00439 00440 if (urdf_link->parent_joint.get()) { 00441 result->joint_origin_transform_ = urdfPose2btTransform(urdf_link->parent_joint->parent_to_joint_origin_transform); 00442 ROS_DEBUG_STREAM("Setting joint origin for " << result->name_ << " to " 00443 << result->joint_origin_transform_.getOrigin().x() << " " 00444 << result->joint_origin_transform_.getOrigin().y() << " " 00445 << result->joint_origin_transform_.getOrigin().z()); 00446 } else { 00447 ROS_DEBUG_STREAM("Setting joint origin to identity for " << result->name_); 00448 result->joint_origin_transform_.setIdentity(); 00449 } 00450 return result; 00451 } 00452 00453 shapes::Shape* planning_models::KinematicModel::constructShape(const urdf::Geometry *geom) 00454 { 00455 ROS_ASSERT(geom); 00456 00457 shapes::Shape *result = NULL; 00458 switch (geom->type) 00459 { 00460 case urdf::Geometry::SPHERE: 00461 result = new shapes::Sphere(dynamic_cast<const urdf::Sphere*>(geom)->radius); 00462 break; 00463 case urdf::Geometry::BOX: 00464 { 00465 urdf::Vector3 dim = dynamic_cast<const urdf::Box*>(geom)->dim; 00466 result = new shapes::Box(dim.x, dim.y, dim.z); 00467 } 00468 break; 00469 case urdf::Geometry::CYLINDER: 00470 result = new shapes::Cylinder(dynamic_cast<const urdf::Cylinder*>(geom)->radius, 00471 dynamic_cast<const urdf::Cylinder*>(geom)->length); 00472 break; 00473 case urdf::Geometry::MESH: 00474 { 00475 const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom); 00476 if (!mesh->filename.empty()) 00477 { 00478 btVector3 scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); 00479 result = shapes::createMeshFromFilename(mesh->filename, &scale); 00480 } 00481 } 00482 break; 00483 default: 00484 ROS_ERROR("Unknown geometry type: %d", (int)geom->type); 00485 break; 00486 } 00487 00488 return result; 00489 } 00490 00491 const planning_models::KinematicModel::JointModel* planning_models::KinematicModel::getRoot(void) const 00492 { 00493 return root_; 00494 } 00495 00496 bool planning_models::KinematicModel::hasJointModel(const std::string &name) const 00497 { 00498 return joint_model_map_.find(name) != joint_model_map_.end(); 00499 } 00500 00501 bool planning_models::KinematicModel::hasLinkModel(const std::string &name) const 00502 { 00503 return link_model_map_.find(name) != link_model_map_.end(); 00504 } 00505 00506 bool planning_models::KinematicModel::hasModelGroup(const std::string &name) const 00507 { 00508 return joint_model_group_map_.find(name) != joint_model_group_map_.end(); 00509 } 00510 00511 const planning_models::KinematicModel::JointModel* planning_models::KinematicModel::getJointModel(const std::string &name) const 00512 { 00513 std::map<std::string, JointModel*>::const_iterator it = joint_model_map_.find(name); 00514 if (it == joint_model_map_.end()) 00515 { 00516 ROS_ERROR("Joint '%s' not found", name.c_str()); 00517 return NULL; 00518 } 00519 else 00520 return it->second; 00521 } 00522 00523 const planning_models::KinematicModel::LinkModel* planning_models::KinematicModel::getLinkModel(const std::string &name) const 00524 { 00525 std::map<std::string, LinkModel*>::const_iterator it = link_model_map_.find(name); 00526 if (it == link_model_map_.end()) 00527 { 00528 ROS_ERROR("Link '%s' not found", name.c_str()); 00529 return NULL; 00530 } 00531 else 00532 return it->second; 00533 } 00534 00535 void planning_models::KinematicModel::getModelGroupNames(std::vector<std::string> &groups) const 00536 { 00537 groups.clear(); 00538 groups.reserve(joint_model_group_map_.size()); 00539 for (std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end() ; ++it) 00540 groups.push_back(it->second->name_); 00541 } 00542 00543 void planning_models::KinematicModel::getLinkModelNames(std::vector<std::string> &links) const 00544 { 00545 links.clear(); 00546 links.reserve(link_model_vector_.size()); 00547 for(unsigned int i = 0; i < link_model_vector_.size(); i++) { 00548 links.push_back(link_model_vector_[i]->getName()); 00549 } 00550 } 00551 00552 void planning_models::KinematicModel::getJointModelNames(std::vector<std::string> &joints) const 00553 { 00554 joints.clear(); 00555 joints.reserve(joint_model_vector_.size()); 00556 for (unsigned int i = 0 ; i < joint_model_vector_.size() ; ++i) 00557 joints.push_back(joint_model_vector_[i]->getName()); 00558 } 00559 00560 planning_models::KinematicModel::JointModel* planning_models::KinematicModel::copyRecursive(LinkModel *parent, const LinkModel *link) 00561 { 00562 JointModel *joint = copyJointModel(link->parent_joint_model_); 00563 joint_model_map_[joint->name_] = joint; 00564 for(JointModel::js_type::iterator it = joint->joint_state_equivalents_.begin(); 00565 it != joint->joint_state_equivalents_.end(); 00566 it++) { 00567 joint_model_map_[it->right] = joint; 00568 } 00569 joint_model_vector_.push_back(joint); 00570 joint->parent_link_model_ = parent; 00571 joint->child_link_model_ = new LinkModel(link); 00572 link_model_map_[joint->child_link_model_->name_] = joint->child_link_model_; 00573 joint->child_link_model_->parent_joint_model_ = joint; 00574 00575 for (unsigned int i = 0 ; i < link->child_joint_models_.size() ; ++i) 00576 joint->child_link_model_->child_joint_models_.push_back(copyRecursive(joint->child_link_model_, link->child_joint_models_[i]->child_link_model_)); 00577 00578 return joint; 00579 } 00580 00581 planning_models::KinematicModel::JointModel* planning_models::KinematicModel::copyJointModel(const JointModel *joint) 00582 { 00583 JointModel *newJoint = NULL; 00584 00585 if (dynamic_cast<const FixedJointModel*>(joint)) 00586 { 00587 newJoint = new FixedJointModel(dynamic_cast<const FixedJointModel*>(joint)); 00588 } 00589 else if (dynamic_cast<const FloatingJointModel*>(joint)) 00590 { 00591 newJoint = new FloatingJointModel(dynamic_cast<const FloatingJointModel*>(joint)); 00592 } 00593 else if (dynamic_cast<const PlanarJointModel*>(joint)) 00594 { 00595 newJoint = new PlanarJointModel(dynamic_cast<const PlanarJointModel*>(joint)); 00596 } 00597 else if (dynamic_cast<const PrismaticJointModel*>(joint)) 00598 { 00599 newJoint = new PrismaticJointModel(dynamic_cast<const PrismaticJointModel*>(joint)); 00600 } 00601 else if (dynamic_cast<const RevoluteJointModel*>(joint)) 00602 { 00603 newJoint = new RevoluteJointModel(dynamic_cast<const RevoluteJointModel*>(joint)); 00604 } 00605 else 00606 ROS_FATAL("Unimplemented type of joint"); 00607 00608 return newJoint; 00609 } 00610 00611 void planning_models::KinematicModel::getChildLinkModels(const KinematicModel::LinkModel *parent, 00612 std::vector<const KinematicModel::LinkModel*> &links) const 00613 { 00614 links.push_back(parent); 00615 std::queue<const KinematicModel::LinkModel*> q; 00616 q.push(parent); 00617 while (!q.empty()) 00618 { 00619 const KinematicModel::LinkModel* t = q.front(); 00620 q.pop(); 00621 00622 for (unsigned int i = 0 ; i < t->child_joint_models_.size() ; ++i) { 00623 if (t->child_joint_models_[i]->child_link_model_) 00624 { 00625 links.push_back(t->child_joint_models_[i]->child_link_model_); 00626 q.push(t->child_joint_models_[i]->child_link_model_); 00627 } 00628 } 00629 } 00630 } 00631 00632 void planning_models::KinematicModel::getChildLinkModels(const KinematicModel::JointModel *parent, 00633 std::vector<const KinematicModel::LinkModel*> &links) const 00634 { 00635 getChildLinkModels(parent->child_link_model_, links); 00636 } 00637 00638 void planning_models::KinematicModel::getChildJointModels(const KinematicModel::LinkModel *parent, 00639 std::vector<const KinematicModel::JointModel*> &joints) const 00640 { 00641 const KinematicModel::LinkModel* t = parent; 00642 00643 std::queue<const KinematicModel::LinkModel*> q; 00644 while(1) { 00645 for (unsigned int i = 0 ; i < t->child_joint_models_.size() ; ++i) { 00646 joints.push_back(t->child_joint_models_[i]); 00647 if (t->child_joint_models_[i]->child_link_model_) 00648 { 00649 q.push(t->child_joint_models_[i]->child_link_model_); 00650 } 00651 } 00652 if(q.empty()) { 00653 break; 00654 } 00655 t = q.front(); 00656 q.pop(); 00657 } while (!q.empty()); 00658 } 00659 00660 void planning_models::KinematicModel::getChildJointModels(const KinematicModel::JointModel *parent, 00661 std::vector<const KinematicModel::JointModel*> &joints) const 00662 { 00663 joints.push_back(parent); 00664 if(parent->child_link_model_) { 00665 getChildJointModels(parent->child_link_model_, joints); 00666 } 00667 } 00668 00669 std::vector<std::string> 00670 planning_models::KinematicModel::getChildLinkModelNames(const KinematicModel::LinkModel *parent) const 00671 { 00672 std::vector<const KinematicModel::LinkModel*> links; 00673 getChildLinkModels(parent, links); 00674 std::vector<std::string> ret_vec(links.size()); 00675 for(unsigned int i = 0; i < links.size(); i++) { 00676 ret_vec[i] = links[i]->getName(); 00677 } 00678 return ret_vec; 00679 } 00680 00681 std::vector<std::string> 00682 planning_models::KinematicModel::getChildLinkModelNames(const KinematicModel::JointModel *parent) const 00683 { 00684 std::vector<const KinematicModel::LinkModel*> links; 00685 getChildLinkModels(parent,links); 00686 std::vector<std::string> ret_vec(links.size()); 00687 for(unsigned int i = 0; i < links.size(); i++) { 00688 ret_vec[i] = links[i]->getName(); 00689 } 00690 return ret_vec; 00691 } 00692 00693 std::vector<std::string> 00694 planning_models::KinematicModel::getChildJointModelNames(const KinematicModel::LinkModel *parent) const 00695 { 00696 std::vector<const KinematicModel::JointModel*> joints; 00697 getChildJointModels(parent,joints); 00698 std::vector<std::string> ret_vec(joints.size()); 00699 for(unsigned int i = 0; i < joints.size(); i++) { 00700 ret_vec[i] = joints[i]->getName(); 00701 } 00702 return ret_vec; 00703 } 00704 00705 std::vector<std::string> 00706 planning_models::KinematicModel::getChildJointModelNames(const KinematicModel::JointModel *parent) const 00707 { 00708 std::vector<const KinematicModel::JointModel*> joints; 00709 getChildJointModels(parent,joints); 00710 std::vector<std::string> ret_vec(joints.size()); 00711 for(unsigned int i = 0; i < joints.size(); i++) { 00712 ret_vec[i] = joints[i]->getName(); 00713 } 00714 return ret_vec; 00715 } 00716 00717 void planning_models::KinematicModel::clearAllAttachedBodyModels() 00718 { 00719 exclusiveLock(); 00720 for(unsigned int i =0; i < link_model_vector_.size(); i++) { 00721 link_model_vector_[i]->clearAttachedBodyModels(); 00722 } 00723 exclusiveUnlock(); 00724 } 00725 00726 std::vector<const planning_models::KinematicModel::AttachedBodyModel*> planning_models::KinematicModel::getAttachedBodyModels(void) const 00727 { 00728 std::vector<const planning_models::KinematicModel::AttachedBodyModel*> ret_vec; 00729 for(unsigned int i =0; i < link_model_vector_.size(); i++) { 00730 ret_vec.insert(ret_vec.end(), link_model_vector_[i]->getAttachedBodyModels().begin(), 00731 link_model_vector_[i]->getAttachedBodyModels().end()); 00732 } 00733 return ret_vec; 00734 } 00735 00736 void planning_models::KinematicModel::clearLinkAttachedBodyModels(const std::string& link_name) 00737 { 00738 exclusiveLock(); 00739 if(link_model_map_.find(link_name) == link_model_map_.end()) { 00740 exclusiveUnlock(); 00741 return; 00742 } 00743 link_model_map_[link_name]->clearAttachedBodyModels(); 00744 exclusiveUnlock(); 00745 } 00746 00747 void planning_models::KinematicModel::replaceAttachedBodyModels(const std::string& link_name, 00748 std::vector<AttachedBodyModel*>& attached_body_vector) 00749 { 00750 exclusiveLock(); 00751 if(link_model_map_.find(link_name) == link_model_map_.end()) 00752 { 00753 ROS_WARN_STREAM("Model has no link named " << link_name << ". This is probably going to introduce a memory leak"); 00754 exclusiveUnlock(); 00755 return; 00756 } 00757 link_model_map_[link_name]->replaceAttachedBodyModels(attached_body_vector); 00758 exclusiveUnlock(); 00759 } 00760 00761 void planning_models::KinematicModel::clearLinkAttachedBodyModel(const std::string& link_name, 00762 const std::string& att_name) 00763 { 00764 exclusiveLock(); 00765 if(link_model_map_.find(link_name) == link_model_map_.end()) { 00766 exclusiveUnlock(); 00767 return; 00768 } 00769 link_model_map_[link_name]->clearLinkAttachedBodyModel(att_name); 00770 exclusiveUnlock(); 00771 } 00772 00773 void planning_models::KinematicModel::addAttachedBodyModel(const std::string& link_name, 00774 planning_models::KinematicModel::AttachedBodyModel* ab) 00775 { 00776 exclusiveLock(); 00777 if(link_model_map_.find(link_name) == link_model_map_.end()) { 00778 ROS_WARN_STREAM("Model has no link named " << link_name << " to attach body to. This is probably going to introduce a memory leak"); 00779 exclusiveUnlock(); 00780 return; 00781 } 00782 link_model_map_[link_name]->addAttachedBodyModel(ab); 00783 exclusiveUnlock(); 00784 } 00785 00786 00787 /* ------------------------ JointModel ------------------------ */ 00788 00789 planning_models::KinematicModel::JointModel::JointModel(const std::string& n) : 00790 name_(n), parent_link_model_(NULL), child_link_model_(NULL) 00791 { 00792 } 00793 00794 void planning_models::KinematicModel::JointModel::initialize(const std::vector<std::string>& local_joint_names, 00795 const MultiDofConfig* config) 00796 { 00797 for(std::vector<std::string>::const_iterator it = local_joint_names.begin(); 00798 it != local_joint_names.end(); 00799 it++) { 00800 joint_state_equivalents_.insert(js_type::value_type(*it,*it)); 00801 } 00802 if(config != NULL) { 00803 for(std::map<std::string, std::string>::const_iterator it = config->name_equivalents.begin(); 00804 it != config->name_equivalents.end(); 00805 it++) { 00806 js_type::left_iterator lit = joint_state_equivalents_.left.find(it->first); 00807 if(lit != joint_state_equivalents_.left.end()) { 00808 joint_state_equivalents_.left.replace_data(lit, it->second); 00809 } 00810 } 00811 parent_frame_id_ = config->parent_frame_id; 00812 child_frame_id_ = config->child_frame_id; 00813 } 00814 unsigned int i = 0; 00815 for(std::vector<std::string>::const_iterator it = local_joint_names.begin(); 00816 it != local_joint_names.end(); 00817 it++, i++) { 00818 computation_order_map_index_[i] = joint_state_equivalents_.left.at(*it); 00819 } 00820 00821 for(js_type::iterator it = joint_state_equivalents_.begin(); 00822 it != joint_state_equivalents_.end(); 00823 it++) { 00824 setVariableBounds(it->right,-DBL_MAX,DBL_MAX); 00825 } 00826 } 00827 00828 planning_models::KinematicModel::JointModel::JointModel(const JointModel* joint) { 00829 name_ = joint->name_; 00830 parent_frame_id_ = joint->parent_frame_id_; 00831 child_frame_id_ = joint->child_frame_id_; 00832 joint_state_equivalents_ = joint->joint_state_equivalents_; 00833 joint_state_bounds_ = joint->joint_state_bounds_; 00834 } 00835 00836 planning_models::KinematicModel::JointModel::~JointModel(void) 00837 { 00838 if (child_link_model_) 00839 delete child_link_model_; 00840 } 00841 00842 std::string planning_models::KinematicModel::JointModel::getEquiv(const std::string& name) const { 00843 js_type::left_const_iterator lit = joint_state_equivalents_.left.find(name); 00844 if(lit != joint_state_equivalents_.left.end()) { 00845 return lit->second; 00846 } else { 00847 return ""; 00848 } 00849 } 00850 00851 bool planning_models::KinematicModel::JointModel::setVariableBounds(const std::string& variable, double low, double high) { 00852 if(joint_state_equivalents_.right.find(variable) == joint_state_equivalents_.right.end()) { 00853 ROS_WARN_STREAM("Can't find variable " << variable << " to set bounds"); 00854 return false; 00855 } 00856 joint_state_bounds_[joint_state_equivalents_.right.at(variable)] = std::pair<double,double>(low, high); 00857 return true; 00858 } 00859 00860 bool planning_models::KinematicModel::JointModel::getVariableBounds(const std::string& variable, std::pair<double, double>& bounds) const{ 00861 if(joint_state_equivalents_.right.find(variable) == joint_state_equivalents_.right.end()) { 00862 ROS_WARN_STREAM("Can't find variable " << variable << " to get bounds"); 00863 return false; 00864 } 00865 std::string config_name = joint_state_equivalents_.right.find(variable)->second; 00866 if(joint_state_bounds_.find(config_name) == joint_state_bounds_.end()) { 00867 ROS_WARN_STREAM("No joint bounds for " << config_name); 00868 return false; 00869 } 00870 bounds = joint_state_bounds_.find(config_name)->second; 00871 return true; 00872 } 00873 00874 void planning_models::KinematicModel::JointModel::getVariableDefaultValuesGivenBounds(std::map<std::string, double>& ret_map) const 00875 { 00876 for(std::map<std::string, std::pair<double, double> >::const_iterator it = joint_state_bounds_.begin(); 00877 it != joint_state_bounds_.end(); 00878 it++) { 00879 //zero is a valid value 00880 if(it->second.first <= 0.0 && it->second.second >= 0.0) { 00881 ret_map[it->first] = 0.0; 00882 } else { 00883 ret_map[it->first] = (it->second.first + it->second.second)/2.0; 00884 } 00885 } 00886 } 00887 00888 bool planning_models::KinematicModel::JointModel::isValueWithinVariableBounds(const std::string& variable, const double& value, bool& within_bounds) const 00889 { 00890 std::pair<double, double> bounds; 00891 if(!getVariableBounds(variable, bounds)) { 00892 return false; 00893 } 00894 if(value < bounds.first || value > bounds.second) { 00895 ROS_DEBUG_STREAM("Violates bounds: Value " << value << " lower " << bounds.first << " upper " << bounds.second); 00896 within_bounds = false; 00897 } else { 00898 ROS_DEBUG_STREAM("Satisfies bounds: Value " << value << " lower " << bounds.first << " upper " << bounds.second); 00899 within_bounds = true; 00900 } 00901 return true; 00902 } 00903 00904 planning_models::KinematicModel::PlanarJointModel::PlanarJointModel(const std::string& name, const MultiDofConfig* multi_dof_config) 00905 : JointModel(name) 00906 { 00907 if(multi_dof_config == NULL) { 00908 ROS_WARN("Planar joint needs a config"); 00909 return; 00910 } 00911 std::vector<std::string> local_names; 00912 local_names.push_back("planar_x"); 00913 local_names.push_back("planar_y"); 00914 local_names.push_back("planar_th"); 00915 initialize(local_names, multi_dof_config); 00916 setVariableBounds(getEquiv("planar_x"),-DBL_MAX,DBL_MAX); 00917 setVariableBounds(getEquiv("planar_y"),-DBL_MAX,DBL_MAX); 00918 setVariableBounds(getEquiv("planar_th"),-M_PI,M_PI); 00919 } 00920 00921 btTransform planning_models::KinematicModel::PlanarJointModel::computeTransform(const std::vector<double>& joint_values) const 00922 { 00923 btTransform variable_transform; 00924 variable_transform.setIdentity(); 00925 if(joint_values.size() != 3) { 00926 ROS_ERROR("Planar joint given too few values"); 00927 return variable_transform; 00928 } 00929 variable_transform.setOrigin(btVector3(joint_values[0], 00930 joint_values[1], 00931 0.0)); 00932 variable_transform.setRotation(btQuaternion(btVector3(0.0, 0.0, 1.0), 00933 joint_values[2])); 00934 return variable_transform; 00935 } 00936 00937 std::vector<double> planning_models::KinematicModel::PlanarJointModel::computeJointStateValues(const btTransform& transform) const 00938 { 00939 std::vector<double> ret; 00940 ret.push_back(transform.getOrigin().x()); 00941 ret.push_back(transform.getOrigin().y()); 00942 ret.push_back(transform.getRotation().getAngle()*transform.getRotation().getAxis().z()); 00943 return ret; 00944 } 00945 00946 planning_models::KinematicModel::FloatingJointModel::FloatingJointModel(const std::string& name, 00947 const MultiDofConfig* multi_dof_config) 00948 : JointModel(name) 00949 { 00950 if(multi_dof_config == NULL) { 00951 ROS_WARN("Planar joint needs a config"); 00952 return; 00953 } 00954 std::vector<std::string> local_names; 00955 local_names.push_back("floating_trans_x"); 00956 local_names.push_back("floating_trans_y"); 00957 local_names.push_back("floating_trans_z"); 00958 local_names.push_back("floating_rot_x"); 00959 local_names.push_back("floating_rot_y"); 00960 local_names.push_back("floating_rot_z"); 00961 local_names.push_back("floating_rot_w"); 00962 initialize(local_names, multi_dof_config); 00963 00964 setVariableBounds(getEquiv("floating_trans_x"),-DBL_MAX,DBL_MAX); 00965 setVariableBounds(getEquiv("floating_trans_y"),-DBL_MAX,DBL_MAX); 00966 setVariableBounds(getEquiv("floating_trans_z"),-DBL_MAX,DBL_MAX); 00967 setVariableBounds(getEquiv("floating_rot_x"),-1.0,1.0); 00968 setVariableBounds(getEquiv("floating_rot_y"),-1.0,1.0); 00969 setVariableBounds(getEquiv("floating_rot_z"),-1.0,1.0); 00970 setVariableBounds(getEquiv("floating_rot_w"),-1.0,1.0); 00971 } 00972 00973 btTransform planning_models::KinematicModel::FloatingJointModel::computeTransform(const std::vector<double>& joint_values) const 00974 { 00975 btTransform variable_transform; 00976 variable_transform.setIdentity(); 00977 if(joint_values.size() != 7) { 00978 ROS_ERROR("Floating joint given too few values"); 00979 return variable_transform; 00980 } 00981 variable_transform.setOrigin(btVector3(joint_values[0], joint_values[1], joint_values[2])); 00982 variable_transform.setRotation(btQuaternion(joint_values[3], joint_values[4], joint_values[5], joint_values[6])); 00983 if(joint_values[3] == 0.0 && joint_values[4] == 0.0 && joint_values[5] == 0.0 && joint_values[6] == 0.0) { 00984 ROS_INFO("Setting quaternion with all zeros"); 00985 } 00986 return variable_transform; 00987 } 00988 00989 std::vector<double> planning_models::KinematicModel::FloatingJointModel::computeJointStateValues(const btTransform& transform) const 00990 { 00991 std::vector<double> ret; 00992 ret.push_back(transform.getOrigin().x()); 00993 ret.push_back(transform.getOrigin().y()); 00994 ret.push_back(transform.getOrigin().z()); 00995 ret.push_back(transform.getRotation().x()); 00996 ret.push_back(transform.getRotation().y()); 00997 ret.push_back(transform.getRotation().z()); 00998 ret.push_back(transform.getRotation().w()); 00999 return ret; 01000 } 01001 01002 void planning_models::KinematicModel::FloatingJointModel::getVariableDefaultValuesGivenBounds(std::map<std::string, double>& ret_map) const 01003 { 01004 //the only reasonable default quaternion is (0,0,0,1) - doesn't matter what the bounds are 01005 ret_map[getEquiv("floating_trans_x")] = 0.0; 01006 ret_map[getEquiv("floating_trans_y")] = 0.0; 01007 ret_map[getEquiv("floating_trans_z")] = 0.0; 01008 ret_map[getEquiv("floating_rot_x")] = 0.0; 01009 ret_map[getEquiv("floating_rot_y")] = 0.0; 01010 ret_map[getEquiv("floating_rot_z")] = 0.0; 01011 ret_map[getEquiv("floating_rot_w")] = 1.0; 01012 } 01013 01014 planning_models::KinematicModel::PrismaticJointModel::PrismaticJointModel(const std::string& name, 01015 const MultiDofConfig* multi_dof_config) 01016 : JointModel(name), 01017 axis_(0.0, 0.0, 0.0) 01018 { 01019 std::vector<std::string> local_names(1,name); 01020 initialize(local_names, multi_dof_config); 01021 } 01022 01023 btTransform planning_models::KinematicModel::PrismaticJointModel::computeTransform(const std::vector<double>& joint_values) const 01024 { 01025 btTransform variable_transform; 01026 variable_transform.setIdentity(); 01027 if(joint_values.size() != 1) { 01028 ROS_ERROR("Prismatic joint given wrong number of values"); 01029 return variable_transform; 01030 } 01031 variable_transform.setOrigin(axis_*joint_values[0]); 01032 return variable_transform; 01033 } 01034 01035 std::vector<double> planning_models::KinematicModel::PrismaticJointModel::computeJointStateValues(const btTransform& transform) const 01036 { 01037 std::vector<double> ret; 01038 ret.push_back(transform.getOrigin().dot(axis_)); 01039 return ret; 01040 } 01041 01042 planning_models::KinematicModel::RevoluteJointModel::RevoluteJointModel(const std::string& name, 01043 const MultiDofConfig* multi_dof_config) 01044 : JointModel(name), 01045 axis_(0.0, 0.0, 0.0), 01046 continuous_(false) 01047 { 01048 std::vector<std::string> local_names(1,name); 01049 initialize(local_names, multi_dof_config); 01050 } 01051 01052 btTransform planning_models::KinematicModel::RevoluteJointModel::computeTransform(const std::vector<double>& joint_values) const 01053 { 01054 btTransform variable_transform; 01055 variable_transform.setIdentity(); 01056 if(joint_values.size() != 1) { 01057 ROS_ERROR("Revolute joint given wrong number of values"); 01058 return variable_transform; 01059 } 01060 double val = joint_values.front(); 01061 if(continuous_) { 01062 val = angles::normalize_angle(val); 01063 } 01064 variable_transform.setRotation(btQuaternion(axis_,val)); 01065 return variable_transform; 01066 } 01067 01068 std::vector<double> planning_models::KinematicModel::RevoluteJointModel::computeJointStateValues(const btTransform& transform) const 01069 { 01070 std::vector<double> ret; 01071 ROS_DEBUG_STREAM("Transform angle is " << transform.getRotation().getAngle() 01072 << " axis x " << transform.getRotation().getAxis().x() 01073 << " axis y " << transform.getRotation().getAxis().y() 01074 << " axis z " << transform.getRotation().getAxis().z()); 01075 double val = transform.getRotation().getAngle()*transform.getRotation().getAxis().dot(axis_); 01076 while(val < -M_PI) { 01077 if(val < -M_PI) { 01078 val += 2*M_PI; 01079 } 01080 } 01081 while(val > M_PI) { 01082 if(val > M_PI) { 01083 val -= 2*M_PI; 01084 } 01085 } 01086 ret.push_back(val); 01087 return ret; 01088 } 01089 01090 bool planning_models::KinematicModel::RevoluteJointModel::isValueWithinVariableBounds(const std::string& variable, 01091 const double& value, 01092 bool& within_bounds) const 01093 { 01094 if(!continuous_) { 01095 return JointModel::isValueWithinVariableBounds(variable, value, within_bounds); 01096 } 01097 if(!hasVariable(variable)) { 01098 return false; 01099 } 01100 within_bounds = true; 01101 return true; 01102 } 01103 01104 /* ------------------------ LinkModel ------------------------ */ 01105 01106 planning_models::KinematicModel::LinkModel::LinkModel(const KinematicModel* kinematic_model) : 01107 kinematic_model_(kinematic_model), 01108 parent_joint_model_(NULL), 01109 shape_(NULL) 01110 { 01111 joint_origin_transform_.setIdentity(); 01112 collision_origin_transform_.setIdentity(); 01113 } 01114 01115 planning_models::KinematicModel::LinkModel::LinkModel(const LinkModel* link_model) : 01116 name_(link_model->name_), 01117 kinematic_model_(link_model->kinematic_model_), 01118 joint_origin_transform_(link_model->joint_origin_transform_), 01119 collision_origin_transform_(link_model->collision_origin_transform_) 01120 { 01121 if(link_model->shape_) { 01122 shape_ = shapes::cloneShape(link_model->shape_); 01123 } else { 01124 shape_ = NULL; 01125 } 01126 for (unsigned int i = 0 ; i < link_model->attached_body_models_.size() ; ++i) 01127 { 01128 std::vector<shapes::Shape*> shapes; 01129 for(unsigned int j = 0; j < link_model->attached_body_models_[i]->getShapes().size(); j++) { 01130 shapes.push_back(shapes::cloneShape(link_model->attached_body_models_[i]->getShapes()[j])); 01131 } 01132 AttachedBodyModel *ab = new AttachedBodyModel(this, 01133 link_model->attached_body_models_[i]->getName(), 01134 link_model->attached_body_models_[i]->getAttachedBodyFixedTransforms(), 01135 link_model->attached_body_models_[i]->getTouchLinks(), 01136 shapes); 01137 attached_body_models_.push_back(ab); 01138 } 01139 } 01140 01141 planning_models::KinematicModel::LinkModel::~LinkModel(void) 01142 { 01143 if (shape_) 01144 delete shape_; 01145 for (unsigned int i = 0 ; i < child_joint_models_.size() ; ++i) 01146 delete child_joint_models_[i]; 01147 for (unsigned int i = 0 ; i < attached_body_models_.size() ; ++i) 01148 delete attached_body_models_[i]; 01149 } 01150 01151 void planning_models::KinematicModel::LinkModel::clearAttachedBodyModels() 01152 { 01153 //assumes exclusive lock has been granted 01154 for (unsigned int i = 0 ; i < attached_body_models_.size() ; ++i) 01155 delete attached_body_models_[i]; 01156 attached_body_models_.clear(); 01157 } 01158 01159 void planning_models::KinematicModel::LinkModel::replaceAttachedBodyModels(std::vector<AttachedBodyModel*>& attached_body_vector) 01160 { 01161 //assumes exclusive lock has been granted 01162 for (unsigned int i = 0 ; i < attached_body_models_.size() ; ++i) 01163 delete attached_body_models_[i]; 01164 attached_body_models_.clear(); 01165 01166 attached_body_models_ = attached_body_vector; 01167 } 01168 01169 void planning_models::KinematicModel::LinkModel::clearLinkAttachedBodyModel(const std::string& att_name) 01170 { 01171 for(std::vector<AttachedBodyModel*>::iterator it = attached_body_models_.begin(); 01172 it != attached_body_models_.end(); 01173 it++) { 01174 if((*it)->getName() == att_name) { 01175 delete (*it); 01176 attached_body_models_.erase(it); 01177 return; 01178 } 01179 } 01180 } 01181 01182 void planning_models::KinematicModel::LinkModel::addAttachedBodyModel(planning_models::KinematicModel::AttachedBodyModel* ab) 01183 { 01184 attached_body_models_.push_back(ab); 01185 } 01186 01187 /* ------------------------ AttachedBodyModel ------------------------ */ 01188 01189 planning_models::KinematicModel::AttachedBodyModel::AttachedBodyModel(const LinkModel *link, 01190 const std::string& nid, 01191 const std::vector<btTransform>& attach_trans, 01192 const std::vector<std::string>& touch_links, 01193 std::vector<shapes::Shape*>& shapes) 01194 01195 : attached_link_model_(link), 01196 id_(nid) 01197 { 01198 attach_trans_ = attach_trans; 01199 touch_links_ = touch_links; 01200 shapes_ = shapes; 01201 } 01202 01203 planning_models::KinematicModel::AttachedBodyModel::~AttachedBodyModel(void) 01204 { 01205 for(unsigned int i = 0; i < shapes_.size(); i++) { 01206 delete shapes_[i]; 01207 } 01208 } 01209 01210 /* ------------------------ JointModelGroup ------------------------ */ 01211 planning_models::KinematicModel::JointModelGroup::JointModelGroup(const std::string& group_name, 01212 const std::vector<const JointModel*> &group_joints, 01213 const std::vector<const JointModel*> &fixed_group_joints, 01214 const KinematicModel* parent_model) : 01215 name_(group_name) 01216 { 01217 ROS_DEBUG_STREAM("Group " << group_name); 01218 01219 joint_model_vector_ = group_joints; 01220 fixed_joint_model_vector_ = fixed_group_joints; 01221 joint_model_name_vector_.resize(group_joints.size()); 01222 01223 ROS_DEBUG_STREAM("Joints:"); 01224 for (unsigned int i = 0 ; i < group_joints.size() ; ++i) 01225 { 01226 ROS_DEBUG_STREAM(group_joints[i]->getName()); 01227 joint_model_name_vector_[i] = group_joints[i]->getName(); 01228 joint_model_map_[group_joints[i]->getName()] = group_joints[i]; 01229 } 01230 01231 ROS_DEBUG_STREAM("Fixed joints:"); 01232 for (unsigned int i = 0 ; i < fixed_joint_model_vector_.size() ; ++i) 01233 { 01234 ROS_DEBUG_STREAM(fixed_joint_model_vector_[i]->getName()); 01235 } 01236 01237 std::vector<const JointModel*> all_joints = group_joints; 01238 all_joints.insert(all_joints.end(), fixed_group_joints.begin(), fixed_group_joints.end()); 01239 01240 //now we need to find all the set of joints within this group 01241 //that root distinct subtrees 01242 std::map<std::string, bool> is_root; 01243 for (unsigned int i = 0 ; i < all_joints.size() ; ++i) 01244 { 01245 bool found = false; 01246 const JointModel *joint = all_joints[i]; 01247 //if we find that an ancestor is also in the group, then the joint is not a root 01248 while (joint->getParentLinkModel() != NULL) 01249 { 01250 joint = joint->getParentLinkModel()->getParentJointModel(); 01251 if (hasJointModel(joint->name_)) 01252 { 01253 found = true; 01254 } 01255 } 01256 01257 if(!found) { 01258 joint_roots_.push_back(all_joints[i]); 01259 is_root[all_joints[i]->getName()] = true; 01260 } else { 01261 is_root[all_joints[i]->getName()] = false; 01262 } 01263 } 01264 01265 //now we need to make another pass for group links 01266 std::set<const LinkModel*> group_links_set; 01267 for(unsigned int i = 0; i < all_joints.size(); i++) { 01268 const JointModel *joint = all_joints[i]; 01269 //push children in directly 01270 group_links_set.insert(joint->getChildLinkModel()); 01271 while (joint->getParentLinkModel() != NULL) 01272 { 01273 if(is_root.find(joint->getName()) != is_root.end() && 01274 is_root[joint->getName()]) { 01275 break; 01276 } 01277 group_links_set.insert(joint->getParentLinkModel()); 01278 joint = joint->getParentLinkModel()->getParentJointModel(); 01279 } 01280 } 01281 01282 ROS_DEBUG("Group links:"); 01283 for(std::set<const LinkModel*>::iterator it = group_links_set.begin(); 01284 it != group_links_set.end(); 01285 it++) { 01286 group_link_model_vector_.push_back(*it); 01287 ROS_DEBUG_STREAM((*it)->getName()); 01288 } 01289 //these subtrees are distinct, so we can stack their updated links on top of each other 01290 for (unsigned int i = 0 ; i < joint_roots_.size() ; ++i) 01291 { 01292 std::vector<const LinkModel*> links; 01293 parent_model->getChildLinkModels(joint_roots_[i], links); 01294 updated_link_model_vector_.insert(updated_link_model_vector_.end(), links.begin(), links.end()); 01295 } 01296 } 01297 01298 planning_models::KinematicModel::JointModelGroup::~JointModelGroup(void) 01299 { 01300 } 01301 01302 bool planning_models::KinematicModel::JointModelGroup::hasJointModel(const std::string &joint) const 01303 { 01304 return joint_model_map_.find(joint) != joint_model_map_.end(); 01305 } 01306 01307 const planning_models::KinematicModel::JointModel* planning_models::KinematicModel::JointModelGroup::getJointModel(const std::string &name) 01308 { 01309 if(!hasJointModel(name)) return NULL; 01310 return joint_model_map_.find(name)->second; 01311 } 01312