$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, 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 <planning_models/kinematic_state.h> 00038 00039 planning_models::KinematicState::KinematicState(const KinematicModel* kinematic_model) : 00040 kinematic_model_(kinematic_model), dimension_(0) 00041 { 00042 kinematic_model_->sharedLock(); 00043 00044 const std::vector<KinematicModel::JointModel*>& joint_model_vector = kinematic_model_->getJointModels(); 00045 joint_state_vector_.resize(joint_model_vector.size()); 00046 //joint_index_location_.resize(joint_model_vector.size()); 00047 unsigned int vector_index_counter = 0; 00048 for(unsigned int i = 0; i < joint_model_vector.size(); i++) { 00049 joint_state_vector_[i] = new JointState(joint_model_vector[i]); 00050 joint_state_map_[joint_state_vector_[i]->getName()] = joint_state_vector_[i]; 00051 unsigned int joint_dim = joint_state_vector_[i]->getDimension(); 00052 dimension_ += joint_dim; 00053 const std::vector<std::string>& name_order = joint_state_vector_[i]->getJointStateNameOrder(); 00054 for(unsigned int j = 0; j < name_order.size(); j++) { 00055 joint_state_map_[name_order[j]] = joint_state_vector_[i]; 00056 kinematic_state_index_map_[name_order[j]] = vector_index_counter+j; 00057 } 00058 vector_index_counter += joint_dim; 00059 } 00060 const std::vector<KinematicModel::LinkModel*>& link_model_vector = kinematic_model_->getLinkModels(); 00061 link_state_vector_.resize(link_model_vector.size()); 00062 for(unsigned int i = 0; i < link_model_vector.size(); i++) { 00063 link_state_vector_[i] = new LinkState(link_model_vector[i]); 00064 link_state_map_[link_state_vector_[i]->getName()] = link_state_vector_[i]; 00065 for(unsigned int j = 0; j < link_state_vector_[i]->getAttachedBodyStateVector().size(); j++) { 00066 attached_body_state_vector_.push_back(link_state_vector_[i]->getAttachedBodyStateVector()[j]); 00067 } 00068 } 00069 setLinkStatesParents(); 00070 00071 //now make joint_state_groups 00072 const std::map<std::string,KinematicModel::JointModelGroup*>& joint_model_group_map = kinematic_model_->getJointModelGroupMap(); 00073 for(std::map<std::string,KinematicModel::JointModelGroup*>::const_iterator it = joint_model_group_map.begin(); 00074 it != joint_model_group_map.end(); 00075 it++) { 00076 joint_state_group_map_[it->first] = new JointStateGroup(it->second, this); 00077 } 00078 } 00079 00080 planning_models::KinematicState::KinematicState(const KinematicState& ks) : 00081 kinematic_model_(ks.getKinematicModel()), dimension_(0) 00082 { 00083 kinematic_model_->sharedLock(); 00084 const std::vector<JointState*>& joint_state_vector = ks.getJointStateVector(); 00085 unsigned int vector_index_counter = 0; 00086 joint_state_vector_.resize(joint_state_vector.size()); 00087 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00088 joint_state_vector_[i] = new JointState(joint_state_vector[i]->getJointModel()); 00089 joint_state_map_[joint_state_vector_[i]->getName()] = joint_state_vector_[i]; 00090 const std::vector<std::string>& name_order = joint_state_vector_[i]->getJointStateNameOrder(); 00091 for(unsigned int j = 0; j < name_order.size(); j++) { 00092 joint_state_map_[name_order[j]] = joint_state_vector_[i]; 00093 kinematic_state_index_map_[name_order[j]] = vector_index_counter+j; 00094 } 00095 unsigned int joint_dim = joint_state_vector_[i]->getDimension(); 00096 dimension_ += joint_dim; 00097 vector_index_counter += joint_dim; 00098 //joint_index_location_[i] = vector_index_counter; 00099 //vector_index_counter += joint_dim; 00100 } 00101 const std::vector<LinkState*>& link_state_vector = ks.getLinkStateVector(); 00102 link_state_vector_.resize(link_state_vector.size()); 00103 for(unsigned int i = 0; i < link_state_vector.size(); i++) { 00104 link_state_vector_[i] = new LinkState(link_state_vector[i]->getLinkModel()); 00105 link_state_map_[link_state_vector_[i]->getName()] = link_state_vector_[i]; 00106 for(unsigned int j = 0; j < link_state_vector_[i]->getAttachedBodyStateVector().size(); j++) { 00107 attached_body_state_vector_.push_back(link_state_vector_[i]->getAttachedBodyStateVector()[j]); 00108 } 00109 } 00110 setLinkStatesParents(); 00111 00112 const std::map<std::string, JointStateGroup*>& joint_state_groups_map = ks.getJointStateGroupMap(); 00113 for(std::map<std::string, JointStateGroup*>::const_iterator it = joint_state_groups_map.begin(); 00114 it != joint_state_groups_map.end(); 00115 it++) { 00116 joint_state_group_map_[it->first] = new JointStateGroup(it->second->getJointModelGroup(), this); 00117 } 00118 //actually setting values 00119 std::map<std::string, double> current_joint_values; 00120 ks.getKinematicStateValues(current_joint_values); 00121 setKinematicState(current_joint_values); 00122 } 00123 00124 planning_models::KinematicState::~KinematicState() 00125 { 00126 kinematic_model_->sharedUnlock(); 00127 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00128 delete joint_state_vector_[i]; 00129 } 00130 for(unsigned int i = 0; i < link_state_vector_.size(); i++) { 00131 delete link_state_vector_[i]; 00132 } 00133 for(std::map<std::string, JointStateGroup*>::iterator it = joint_state_group_map_.begin(); 00134 it != joint_state_group_map_.end(); 00135 it++) { 00136 delete it->second; 00137 } 00138 } 00139 00140 bool planning_models::KinematicState::setKinematicState(const std::vector<double>& joint_state_values) { 00141 if(joint_state_values.size() != dimension_) return false; 00142 unsigned int value_counter = 0; 00143 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00144 unsigned int dim = joint_state_vector_[i]->getDimension(); 00145 if(dim != 0) { 00146 std::vector<double> vec(dim, 0.0); 00147 copy(joint_state_values.begin()+value_counter, 00148 joint_state_values.begin()+value_counter+dim, 00149 vec.begin()); 00150 bool ok = joint_state_vector_[i]->setJointStateValues(vec); 00151 if(!ok) { 00152 ROS_WARN("Joint state unhappy"); 00153 } 00154 value_counter += dim; 00155 } 00156 } 00157 updateKinematicLinks(); 00158 return true; 00159 } 00160 00161 bool planning_models::KinematicState::setKinematicState(const std::map<std::string, double>& joint_state_map) 00162 { 00163 bool all_set = true; 00164 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00165 bool is_set = joint_state_vector_[i]->setJointStateValues(joint_state_map); 00166 if(!is_set) all_set = false; 00167 } 00168 updateKinematicLinks(); 00169 return all_set; 00170 } 00171 00172 bool planning_models::KinematicState::setKinematicState(const std::map<std::string, double>& joint_state_map, 00173 std::vector<std::string>& missing_states) 00174 { 00175 missing_states.clear(); 00176 bool all_set = true; 00177 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00178 bool is_set = joint_state_vector_[i]->setJointStateValues(joint_state_map, missing_states); 00179 if(!is_set) { 00180 all_set = false; 00181 } 00182 } 00183 updateKinematicLinks(); 00184 return all_set; 00185 } 00186 00187 void planning_models::KinematicState::getKinematicStateValues(std::vector<double>& joint_state_values) const { 00188 joint_state_values.clear(); 00189 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00190 unsigned int dim = joint_state_vector_[i]->getDimension(); 00191 if(dim != 0) { 00192 for(unsigned int j = 0; j < joint_state_vector_[i]->getJointStateValues().size(); j++) { 00193 joint_state_values.push_back(joint_state_vector_[i]->getJointStateValues()[j]); 00194 } 00195 } 00196 } 00197 if(joint_state_values.size() != dimension_) { 00198 ROS_WARN_STREAM("Some problems with state vector dimension values " << joint_state_values.size() << " dimension is " << dimension_); 00199 } 00200 } 00201 00202 void planning_models::KinematicState::getKinematicStateValues(std::map<std::string,double>& joint_state_values) const { 00203 joint_state_values.clear(); 00204 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00205 unsigned int dim = joint_state_vector_[i]->getDimension(); 00206 if(dim != 0) { 00207 for(unsigned int j = 0; j < joint_state_vector_[i]->getJointStateValues().size(); j++) { 00208 joint_state_values[joint_state_vector_[i]->getJointStateNameOrder()[j]] = joint_state_vector_[i]->getJointStateValues()[j]; 00209 } 00210 } 00211 } 00212 if(joint_state_values.size() != dimension_) { 00213 ROS_WARN_STREAM("Some problems with state map dimension values " << joint_state_values.size() << " dimension is " << dimension_); 00214 } 00215 } 00216 00217 void planning_models::KinematicState::updateKinematicLinks() 00218 { 00219 for(unsigned int i = 0; i < link_state_vector_.size(); i++) { 00220 link_state_vector_[i]->computeTransform(); 00221 } 00222 } 00223 00224 bool planning_models::KinematicState::updateKinematicStateWithLinkAt(const std::string& link_name, const btTransform& transform) 00225 { 00226 if(!hasLinkState(link_name)) return false; 00227 00228 link_state_map_.find(link_name)->second->updateGivenGlobalLinkTransform(transform); 00229 std::vector<LinkState*> child_links = getChildLinkStates(link_name); 00230 //the zeroith link will be the link itself, which shouldn't be getting updated, so we start at 1 00231 for(unsigned int i = 1; i < child_links.size(); i++) { 00232 child_links[i]->computeTransform(); 00233 } 00234 return true; 00235 } 00236 00237 const btTransform& planning_models::KinematicState::getRootTransform() const 00238 { 00239 return joint_state_vector_[0]->getVariableTransform(); 00240 } 00241 00242 std::vector<planning_models::KinematicState::LinkState*> planning_models::KinematicState::getChildLinkStates(const std::string& link_name) const { 00243 std::vector<LinkState*> child_link_states; 00244 std::vector<const KinematicModel::LinkModel*> child_link_models; 00245 kinematic_model_->getChildLinkModels(kinematic_model_->getLinkModel(link_name), child_link_models); 00246 for(unsigned int i = 0; i < child_link_models.size(); i++) { 00247 child_link_states.push_back(link_state_map_.find((child_link_models[i]->getName()))->second); 00248 } 00249 return child_link_states; 00250 } 00251 00252 void planning_models::KinematicState::setKinematicStateToDefault(void) 00253 { 00254 std::map<std::string, double> default_joint_states; 00255 00256 const unsigned int js = joint_state_vector_.size(); 00257 for (unsigned int i = 0 ; i < js ; ++i) 00258 { 00259 joint_state_vector_[i]->getJointModel()->getVariableDefaultValuesGivenBounds(default_joint_states); 00260 } 00261 setKinematicState(default_joint_states); 00262 } 00263 00264 bool planning_models::KinematicState::isJointWithinBounds(const std::string& joint) const { 00265 const JointState* joint_state = getJointState(joint); 00266 if(joint_state == NULL) { 00267 ROS_WARN_STREAM("No joint with name " << joint); 00268 return false; 00269 } 00270 return(joint_state->areJointStateValuesWithinBounds()); 00271 } 00272 00273 bool planning_models::KinematicState::areJointsWithinBounds(const std::vector<std::string>& joints) const 00274 { 00275 for(std::vector<std::string>::const_iterator it = joints.begin(); 00276 it != joints.end(); 00277 it++) { 00278 const JointState* joint_state = getJointState(*it); 00279 if(joint_state == NULL) { 00280 ROS_WARN_STREAM("No joint with name " << *it); 00281 return false; 00282 } 00283 if(!joint_state->areJointStateValuesWithinBounds()) { 00284 return false; 00285 } 00286 } 00287 return true; 00288 } 00289 00290 void planning_models::KinematicState::setLinkStatesParents() { 00291 //now we need to figure out who are the link parents are 00292 for(unsigned int i = 0; i < link_state_vector_.size(); i++) { 00293 const KinematicModel::JointModel* parent_joint_model = link_state_vector_[i]->getLinkModel()->getParentJointModel(); 00294 if(parent_joint_model == NULL) { 00295 ROS_WARN("Parent joint really should be NULL"); 00296 continue; 00297 } 00298 if(joint_state_map_.find(parent_joint_model->getName()) == joint_state_map_.end()) { 00299 ROS_WARN_STREAM("Don't have a joint state for parent joint " << parent_joint_model->getName()); 00300 continue; 00301 } 00302 link_state_vector_[i]->setParentJointState(joint_state_map_[parent_joint_model->getName()]); 00303 if(parent_joint_model->getParentLinkModel() != NULL) { 00304 if(link_state_map_.find(parent_joint_model->getParentLinkModel()->getName()) == link_state_map_.end()) { 00305 ROS_WARN_STREAM("Don't have a link state for parent link " << parent_joint_model->getParentLinkModel()->getName()); 00306 continue; 00307 } 00308 link_state_vector_[i]->setParentLinkState(link_state_map_[parent_joint_model->getParentLinkModel()->getName()]); 00309 } 00310 } 00311 } 00312 00313 const planning_models::KinematicState::JointStateGroup* planning_models::KinematicState::getJointStateGroup(const std::string &name) const 00314 { 00315 if(joint_state_group_map_.find(name) == joint_state_group_map_.end()) return NULL; 00316 return joint_state_group_map_.find(name)->second; 00317 } 00318 00319 planning_models::KinematicState::JointStateGroup* planning_models::KinematicState::getJointStateGroup(const std::string &name) 00320 { 00321 if(joint_state_group_map_.find(name) == joint_state_group_map_.end()) return NULL; 00322 return joint_state_group_map_.find(name)->second; 00323 } 00324 00325 bool planning_models::KinematicState::hasJointStateGroup(const std::string &name) const 00326 { 00327 return(joint_state_group_map_.find(name) != joint_state_group_map_.end()); 00328 } 00329 00330 void planning_models::KinematicState::getJointStateGroupNames(std::vector<std::string>& names) const 00331 { 00332 for(std::map<std::string, JointStateGroup*>::const_iterator it = joint_state_group_map_.begin(); 00333 it != joint_state_group_map_.end(); 00334 it++) { 00335 names.push_back(it->first); 00336 } 00337 } 00338 00339 bool planning_models::KinematicState::hasJointState(const std::string &joint) const 00340 { 00341 return joint_state_map_.find(joint) != joint_state_map_.end(); 00342 } 00343 bool planning_models::KinematicState::hasLinkState(const std::string& link) const 00344 { 00345 return (link_state_map_.find(link) != link_state_map_.end()); 00346 } 00347 00348 planning_models::KinematicState::JointState* planning_models::KinematicState::getJointState(const std::string &joint) const 00349 { 00350 if(!hasJointState(joint)) return NULL; 00351 return joint_state_map_.find(joint)->second; 00352 } 00353 00354 planning_models::KinematicState::LinkState* planning_models::KinematicState::getLinkState(const std::string &link) const 00355 { 00356 if(!hasLinkState(link)) return NULL; 00357 return link_state_map_.find(link)->second; 00358 } 00359 00360 planning_models::KinematicState::AttachedBodyState* planning_models::KinematicState::getAttachedBodyState(const std::string &att) const 00361 { 00362 for(unsigned int i = 0; i < link_state_vector_.size(); i++) { 00363 for(unsigned int j = 0; j < link_state_vector_[i]->getAttachedBodyStateVector().size(); j++) { 00364 if(link_state_vector_[i]->getAttachedBodyStateVector()[j]->getName() == att) { 00365 return (link_state_vector_[i]->getAttachedBodyStateVector()[j]); 00366 } 00367 } 00368 } 00369 return NULL; 00370 } 00371 00372 //-------------------- JointState --------------------- 00373 00374 planning_models::KinematicState::JointState::JointState(const planning_models::KinematicModel::JointModel* jm) : 00375 joint_model_(jm) 00376 { 00377 variable_transform_.setIdentity(); 00378 joint_state_values_ = joint_model_->computeJointStateValues(variable_transform_); 00379 joint_state_name_order_.resize(joint_state_values_.size()); 00380 00381 unsigned int i = 0; 00382 for(std::map<unsigned int, std::string>::const_iterator it = joint_model_->getComputatationOrderMapIndex().begin(); 00383 it != joint_model_->getComputatationOrderMapIndex().end(); 00384 it++,i++) { 00385 if(i != it->first) { 00386 ROS_WARN("Missing value"); 00387 continue; 00388 } 00389 joint_state_index_map_[it->second] = it->first; 00390 joint_state_name_order_[i] = it->second; 00391 } 00392 } 00393 00394 bool planning_models::KinematicState::JointState::setJointStateValues(const std::vector<double>& joint_state_values) { 00395 if(joint_state_values.size() != joint_state_index_map_.size()) { 00396 return false; 00397 } 00398 joint_state_values_ = joint_state_values; 00399 variable_transform_ = joint_model_->computeTransform(joint_state_values); 00400 return true; 00401 } 00402 00403 bool planning_models::KinematicState::JointState::setJointStateValues(const std::map<std::string, double>& joint_value_map) { 00404 bool has_all = true; 00405 bool has_any = false; 00406 for(std::map<std::string, unsigned int>::const_iterator it = joint_state_index_map_.begin(); 00407 it != joint_state_index_map_.end(); 00408 it++) { 00409 std::map<std::string,double>::const_iterator it2 = joint_value_map.find(it->first); 00410 if(it2 == joint_value_map.end()) { 00411 has_all = false; 00412 continue; 00413 } else { 00414 has_any = true; 00415 } 00416 if(it->second > joint_state_values_.size()) { 00417 ROS_WARN_STREAM("Trying to set value " << it->second << " which is larger than joint state values size " << joint_state_values_.size()); 00418 } else { 00419 joint_state_values_[it->second] = it2->second; 00420 } 00421 } 00422 if(has_any) { 00423 variable_transform_ = joint_model_->computeTransform(joint_state_values_); 00424 } 00425 return has_all; 00426 } 00427 00428 bool planning_models::KinematicState::JointState::setJointStateValues(const std::map<std::string, double>& joint_value_map, 00429 std::vector<std::string>& missing_values) { 00430 bool has_all = true; 00431 bool has_any = false; 00432 for(std::map<std::string, unsigned int>::const_iterator it = joint_state_index_map_.begin(); 00433 it != joint_state_index_map_.end(); 00434 it++) { 00435 std::map<std::string,double>::const_iterator it2 = joint_value_map.find(it->first); 00436 if(it2 == joint_value_map.end()) { 00437 has_all = false; 00438 missing_values.push_back(it->first); 00439 continue; 00440 } else { 00441 has_any = true; 00442 } 00443 if(it->second > joint_state_values_.size()) { 00444 ROS_WARN_STREAM("Trying to set value " << it->second << " which is larger than joint state values size " << joint_state_values_.size()); 00445 } else { 00446 joint_state_values_[it->second] = it2->second; 00447 } 00448 } 00449 if(has_any) { 00450 variable_transform_ = joint_model_->computeTransform(joint_state_values_); 00451 } 00452 return has_all; 00453 } 00454 00455 bool planning_models::KinematicState::JointState::setJointStateValues(const btTransform& transform) { 00456 variable_transform_ = transform; 00457 joint_state_values_ = joint_model_->computeJointStateValues(variable_transform_); 00458 return true; 00459 } 00460 00461 bool planning_models::KinematicState::JointState::allJointStateValuesAreDefined(const std::map<std::string, double>& joint_value_map) const { 00462 for(planning_models::KinematicModel::JointModel::js_type::const_iterator it = joint_model_->getJointStateEquivalents().begin(); 00463 it != joint_model_->getJointStateEquivalents().end(); 00464 it++) { 00465 if(joint_value_map.find(it->right) == joint_value_map.end()) { 00466 return false; 00467 } 00468 } 00469 return true; 00470 } 00471 00472 const std::vector<double>& planning_models::KinematicState::JointState::getJointStateValues() const { 00473 return joint_state_values_; 00474 } 00475 00476 const std::vector<std::string>& planning_models::KinematicState::JointState::getJointStateNameOrder() const { 00477 return joint_state_name_order_; 00478 } 00479 00480 bool planning_models::KinematicState::JointState::getJointValueBounds(const std::string& value_name, 00481 double& low, 00482 double& high) const { 00483 if(!joint_model_->hasVariable(value_name)) return false; 00484 std::pair<double,double> bounds; 00485 joint_model_->getVariableBounds(value_name, bounds); 00486 low = bounds.first; 00487 high = bounds.second; 00488 return true; 00489 } 00490 00491 const std::map<std::string, std::pair<double, double> >& planning_models::KinematicState::JointState::getAllJointValueBounds() const 00492 { 00493 return joint_model_->getAllVariableBounds(); 00494 } 00495 00496 bool planning_models::KinematicState::JointState::areJointStateValuesWithinBounds() const { 00497 for(std::map<std::string, unsigned int>::const_iterator it = joint_state_index_map_.begin(); 00498 it != joint_state_index_map_.end(); 00499 it++) { 00500 bool within_bounds; 00501 joint_model_->isValueWithinVariableBounds(it->first, joint_state_values_[it->second], within_bounds); 00502 if(!within_bounds) { 00503 ROS_DEBUG_STREAM("Joint " << it->first << " value " << joint_state_values_[it->second] << " not within bounds"); 00504 return false; 00505 } 00506 } 00507 return true; 00508 } 00509 00510 //-------------------- LinkState --------------------- 00511 00512 planning_models::KinematicState::LinkState::LinkState(const planning_models::KinematicModel::LinkModel* lm) : 00513 link_model_(lm), parent_joint_state_(NULL), parent_link_state_(NULL) 00514 { 00515 global_link_transform_.setIdentity(); 00516 global_collision_body_transform_.setIdentity(); 00517 const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_body_vector = link_model_->getAttachedBodyModels(); 00518 attached_body_state_vector_.resize(attached_body_vector.size()); 00519 unsigned int j = 0; 00520 for(std::vector<planning_models::KinematicModel::AttachedBodyModel*>::const_iterator it = attached_body_vector.begin(); 00521 it != attached_body_vector.end(); 00522 it++, j++) { 00523 attached_body_state_vector_[j] = new AttachedBodyState(*it, this); 00524 } 00525 } 00526 00527 planning_models::KinematicState::LinkState::~LinkState() 00528 { 00529 for(unsigned int i = 0; i < attached_body_state_vector_.size(); i++) { 00530 delete attached_body_state_vector_[i]; 00531 } 00532 } 00533 00534 void planning_models::KinematicState::LinkState::computeTransform() { 00535 btTransform ident; 00536 ident.setIdentity(); 00537 global_link_transform_.mult(parent_link_state_ ? parent_link_state_->global_link_transform_ : ident, link_model_->getJointOriginTransform()); 00538 global_link_transform_ *= parent_joint_state_->getVariableTransform(); 00539 global_collision_body_transform_.mult(global_link_transform_, link_model_->getCollisionOriginTransform()); 00540 updateAttachedBodies(); 00541 } 00542 00543 void planning_models::KinematicState::LinkState::updateAttachedBodies() 00544 { 00545 for (unsigned int i = 0 ; i < attached_body_state_vector_.size() ; ++i) { 00546 attached_body_state_vector_[i]->computeTransform(); 00547 } 00548 } 00549 00550 //-------------------- AttachedBodyState --------------------- 00551 00552 planning_models::KinematicState::AttachedBodyState::AttachedBodyState(const planning_models::KinematicModel::AttachedBodyModel* abm, 00553 const planning_models::KinematicState::LinkState* parent_link_state) : 00554 attached_body_model_(abm), 00555 parent_link_state_(parent_link_state) 00556 { 00557 global_collision_body_transforms_.resize(attached_body_model_->getAttachedBodyFixedTransforms().size()); 00558 for(unsigned int i = 0; i < attached_body_model_->getAttachedBodyFixedTransforms().size(); i++) { 00559 global_collision_body_transforms_[i].setIdentity(); 00560 } 00561 } 00562 00563 void planning_models::KinematicState::AttachedBodyState::computeTransform() 00564 { 00565 for(unsigned int i = 0; i < global_collision_body_transforms_.size(); i++) { 00566 global_collision_body_transforms_[i] = parent_link_state_->getGlobalLinkTransform() * attached_body_model_->getAttachedBodyFixedTransforms()[i]; 00567 } 00568 } 00569 00570 //--------------------- JointStateGroup -------------------------- 00571 00572 planning_models::KinematicState::JointStateGroup::JointStateGroup(const planning_models::KinematicModel::JointModelGroup* jmg, 00573 const planning_models::KinematicState* kinematic_state) : 00574 joint_model_group_(jmg), dimension_(0) 00575 { 00576 const std::vector<const KinematicModel::JointModel*>& joint_model_vector = jmg->getJointModels(); 00577 unsigned int vector_index_counter = 0; 00578 for(unsigned int i = 0; i < joint_model_vector.size(); i++) { 00579 if(!kinematic_state->hasJointState(joint_model_vector[i]->getName())) { 00580 ROS_WARN_STREAM("No joint state for group joint name " << joint_model_vector[i]->getName()); 00581 continue; 00582 } 00583 JointState* js = kinematic_state->getJointState(joint_model_vector[i]->getName()); 00584 joint_state_vector_.push_back(js); 00585 joint_names_.push_back(joint_model_vector[i]->getName()); 00586 joint_state_map_[joint_model_vector[i]->getName()] = js; 00587 unsigned int joint_dim = joint_state_vector_[i]->getDimension(); 00588 dimension_ += joint_dim; 00589 const std::vector<std::string>& name_order = joint_state_vector_[i]->getJointStateNameOrder(); 00590 for(unsigned int i = 0; i < name_order.size(); i++) { 00591 kinematic_state_index_map_[name_order[i]] = vector_index_counter+i; 00592 } 00593 vector_index_counter += joint_dim; 00594 } 00595 const std::vector<const KinematicModel::LinkModel*>& link_model_vector = jmg->getUpdatedLinkModels(); 00596 for(unsigned int i = 0; i < link_model_vector.size(); i++) { 00597 if(!kinematic_state->hasLinkState(link_model_vector[i]->getName())) { 00598 ROS_WARN_STREAM("No link state for link joint name " << link_model_vector[i]->getName()); 00599 continue; 00600 } 00601 LinkState* ls = kinematic_state->getLinkState(link_model_vector[i]->getName()); 00602 updated_links_.push_back(ls); 00603 } 00604 00605 const std::vector<const KinematicModel::JointModel*>& joint_root_vector = jmg->getJointRoots(); 00606 for(unsigned int i = 0; i < joint_root_vector.size(); i++) { 00607 joint_roots_.push_back(joint_state_map_[joint_root_vector[i]->getName()]); 00608 } 00609 } 00610 00611 bool planning_models::KinematicState::JointStateGroup::hasJointState(const std::string &joint) const 00612 { 00613 return joint_state_map_.find(joint) != joint_state_map_.end(); 00614 } 00615 bool planning_models::KinematicState::JointStateGroup::updatesLinkState(const std::string& link) const 00616 { 00617 for(unsigned int i = 0; i < updated_links_.size(); i++) { 00618 if(updated_links_[i]->getName() == link) return true; 00619 } 00620 return false; 00621 } 00622 00623 bool planning_models::KinematicState::JointStateGroup::setKinematicState(const std::vector<double>& joint_state_values) { 00624 if(joint_state_values.size() != dimension_) return false; 00625 unsigned int value_counter = 0; 00626 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00627 unsigned int dim = joint_state_vector_[i]->getDimension(); 00628 if(dim != 0) { 00629 std::vector<double> vec(dim); 00630 copy(joint_state_values.begin()+value_counter, 00631 joint_state_values.begin()+value_counter+dim, 00632 vec.begin()); 00633 bool ok = joint_state_vector_[i]->setJointStateValues(vec); 00634 if(!ok) { 00635 ROS_WARN("Joint state unhappy"); 00636 } 00637 value_counter += dim; 00638 } 00639 } 00640 updateKinematicLinks(); 00641 return true; 00642 } 00643 00644 bool planning_models::KinematicState::JointStateGroup::setKinematicState(const std::map<std::string, double>& joint_state_map) 00645 { 00646 bool all_set = true; 00647 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00648 bool is_set = joint_state_vector_[i]->setJointStateValues(joint_state_map); 00649 if(!is_set) all_set = false; 00650 } 00651 updateKinematicLinks(); 00652 return all_set; 00653 } 00654 00655 void planning_models::KinematicState::JointStateGroup::updateKinematicLinks() 00656 { 00657 for(unsigned int i = 0; i < updated_links_.size(); i++) { 00658 updated_links_[i]->computeTransform(); 00659 } 00660 } 00661 00662 void planning_models::KinematicState::JointStateGroup::setKinematicStateToDefault(void) 00663 { 00664 std::map<std::string, double> default_joint_states; 00665 00666 const unsigned int js = joint_state_vector_.size(); 00667 for (unsigned int i = 0 ; i < js ; ++i) 00668 { 00669 joint_state_vector_[i]->getJointModel()->getVariableDefaultValuesGivenBounds(default_joint_states); 00670 } 00671 setKinematicState(default_joint_states); 00672 } 00673 00674 void planning_models::KinematicState::JointStateGroup::getKinematicStateValues(std::vector<double>& joint_state_values) const { 00675 joint_state_values.clear(); 00676 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00677 unsigned int dim = joint_state_vector_[i]->getDimension(); 00678 if(dim != 0) { 00679 for(unsigned int j = 0; j < joint_state_vector_[i]->getJointStateValues().size(); j++) { 00680 joint_state_values.push_back(joint_state_vector_[i]->getJointStateValues()[j]); 00681 } 00682 } 00683 } 00684 if(joint_state_values.size() != dimension_) { 00685 ROS_WARN_STREAM("Some problems with group vector dimension values " << joint_state_values.size() << " dimension is " << dimension_); 00686 } 00687 } 00688 00689 void planning_models::KinematicState::JointStateGroup::getKinematicStateValues(std::map<std::string,double>& joint_state_values) const { 00690 joint_state_values.clear(); 00691 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00692 unsigned int dim = joint_state_vector_[i]->getDimension(); 00693 if(dim != 0) { 00694 for(unsigned int j = 0; j < joint_state_vector_[i]->getJointStateValues().size(); j++) { 00695 joint_state_values[joint_state_vector_[i]->getJointStateNameOrder()[j]] = joint_state_vector_[i]->getJointStateValues()[j]; 00696 } 00697 } 00698 } 00699 if(joint_state_values.size() != dimension_) { 00700 ROS_WARN_STREAM("Some problems with group map dimension values " << joint_state_values.size() << " dimension is " << dimension_); 00701 } 00702 } 00703 00704 planning_models::KinematicState::JointState* planning_models::KinematicState::JointStateGroup::getJointState(const std::string &joint) const 00705 { 00706 if(!hasJointState(joint)) return NULL; 00707 return joint_state_map_.find(joint)->second; 00708 } 00709 00710 // ------ printing transforms ----- 00711 00712 void planning_models::KinematicState::printStateInfo(std::ostream &out) const 00713 { 00714 out << "Complete model state dimension = " << getDimension() << std::endl; 00715 00716 std::ios_base::fmtflags old_flags = out.flags(); 00717 out.setf(std::ios::fixed, std::ios::floatfield); 00718 std::streamsize old_prec = out.precision(); 00719 out.precision(5); 00720 out << "State bounds: "; 00721 for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { 00722 for(std::map<std::string, std::pair<double, double> >::const_iterator it = joint_state_vector_[i]->getAllJointValueBounds().begin(); 00723 it != joint_state_vector_[i]->getAllJointValueBounds().end(); 00724 it++) { 00725 if(it->second.first == -DBL_MAX) { 00726 out << "[-DBL_MAX, "; 00727 } else { 00728 out << "[" << it->second.first << ", "; 00729 } 00730 if(it->second.second == DBL_MAX) { 00731 out << "DBL_MAX] "; 00732 } else { 00733 out << it->second.second << "] "; 00734 } 00735 } 00736 } 00737 00738 out << std::endl; 00739 out.precision(old_prec); 00740 out.flags(old_flags); 00741 00742 out << "Root joint : "; 00743 out << kinematic_model_->getRoot()->getName() << " "; 00744 out << std::endl; 00745 00746 out << "Available groups: "; 00747 std::vector<std::string> l; 00748 getJointStateGroupNames(l); 00749 for (unsigned int i = 0 ; i < l.size() ; ++i) 00750 out << l[i] << " "; 00751 out << std::endl; 00752 00753 for (unsigned int i = 0 ; i < l.size() ; ++i) 00754 { 00755 const JointStateGroup *g = getJointStateGroup(l[i]); 00756 out << "Group " << l[i] << " has " << g->getJointRoots().size() << " roots: "; 00757 for (unsigned int j = 0 ; j < g->getJointRoots().size() ; ++j) 00758 out << g->getJointRoots()[j]->getName() << " "; 00759 out << std::endl; 00760 } 00761 } 00762 00763 void planning_models::KinematicState::printTransform(const std::string &st, const btTransform &t, std::ostream &out) const 00764 { 00765 out << st << std::endl; 00766 const btVector3 &v = t.getOrigin(); 00767 out << " origin: " << v.x() << ", " << v.y() << ", " << v.z() << std::endl; 00768 const btQuaternion &q = t.getRotation(); 00769 out << " quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl; 00770 } 00771 00772 void planning_models::KinematicState::printTransforms(std::ostream &out) const 00773 { 00774 out << "Joint transforms:" << std::endl; 00775 for (unsigned int i = 0 ; i < joint_state_vector_.size() ; ++i) 00776 { 00777 printTransform(joint_state_vector_[i]->getName(), joint_state_vector_[i]->getVariableTransform(), out); 00778 out << std::endl; 00779 } 00780 out << "Link poses:" << std::endl; 00781 for (unsigned int i = 0 ; i < link_state_vector_.size() ; ++i) 00782 { 00783 printTransform(link_state_vector_[i]->getName(), link_state_vector_[i]->getGlobalCollisionBodyTransform(), out); 00784 out << std::endl; 00785 } 00786 } 00787