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


planning_models
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Mon Dec 2 2013 12:33:37