robot_state.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Ioan A. Sucan
00005 *  Copyright (c) 2013, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *********************************************************************/
00035 
00036 /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */
00037 
00038 #include <moveit/robot_state/robot_state.h>
00039 #include <moveit/transforms/transforms.h>
00040 #include <geometric_shapes/shape_operations.h>
00041 #include <eigen_conversions/eigen_msg.h>
00042 #include <moveit/backtrace/backtrace.h>
00043 #include <moveit/profiler/profiler.h>
00044 #include <boost/bind.hpp>
00045 
00046 moveit::core::RobotState::RobotState(const RobotModelConstPtr &robot_model)
00047   : robot_model_(robot_model)
00048   , has_velocity_(false)
00049   , has_acceleration_(false)
00050   , has_effort_(false)
00051   , dirty_link_transforms_(robot_model_->getRootJoint())
00052   , dirty_collision_body_transforms_(NULL)
00053   , rng_(NULL)
00054 {
00055   allocMemory();
00056   
00057   // all transforms are dirty initially
00058   const int nr_doubles_for_dirty_joint_transforms = 1 + robot_model_->getJointModelCount() / (sizeof(double)/sizeof(unsigned char));
00059   memset(dirty_joint_transforms_, 1, sizeof(double) * nr_doubles_for_dirty_joint_transforms);
00060 }
00061 
00062 moveit::core::RobotState::RobotState(const RobotState &other)
00063   : rng_(NULL)
00064 {
00065   robot_model_ = other.robot_model_;
00066   allocMemory();
00067   copyFrom(other);
00068 }
00069 
00070 moveit::core::RobotState::~RobotState()
00071 {
00072   free(memory_);
00073   if (rng_)
00074     delete rng_;
00075 }
00076 
00077 void moveit::core::RobotState::allocMemory(void)
00078 {
00079   // memory for the dirty joint transforms
00080   const int nr_doubles_for_dirty_joint_transforms = 1 + robot_model_->getJointModelCount() / (sizeof(double)/sizeof(unsigned char));
00081   const size_t bytes = sizeof(Eigen::Affine3d) * (robot_model_->getJointModelCount() + robot_model_->getLinkModelCount() + robot_model_->getLinkGeometryCount())
00082     + sizeof(double) * (robot_model_->getVariableCount() * 3 + nr_doubles_for_dirty_joint_transforms) + 15;
00083   memory_ = malloc(bytes);
00084 
00085   // make the memory for transforms align at 16 bytes
00086   variable_joint_transforms_ = reinterpret_cast<Eigen::Affine3d*>(((uintptr_t)memory_ + 15) & ~ (uintptr_t)0x0F);
00087   global_link_transforms_ = variable_joint_transforms_ + robot_model_->getJointModelCount();
00088   global_collision_body_transforms_ = global_link_transforms_ + robot_model_->getLinkModelCount();
00089   dirty_joint_transforms_ = reinterpret_cast<unsigned char*>(global_collision_body_transforms_ + robot_model_->getLinkGeometryCount());
00090   position_ = reinterpret_cast<double*>(dirty_joint_transforms_) + nr_doubles_for_dirty_joint_transforms;
00091   velocity_ = position_ + robot_model_->getVariableCount();
00092   // acceleration and effort share the memory (not both can be specified)
00093   effort_ = acceleration_ = velocity_ + robot_model_->getVariableCount();
00094 }
00095 
00096 moveit::core::RobotState& moveit::core::RobotState::operator=(const RobotState &other)
00097 {
00098   if (this != &other)
00099     copyFrom(other);
00100   return *this;
00101 }
00102 
00103 void moveit::core::RobotState::copyFrom(const RobotState &other)
00104 {
00105   has_velocity_ = other.has_velocity_;
00106   has_acceleration_ = other.has_acceleration_;
00107   has_effort_ = other.has_effort_;
00108   
00109   dirty_collision_body_transforms_ = other.dirty_collision_body_transforms_;
00110   dirty_link_transforms_ = other.dirty_link_transforms_;
00111 
00112   if (dirty_link_transforms_ == robot_model_->getRootJoint())
00113   {
00114     // everything is dirty; no point in copying transforms; copy positions, potentially velocity & acceleration
00115     memcpy(position_, other.position_, robot_model_->getVariableCount() * sizeof(double) *
00116            (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) + ((has_acceleration_ || has_effort_) ? 1 : 0)));
00117     
00118     // mark all transforms as dirty
00119     const int nr_doubles_for_dirty_joint_transforms = 1 + robot_model_->getJointModelCount() / (sizeof(double)/sizeof(unsigned char));
00120     memset(dirty_joint_transforms_, 1, sizeof(double) * nr_doubles_for_dirty_joint_transforms);
00121   }
00122   else
00123   {
00124     // copy all the memory; maybe avoid copying velocity and acceleration if possible
00125     const int nr_doubles_for_dirty_joint_transforms = 1 + robot_model_->getJointModelCount() / (sizeof(double)/sizeof(unsigned char));
00126     const size_t bytes = sizeof(Eigen::Affine3d) * (robot_model_->getJointModelCount() + robot_model_->getLinkModelCount() + robot_model_->getLinkGeometryCount())
00127       + sizeof(double) * (robot_model_->getVariableCount() * (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) +
00128                                                               ((has_acceleration_ || has_effort_ ) ? 1 : 0)) + nr_doubles_for_dirty_joint_transforms);
00129     memcpy(variable_joint_transforms_, other.variable_joint_transforms_, bytes);
00130   }
00131   
00132   // copy attached bodies
00133   clearAttachedBodies();
00134   for (std::map<std::string, AttachedBody*>::const_iterator it = other.attached_body_map_.begin() ; it != other.attached_body_map_.end() ; ++it)
00135     attachBody(it->second->getName(), it->second->getShapes(), it->second->getFixedTransforms(),
00136                it->second->getTouchLinks(), it->second->getAttachedLinkName(), it->second->getDetachPosture());
00137 }
00138 
00139 bool moveit::core::RobotState::checkJointTransforms(const JointModel *joint) const
00140 {
00141   if (dirtyJointTransform(joint))
00142   {
00143     logWarn("Returning dirty joint transforms for joint '%s'", joint->getName().c_str());
00144     return false;
00145   }
00146   return true;
00147 }
00148 
00149 bool moveit::core::RobotState::checkLinkTransforms() const
00150 {
00151   if (dirtyLinkTransforms())
00152   {
00153     logWarn("Returning dirty link transforms");
00154     return false;
00155   }
00156   return true;
00157 }
00158 
00159 bool moveit::core::RobotState::checkCollisionTransforms() const
00160 {
00161   if (dirtyCollisionBodyTransforms())
00162   {
00163     logWarn("Returning dirty collision body transforms");
00164     return false;
00165   }
00166   return true;
00167 }
00168 
00169 void moveit::core::RobotState::markVelocity()
00170 {
00171   if (!has_velocity_)
00172   {
00173     has_velocity_ = true;
00174     memset(velocity_, 0, sizeof(double) * robot_model_->getVariableCount());
00175   }
00176 }
00177 
00178 void moveit::core::RobotState::markAcceleration()
00179 {
00180   if (!has_acceleration_)
00181   {
00182     has_acceleration_ = true;
00183     has_effort_ = false;
00184     memset(acceleration_, 0, sizeof(double) * robot_model_->getVariableCount());
00185   }
00186 }
00187 
00188 void moveit::core::RobotState::markEffort()
00189 {
00190   if (!has_effort_)
00191   {
00192     has_acceleration_ = false;
00193     has_effort_ = true;
00194     memset(effort_, 0, sizeof(double) * robot_model_->getVariableCount());
00195   }
00196 }
00197 
00198 void moveit::core::RobotState::setToRandomPositions()
00199 {
00200   random_numbers::RandomNumberGenerator &rng = getRandomNumberGenerator();
00201   robot_model_->getVariableRandomPositions(rng, position_);
00202   memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char));
00203   dirty_link_transforms_ = robot_model_->getRootJoint();
00204   // mimic values are correctly set in RobotModel
00205 }
00206 
00207 void moveit::core::RobotState::setToRandomPositions(const JointModelGroup *group)
00208 {
00209   // we do not make calls to RobotModel for random number generation because mimic joints
00210   // could trigger updates outside the state of the group itself
00211   random_numbers::RandomNumberGenerator &rng = getRandomNumberGenerator();
00212   setToRandomPositions(group, rng);
00213 }
00214 void moveit::core::RobotState::setToRandomPositions(const JointModelGroup *group, random_numbers::RandomNumberGenerator &rng)
00215 {
00216   const std::vector<const JointModel*> &joints = group->getActiveJointModels();
00217   for (std::size_t i = 0 ; i < joints.size() ; ++i)
00218     joints[i]->getVariableRandomPositions(rng, position_ + joints[i]->getFirstVariableIndex());
00219   updateMimicJoint(group->getMimicJointModels());
00220   markDirtyJointTransforms(group);
00221 }
00222 
00223 void moveit::core::RobotState::setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, const std::vector<double> &distances)
00224 {
00225   // we do not make calls to RobotModel for random number generation because mimic joints
00226   // could trigger updates outside the state of the group itself
00227   random_numbers::RandomNumberGenerator &rng = getRandomNumberGenerator();
00228   const std::vector<const JointModel*> &joints = group->getActiveJointModels();
00229   assert(distances.size() == joints.size());
00230   for (std::size_t i = 0 ; i < joints.size() ; ++i)
00231   {
00232     const int idx = joints[i]->getFirstVariableIndex();
00233     joints[i]->getVariableRandomPositionsNearBy(rng, position_ + joints[i]->getFirstVariableIndex(), near.position_ + idx, distances[i]);
00234   }
00235   updateMimicJoint(group->getMimicJointModels());
00236   markDirtyJointTransforms(group);
00237 }
00238 
00239 void moveit::core::RobotState::setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, double distance)
00240 {
00241   // we do not make calls to RobotModel for random number generation because mimic joints
00242   // could trigger updates outside the state of the group itself
00243   random_numbers::RandomNumberGenerator &rng = getRandomNumberGenerator();
00244   const std::vector<const JointModel*> &joints = group->getActiveJointModels();
00245   for (std::size_t i = 0 ; i < joints.size() ; ++i)
00246   {
00247     const int idx = joints[i]->getFirstVariableIndex();
00248     joints[i]->getVariableRandomPositionsNearBy(rng, position_ + joints[i]->getFirstVariableIndex(), near.position_ + idx, distance);
00249   }
00250   updateMimicJoint(group->getMimicJointModels());
00251   markDirtyJointTransforms(group);
00252 }
00253 
00254 bool moveit::core::RobotState::setToDefaultValues(const JointModelGroup *group, const std::string &name)
00255 {
00256   std::map<std::string, double> m;
00257   bool r = group->getVariableDefaultPositions(name, m); // mimic values are updated
00258   setVariablePositions(m);
00259   return r;
00260 }
00261 
00262 void moveit::core::RobotState::setToDefaultValues()
00263 {
00264   robot_model_->getVariableDefaultPositions(position_); // mimic values are updated
00265   // set velocity & acceleration to 0
00266   memset(velocity_, 0, sizeof(double) * 2 * robot_model_->getVariableCount());
00267   memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char));
00268   dirty_link_transforms_ = robot_model_->getRootJoint();
00269 }
00270 
00271 void moveit::core::RobotState::setVariablePositions(const double *position)
00272 {
00273   // assume everything is in order in terms of array lengths (for efficiency reasons)
00274   memcpy(position_, position, robot_model_->getVariableCount() * sizeof(double));
00275   
00276   // the full state includes mimic joint values, so no need to update mimic here
00277   
00278   // Since all joint values have potentially changed, we will need to recompute all transforms
00279   memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char));
00280   dirty_link_transforms_ = robot_model_->getRootJoint();
00281 }
00282 
00283 void moveit::core::RobotState::setVariablePositions(const std::map<std::string, double> &variable_map)
00284 {
00285   for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end() ; it != end ; ++it)
00286   {
00287     const int index = robot_model_->getVariableIndex(it->first);
00288     position_[index] = it->second;
00289     const JointModel *jm = robot_model_->getJointOfVariable(index);
00290     markDirtyJointTransforms(jm);
00291     updateMimicJoint(jm);
00292   }
00293 }
00294 
00295 void moveit::core::RobotState::getMissingKeys(const std::map<std::string, double> &variable_map, std::vector<std::string> &missing_variables) const
00296 {
00297   missing_variables.clear();
00298   const std::vector<std::string> &nm = robot_model_->getVariableNames();
00299   for (std::size_t i = 0 ; i < nm.size() ; ++i)
00300     if (variable_map.find(nm[i]) == variable_map.end())
00301       if (robot_model_->getJointOfVariable(nm[i])->getMimic() == NULL)
00302         missing_variables.push_back(nm[i]);
00303 }
00304 
00305 void moveit::core::RobotState::setVariablePositions(const std::map<std::string, double> &variable_map, std::vector<std::string> &missing_variables)
00306 {
00307   setVariablePositions(variable_map);
00308   getMissingKeys(variable_map, missing_variables);
00309 }
00310 
00311 void moveit::core::RobotState::setVariablePositions(const std::vector<std::string>& variable_names, const std::vector<double>& variable_position)
00312 {
00313   for (std::size_t i = 0 ; i < variable_names.size() ; ++i)
00314   { 
00315     const int index = robot_model_->getVariableIndex(variable_names[i]);
00316     position_[index] = variable_position[i];  
00317     const JointModel *jm = robot_model_->getJointOfVariable(index);
00318     markDirtyJointTransforms(jm);
00319     updateMimicJoint(jm);
00320   }
00321 }
00322 
00323 void moveit::core::RobotState::setVariableVelocities(const std::map<std::string, double> &variable_map)
00324 {
00325   markVelocity();
00326   for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end() ; it != end ; ++it)
00327     velocity_[robot_model_->getVariableIndex(it->first)] = it->second;
00328 }
00329 
00330 void moveit::core::RobotState::setVariableVelocities(const std::map<std::string, double> &variable_map, std::vector<std::string>& missing_variables)
00331 {
00332   setVariableVelocities(variable_map);
00333   getMissingKeys(variable_map, missing_variables);
00334 }
00335 
00336 void moveit::core::RobotState::setVariableVelocities(const std::vector<std::string>& variable_names, const std::vector<double>& variable_velocity)
00337 {
00338   markVelocity();  
00339   assert(variable_names.size() == variable_velocity.size());
00340   for (std::size_t i = 0 ; i < variable_names.size() ; ++i)
00341     velocity_[robot_model_->getVariableIndex(variable_names[i])] = variable_velocity[i];  
00342 }
00343 
00344 void moveit::core::RobotState::setVariableAccelerations(const std::map<std::string, double> &variable_map)
00345 {
00346   markAcceleration();
00347   for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end() ; it != end ; ++it)
00348     acceleration_[robot_model_->getVariableIndex(it->first)] = it->second;
00349 }
00350 
00351 void moveit::core::RobotState::setVariableAccelerations(const std::map<std::string, double> &variable_map, std::vector<std::string>& missing_variables)
00352 {
00353   setVariableAccelerations(variable_map);
00354   getMissingKeys(variable_map, missing_variables);
00355 }
00356 
00357 void moveit::core::RobotState::setVariableAccelerations(const std::vector<std::string>& variable_names, const std::vector<double>& variable_acceleration)
00358 {
00359   markAcceleration();
00360   assert(variable_names.size() == variable_acceleration.size());
00361   for (std::size_t i = 0 ; i < variable_names.size() ; ++i)
00362     acceleration_[robot_model_->getVariableIndex(variable_names[i])] = variable_acceleration[i];  
00363 }
00364 
00365 void moveit::core::RobotState::setVariableEffort(const std::map<std::string, double> &variable_map)
00366 {
00367   markEffort();
00368   for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end() ; it != end ; ++it)
00369     acceleration_[robot_model_->getVariableIndex(it->first)] = it->second;
00370 }
00371 
00372 void moveit::core::RobotState::setVariableEffort(const std::map<std::string, double> &variable_map, std::vector<std::string>& missing_variables)
00373 {
00374   setVariableEffort(variable_map);
00375   getMissingKeys(variable_map, missing_variables);
00376 }
00377 
00378 void moveit::core::RobotState::setVariableEffort(const std::vector<std::string>& variable_names, const std::vector<double>& variable_effort)
00379 {
00380   markEffort();
00381   assert(variable_names.size() == variable_effort.size());
00382   for (std::size_t i = 0 ; i < variable_names.size() ; ++i)
00383     effort_[robot_model_->getVariableIndex(variable_names[i])] = variable_effort[i];  
00384 }
00385 
00386 void moveit::core::RobotState::setJointGroupPositions(const JointModelGroup *group, const double *gstate)
00387 {
00388   const std::vector<int> &il = group->getVariableIndexList();
00389   if (group->isContiguousWithinState())
00390     memcpy(position_ + il[0], gstate, group->getVariableCount() * sizeof(double));
00391   else
00392   {
00393     for (std::size_t i = 0 ; i < il.size() ; ++i)
00394       position_[il[i]] = gstate[i];
00395   }
00396   updateMimicJoint(group->getMimicJointModels());
00397   markDirtyJointTransforms(group);
00398 }
00399 
00400 void moveit::core::RobotState::setJointGroupPositions(const JointModelGroup *group, const Eigen::VectorXd& values)
00401 {
00402   const std::vector<int> &il = group->getVariableIndexList();
00403   for (std::size_t i = 0 ; i < il.size() ; ++i)
00404     position_[il[i]] = values(i); 
00405   updateMimicJoint(group->getMimicJointModels());
00406   markDirtyJointTransforms(group);
00407 }
00408 
00409 void moveit::core::RobotState::copyJointGroupPositions(const JointModelGroup *group, double *gstate) const
00410 {
00411   const std::vector<int> &il = group->getVariableIndexList();
00412   if (group->isContiguousWithinState())
00413     memcpy(gstate, position_ + il[0], group->getVariableCount() * sizeof(double));
00414   else
00415     for (std::size_t i = 0 ; i < il.size() ; ++i)
00416       gstate[i] = position_[il[i]];
00417 }
00418 
00419 void moveit::core::RobotState::copyJointGroupPositions(const JointModelGroup *group, Eigen::VectorXd& values) const
00420 {
00421   const std::vector<int> &il = group->getVariableIndexList();
00422   values.resize(il.size());
00423   for (std::size_t i = 0 ; i < il.size() ; ++i)
00424     values(i) = position_[il[i]];  
00425 } 
00426 
00427 void moveit::core::RobotState::update(bool force)
00428 {
00429   // make sure we do everything from scratch if needed
00430   if (force)
00431   {
00432     memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char));
00433     dirty_link_transforms_ = robot_model_->getRootJoint();
00434   }
00435   
00436   // this actually triggers all needed updates
00437   updateCollisionBodyTransforms();
00438 }
00439 
00440 void moveit::core::RobotState::updateCollisionBodyTransforms()
00441 {
00442   if (dirty_link_transforms_ != NULL)
00443     updateLinkTransforms();
00444   
00445   if (dirty_collision_body_transforms_ != NULL)
00446   {
00447     const std::vector<const LinkModel*> &links = dirty_collision_body_transforms_->getDescendantLinkModels();
00448     dirty_collision_body_transforms_ = NULL; 
00449 
00450     for (std::size_t i = 0 ; i < links.size() ; ++i)
00451     {
00452       const EigenSTL::vector_Affine3d &ot = links[i]->getCollisionOriginTransforms();
00453       const std::vector<int> &ot_id = links[i]->areCollisionOriginTransformsIdentity();
00454       const int index_co = links[i]->getFirstCollisionBodyTransformIndex();
00455       const int index_l = links[i]->getLinkIndex();
00456       for (std::size_t j = 0 ; j < ot.size() ; ++j)
00457         global_collision_body_transforms_[index_co + j].matrix().noalias() = ot_id[j] ? global_link_transforms_[index_l].matrix() : global_link_transforms_[index_l].matrix() * ot[j].matrix();
00458     }
00459   }
00460 }
00461 
00462 void moveit::core::RobotState::updateLinkTransforms()
00463 {
00464   if (dirty_link_transforms_ != NULL)
00465   {
00466     updateLinkTransformsInternal(dirty_link_transforms_);
00467     if (dirty_collision_body_transforms_)
00468       dirty_collision_body_transforms_ = robot_model_->getCommonRoot(dirty_collision_body_transforms_, dirty_link_transforms_);
00469     else
00470       dirty_collision_body_transforms_ = dirty_link_transforms_;
00471     dirty_link_transforms_ = NULL;
00472   }
00473 }
00474 
00475 void moveit::core::RobotState::updateLinkTransformsInternal(const JointModel *start)
00476 {  
00477   const std::vector<const LinkModel*> &links = start->getDescendantLinkModels();
00478   if (!links.empty())
00479   { 
00480     const LinkModel *parent = links[0]->getParentLinkModel();
00481     if (parent)
00482     {
00483       if (links[0]->parentJointIsFixed())
00484         global_link_transforms_[links[0]->getLinkIndex()].matrix().noalias() = 
00485           global_link_transforms_[parent->getLinkIndex()].matrix() * links[0]->getJointOriginTransform().matrix();
00486       else
00487       {
00488         if (links[0]->jointOriginTransformIsIdentity())
00489           global_link_transforms_[links[0]->getLinkIndex()].matrix().noalias() =
00490             global_link_transforms_[parent->getLinkIndex()].matrix() * getJointTransform(links[0]->getParentJointModel()).matrix();
00491         else
00492           global_link_transforms_[links[0]->getLinkIndex()].matrix().noalias() = 
00493             global_link_transforms_[parent->getLinkIndex()].matrix()
00494             * links[0]->getJointOriginTransform().matrix()
00495             * getJointTransform(links[0]->getParentJointModel()).matrix();
00496       }
00497     }
00498     else
00499     {
00500       if (links[0]->jointOriginTransformIsIdentity())
00501         global_link_transforms_[links[0]->getLinkIndex()] = getJointTransform(links[0]->getParentJointModel());
00502       else
00503         global_link_transforms_[links[0]->getLinkIndex()].matrix().noalias() = links[0]->getJointOriginTransform().matrix() * getJointTransform(links[0]->getParentJointModel()).matrix();
00504     }
00505     
00506     // we know the rest of the links have parents
00507     for (std::size_t i = 1 ; i < links.size() ; ++i)
00508     {
00509       if (links[i]->parentJointIsFixed())
00510         global_link_transforms_[links[i]->getLinkIndex()].matrix().noalias() = 
00511           global_link_transforms_[links[i]->getParentLinkModel()->getLinkIndex()].matrix()
00512           * links[i]->getJointOriginTransform().matrix();
00513       else
00514       {
00515         if (links[i]->jointOriginTransformIsIdentity())
00516           global_link_transforms_[links[i]->getLinkIndex()].matrix().noalias() =
00517             global_link_transforms_[links[i]->getParentLinkModel()->getLinkIndex()].matrix()
00518             * getJointTransform(links[i]->getParentJointModel()).matrix();
00519         else
00520           global_link_transforms_[links[i]->getLinkIndex()].matrix().noalias() = 
00521             global_link_transforms_[links[i]->getParentLinkModel()->getLinkIndex()] .matrix()
00522             * links[i]->getJointOriginTransform().matrix()
00523             * getJointTransform(links[i]->getParentJointModel()).matrix();
00524       }
00525     }
00526   }
00527   
00528   // update attached bodies tf; these are usually very few, so we update them all
00529   for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin() ; it != attached_body_map_.end() ; ++it)
00530     it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]);
00531 }
00532 
00533 void moveit::core::RobotState::updateStateWithLinkAt(const LinkModel *link, const Eigen::Affine3d& transform, bool backward)
00534 {
00535   updateLinkTransforms(); // no link transforms must be dirty, otherwise the transform we set will be overwritten
00536   
00537   // update the fact that collision body transforms are out of date
00538   if (dirty_collision_body_transforms_)
00539     dirty_collision_body_transforms_ = robot_model_->getCommonRoot(dirty_collision_body_transforms_, link->getParentJointModel());
00540   else
00541     dirty_collision_body_transforms_ = link->getParentJointModel();
00542   
00543   global_link_transforms_[link->getLinkIndex()] = transform;
00544 
00545   // update link transforms for descendant links only (leaving the transform for the current link untouched)
00546   const std::vector<const JointModel*> &cj = link->getChildJointModels();
00547   for (std::size_t i = 0 ; i < cj.size() ; ++i)
00548     updateLinkTransformsInternal(cj[i]);
00549   
00550   // if we also need to go backward
00551   if (backward)
00552   {
00553     const LinkModel *parent_link = link;
00554     const LinkModel *child_link;
00555     while (parent_link->getParentJointModel()->getParentLinkModel())
00556     {
00557       child_link = parent_link;
00558       parent_link = parent_link->getParentJointModel()->getParentLinkModel();
00559 
00560       // update the transform of the parent
00561       global_link_transforms_[parent_link->getLinkIndex()] = global_link_transforms_[child_link->getLinkIndex()] *
00562         (child_link->getJointOriginTransform() * variable_joint_transforms_[child_link->getParentJointModel()->getJointIndex()]).inverse();
00563 
00564       // update link transforms for descendant links only (leaving the transform for the current link untouched)
00565       // with the exception of the child link we are coming backwards from
00566       const std::vector<const JointModel*> &cj = parent_link->getChildJointModels();
00567       for (std::size_t i = 0 ; i < cj.size() ; ++i)
00568         if (cj[i] != child_link->getParentJointModel())
00569           updateLinkTransformsInternal(cj[i]);
00570     }
00571     // update the root joint of the model to match (as best as possible given #DOF) the transform we wish to obtain for the root link.
00572     // but I am disabling this code, since I do not think this function should modify variable values.
00573     //    parent_link->getParentJointModel()->computeVariableValues(global_link_transforms_[parent_link->getLinkIndex()],
00574     //                                                              position_ + parent_link->getParentJointModel()->getFirstVariableIndex());
00575   }
00576   
00577   // update attached bodies tf; these are usually very few, so we update them all
00578   for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin() ; it != attached_body_map_.end() ; ++it)
00579     it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]);
00580 }
00581 
00582 bool moveit::core::RobotState::satisfiesBounds(double margin) const
00583 {
00584   const std::vector<const JointModel*> &jm = robot_model_->getActiveJointModels();
00585   for (std::size_t i = 0 ; i < jm.size() ; ++i)
00586     if (!satisfiesBounds(jm[i], margin))
00587       return false;
00588   return true;
00589 }
00590 
00591 bool moveit::core::RobotState::satisfiesBounds(const JointModelGroup *group, double margin) const
00592 {
00593   const std::vector<const JointModel*> &jm = group->getActiveJointModels();
00594   for (std::size_t i = 0 ; i < jm.size() ; ++i)
00595     if (!satisfiesBounds(jm[i], margin))
00596       return false;
00597   return true;  
00598 }
00599 
00600 void moveit::core::RobotState::enforceBounds()
00601 {
00602   const std::vector<const JointModel*> &jm = robot_model_->getActiveJointModels();
00603   for (std::size_t i = 0 ; i < jm.size() ; ++i)
00604     enforceBounds(jm[i]);
00605 }
00606 
00607 void moveit::core::RobotState::enforceBounds(const JointModelGroup *joint_group)
00608 {
00609   const std::vector<const JointModel*> &jm = joint_group->getActiveJointModels();
00610   for (std::size_t i = 0 ; i < jm.size() ; ++i)
00611     enforceBounds(jm[i]);
00612 }
00613 
00614 std::pair<double, const moveit::core::JointModel*> moveit::core::RobotState::getMinDistanceToPositionBounds() const
00615 {
00616   return getMinDistanceToPositionBounds(robot_model_->getActiveJointModels());
00617 }
00618 
00619 std::pair<double, const moveit::core::JointModel*> moveit::core::RobotState::getMinDistanceToPositionBounds(const JointModelGroup *group) const
00620 {
00621   return getMinDistanceToPositionBounds(group->getActiveJointModels());
00622 }
00623 
00624 std::pair<double, const moveit::core::JointModel*> moveit::core::RobotState::getMinDistanceToPositionBounds(const std::vector<const JointModel*> &joints) const
00625 {
00626   double distance = std::numeric_limits<double>::max();
00627   const JointModel* index = NULL;
00628   for (std::size_t i = 0; i < joints.size() ; ++i)
00629   {
00630     if (joints[i]->getType() == JointModel::PLANAR || joints[i]->getType() == JointModel::FLOATING)
00631       continue;
00632     if (joints[i]->getType() == JointModel::REVOLUTE)
00633       if (static_cast<const RevoluteJointModel*>(joints[i])->isContinuous())
00634         continue;
00635     
00636     const double* joint_values = getJointPositions(joints[i]);
00637     const JointModel::Bounds& bounds = joints[i]->getVariableBounds();
00638     std::vector<double> lower_bounds(bounds.size()), upper_bounds(bounds.size());
00639     for (std::size_t j = 0; j < bounds.size() ; ++j)
00640     {
00641       lower_bounds[j] = bounds[j].min_position_;
00642       upper_bounds[j] = bounds[j].max_position_;
00643     }
00644     double new_distance = joints[i]->distance(joint_values, &lower_bounds[0]);
00645     if (new_distance < distance)
00646     {
00647       index = joints[i];
00648       distance = new_distance;
00649     }
00650     new_distance = joints[i]->distance(joint_values, &upper_bounds[0]);
00651     if (new_distance < distance)
00652     {
00653       index = joints[i];
00654       distance = new_distance;
00655     }
00656   }
00657   return std::make_pair(distance, index);
00658 }
00659 
00660 double moveit::core::RobotState::distance(const RobotState &other, const JointModelGroup *joint_group) const
00661 {
00662   double d = 0.0;
00663   const std::vector<const JointModel*> &jm = joint_group->getActiveJointModels();
00664   for (std::size_t i = 0 ; i < jm.size() ; ++i)
00665   {
00666     const int idx = jm[i]->getFirstVariableIndex();
00667     d += jm[i]->getDistanceFactor() * jm[i]->distance(position_ + idx, other.position_ + idx);
00668   }
00669   return d;  
00670 }
00671 
00672 void moveit::core::RobotState::interpolate(const RobotState &to, double t, RobotState &state) const
00673 {
00674   robot_model_->interpolate(getVariablePositions(), to.getVariablePositions(), t, state.getVariablePositions());
00675   
00676   memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() * sizeof(unsigned char));
00677   state.dirty_link_transforms_ = state.robot_model_->getRootJoint();
00678 }
00679 
00680 void moveit::core::RobotState::interpolate(const RobotState &to, double t, RobotState &state, const JointModelGroup *joint_group) const
00681 {
00682   const std::vector<const JointModel*> &jm = joint_group->getActiveJointModels();
00683   for (std::size_t i = 0 ; i < jm.size() ; ++i)
00684   {
00685     const int idx = jm[i]->getFirstVariableIndex();
00686     jm[i]->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
00687   }
00688   state.markDirtyJointTransforms(joint_group);
00689   state.updateMimicJoint(joint_group->getMimicJointModels());
00690 }
00691 
00692 void moveit::core::RobotState::setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback)
00693 {
00694   attached_body_update_callback_ = callback;
00695 }
00696 
00697 bool moveit::core::RobotState::hasAttachedBody(const std::string &id) const
00698 {
00699   return attached_body_map_.find(id) != attached_body_map_.end();
00700 }
00701 
00702 const moveit::core::AttachedBody* moveit::core::RobotState::getAttachedBody(const std::string &id) const
00703 {
00704   std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.find(id);
00705   if (it == attached_body_map_.end())
00706   {
00707     logError("Attached body '%s' not found", id.c_str());
00708     return NULL;
00709   }
00710   else
00711     return it->second;
00712 }
00713 
00714 void moveit::core::RobotState::attachBody(AttachedBody *attached_body)
00715 {
00716   attached_body_map_[attached_body->getName()] = attached_body;
00717   attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink()));
00718   if (attached_body_update_callback_)
00719     attached_body_update_callback_(attached_body, true);
00720 }
00721 
00722 void moveit::core::RobotState::attachBody(const std::string &id,
00723                                           const std::vector<shapes::ShapeConstPtr> &shapes,
00724                                           const EigenSTL::vector_Affine3d &attach_trans,
00725                                           const std::set<std::string> &touch_links,
00726                                           const std::string &link,
00727                                           const trajectory_msgs::JointTrajectory &detach_posture)
00728 {
00729   const LinkModel *l = robot_model_->getLinkModel(link);
00730   AttachedBody *ab = new AttachedBody(l, id, shapes, attach_trans, touch_links, detach_posture);
00731   attached_body_map_[id] = ab;
00732   ab->computeTransform(getGlobalLinkTransform(l));
00733   if (attached_body_update_callback_)
00734     attached_body_update_callback_(ab, true);
00735 }
00736 
00737 void moveit::core::RobotState::getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies) const
00738 {
00739   attached_bodies.clear();
00740   attached_bodies.reserve(attached_body_map_.size());
00741   for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin() ; it != attached_body_map_.end() ;  ++it)
00742     attached_bodies.push_back(it->second);
00743 }
00744 
00745 void moveit::core::RobotState::getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies, const JointModelGroup *group) const
00746 {
00747   attached_bodies.clear();
00748   for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin() ; it != attached_body_map_.end() ;  ++it)
00749     if (group->hasLinkModel(it->second->getAttachedLinkName()))
00750       attached_bodies.push_back(it->second);
00751 }
00752 
00753 void moveit::core::RobotState::getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies, const LinkModel *lm) const
00754 {
00755   attached_bodies.clear();
00756   for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin() ; it != attached_body_map_.end() ;  ++it)
00757     if (it->second->getAttachedLink() == lm)
00758       attached_bodies.push_back(it->second);
00759 }
00760 
00761 void moveit::core::RobotState::clearAttachedBodies()
00762 {
00763   for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin() ; it != attached_body_map_.end() ;  ++it)
00764   {
00765     if (attached_body_update_callback_)
00766       attached_body_update_callback_(it->second, false);
00767     delete it->second;
00768   }
00769   attached_body_map_.clear();
00770 }
00771 
00772 void moveit::core::RobotState::clearAttachedBodies(const LinkModel *link)
00773 {
00774   std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.begin();
00775   while (it != attached_body_map_.end())
00776   {
00777     if (it->second->getAttachedLink() != link)
00778     {
00779       ++it;
00780       continue;
00781     }
00782     if (attached_body_update_callback_)
00783       attached_body_update_callback_(it->second, false);
00784     delete it->second;
00785     std::map<std::string, AttachedBody*>::iterator del = it++;
00786     attached_body_map_.erase(del);
00787   }
00788 }
00789 
00790 void moveit::core::RobotState::clearAttachedBodies(const JointModelGroup *group)
00791 {
00792   std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.begin();
00793   while (it != attached_body_map_.end())
00794   {
00795     if (!group->hasLinkModel(it->second->getAttachedLinkName()))
00796     {
00797       ++it;
00798       continue;
00799     }
00800     if (attached_body_update_callback_)
00801       attached_body_update_callback_(it->second, false);
00802     delete it->second;
00803     std::map<std::string, AttachedBody*>::iterator del = it++;
00804     attached_body_map_.erase(del);
00805   }
00806 }
00807 
00808 bool moveit::core::RobotState::clearAttachedBody(const std::string &id)
00809 {
00810   std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.find(id);
00811   if (it != attached_body_map_.end())
00812   {
00813     if (attached_body_update_callback_)
00814       attached_body_update_callback_(it->second, false);
00815     delete it->second;
00816     attached_body_map_.erase(it);
00817     return true;
00818   }
00819   else
00820     return false;
00821 }
00822 
00823 const Eigen::Affine3d& moveit::core::RobotState::getFrameTransform(const std::string &id)
00824 {
00825   updateLinkTransforms();
00826   return const_cast<const RobotState*>(this)->getFrameTransform(id);
00827 }
00828 
00829 const Eigen::Affine3d& moveit::core::RobotState::getFrameTransform(const std::string &id) const
00830 {
00831   if (!id.empty() && id[0] == '/')
00832     return getFrameTransform(id.substr(1)); 
00833   BOOST_VERIFY(checkLinkTransforms());
00834   
00835   static const Eigen::Affine3d identity_transform = Eigen::Affine3d::Identity();
00836   if (id.size() + 1 == robot_model_->getModelFrame().size() && '/' + id == robot_model_->getModelFrame())
00837     return identity_transform;
00838   if (robot_model_->hasLinkModel(id))
00839   {
00840     const LinkModel *lm = robot_model_->getLinkModel(id);
00841     return global_link_transforms_[lm->getLinkIndex()];
00842   }
00843   std::map<std::string, AttachedBody*>::const_iterator jt = attached_body_map_.find(id);
00844   if (jt == attached_body_map_.end())
00845   {
00846     logError("Transform from frame '%s' to frame '%s' is not known ('%s' should be a link name or an attached body id).",
00847              id.c_str(), robot_model_->getModelFrame().c_str(), id.c_str());
00848     return identity_transform;
00849   }
00850   const EigenSTL::vector_Affine3d &tf = jt->second->getGlobalCollisionBodyTransforms();
00851   if (tf.empty())
00852   {
00853     logError("Attached body '%s' has no geometry associated to it. No transform to return.", id.c_str());
00854     return identity_transform;
00855   }
00856   if (tf.size() > 1)
00857     logWarn("There are multiple geometries associated to attached body '%s'. Returning the transform for the first one.", id.c_str());
00858   return tf[0];
00859 }
00860 
00861 bool moveit::core::RobotState::knowsFrameTransform(const std::string &id) const
00862 {
00863   if (!id.empty() && id[0] == '/')
00864     return knowsFrameTransform(id.substr(1));
00865   if (robot_model_->hasLinkModel(id))
00866     return true;
00867   std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.find(id);
00868   return it != attached_body_map_.end() && it->second->getGlobalCollisionBodyTransforms().size() == 1;
00869 }
00870 
00871 void moveit::core::RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr,
00872                                                const std::vector<std::string> &link_names,
00873                                                const std_msgs::ColorRGBA& color,
00874                                                const std::string& ns,
00875                                                const ros::Duration& dur,
00876                                                bool include_attached) const
00877 {
00878   std::size_t cur_num = arr.markers.size();
00879   getRobotMarkers(arr, link_names, include_attached);
00880   unsigned int id = cur_num;
00881   for (std::size_t i = cur_num ; i < arr.markers.size() ; ++i, ++id)
00882   {
00883     arr.markers[i].ns = ns;
00884     arr.markers[i].id = id;
00885     arr.markers[i].lifetime = dur;
00886     arr.markers[i].color = color;
00887   }
00888 }
00889 
00890 void moveit::core::RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string> &link_names, bool include_attached) const
00891 {
00892   ros::Time tm = ros::Time::now();
00893   for (std::size_t i = 0; i < link_names.size(); ++i)
00894   {
00895     logDebug("Trying to get marker for link '%s'", link_names[i].c_str());
00896     const LinkModel* lm = robot_model_->getLinkModel(link_names[i]);
00897     if (!lm)
00898       continue;
00899     if (include_attached)
00900       for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin() ; it != attached_body_map_.end() ; ++it)
00901         if (it->second->getAttachedLink() == lm)
00902         {
00903           for (std::size_t j = 0 ; j < it->second->getShapes().size() ; ++j)
00904           {
00905             visualization_msgs::Marker att_mark;
00906             att_mark.header.frame_id = robot_model_->getModelFrame();
00907             att_mark.header.stamp = tm;
00908             if (shapes::constructMarkerFromShape(it->second->getShapes()[j].get(), att_mark))
00909             {
00910               // if the object is invisible (0 volume) we skip it
00911               if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<float>::epsilon())
00912                 continue;
00913               tf::poseEigenToMsg(it->second->getGlobalCollisionBodyTransforms()[j], att_mark.pose);
00914               arr.markers.push_back(att_mark);
00915             }
00916           }
00917         }
00918     
00919     if (lm->getShapes().empty())
00920       continue;
00921 
00922     for (std::size_t j = 0 ; j < lm->getShapes().size() ; ++j)
00923     {
00924       visualization_msgs::Marker mark;
00925       mark.header.frame_id = robot_model_->getModelFrame();
00926       mark.header.stamp = tm;
00927       
00928       // we prefer using the visual mesh, if a mesh is available and we have one body to render
00929       const std::string& mesh_resource = lm->getVisualMeshFilename();
00930       if (mesh_resource.empty() || lm->getShapes().size() > 1)
00931       {
00932         if (!shapes::constructMarkerFromShape(lm->getShapes()[j].get(), mark))
00933           continue;
00934         // if the object is invisible (0 volume) we skip it
00935         if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<float>::epsilon())
00936           continue;  
00937         tf::poseEigenToMsg(global_collision_body_transforms_[lm->getFirstCollisionBodyTransformIndex() + j], mark.pose);
00938       }
00939       else
00940       {
00941         mark.type = mark.MESH_RESOURCE;
00942         mark.mesh_use_embedded_materials = false;
00943         mark.mesh_resource = mesh_resource;
00944         const Eigen::Vector3d &mesh_scale = lm->getVisualMeshScale();
00945         
00946         mark.scale.x = mesh_scale[0];
00947         mark.scale.y = mesh_scale[1];
00948         mark.scale.z = mesh_scale[2]; 
00949         tf::poseEigenToMsg(global_link_transforms_[lm->getLinkIndex()] * lm->getVisualMeshOrigin(), mark.pose);
00950       }
00951 
00952       arr.markers.push_back(mark);
00953     }
00954   }
00955 }
00956 
00957 Eigen::MatrixXd moveit::core::RobotState::getJacobian(const JointModelGroup *group, const Eigen::Vector3d &reference_point_position) const
00958 {
00959   Eigen::MatrixXd result;
00960   if (!getJacobian(group, group->getLinkModels().back(), reference_point_position, result, false))
00961     throw Exception("Unable to compute Jacobian");
00962   return result;
00963 }
00964 
00965 bool moveit::core::RobotState::getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position,
00966                                            Eigen::MatrixXd& jacobian, bool use_quaternion_representation) const
00967 {
00968   BOOST_VERIFY(checkLinkTransforms());
00969   
00970   if (!group->isChain())
00971   {
00972     logError("The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str());
00973     return false;
00974   }
00975 
00976   if (!group->isLinkUpdated(link->getName()))
00977   {
00978     logError("Link name '%s' does not exist in the chain '%s' or is not a child for this chain", link->getName().c_str(), group->getName().c_str());
00979     return false;
00980   }
00981 
00982   const robot_model::JointModel* root_joint_model = group->getJointModels()[0];//group->getJointRoots()[0];
00983   const robot_model::LinkModel* root_link_model = root_joint_model->getParentLinkModel();
00984   Eigen::Affine3d reference_transform = root_link_model ? getGlobalLinkTransform(root_link_model).inverse() : Eigen::Affine3d::Identity();
00985   int rows = use_quaternion_representation ? 7 : 6;
00986   int columns = group->getVariableCount();
00987   jacobian = Eigen::MatrixXd::Zero(rows, columns);
00988 
00989   Eigen::Affine3d link_transform = reference_transform * getGlobalLinkTransform(link);
00990   Eigen::Vector3d point_transform = link_transform * reference_point_position;
00991 
00992   /*
00993   logDebug("Point from reference origin expressed in world coordinates: %f %f %f",
00994            point_transform.x(),
00995            point_transform.y(),
00996            point_transform.z());
00997   */
00998 
00999   Eigen::Vector3d joint_axis;
01000   Eigen::Affine3d joint_transform;
01001 
01002   while (link)
01003   {
01004     /*
01005     logDebug("Link: %s, %f %f %f",link_state->getName().c_str(),
01006              link_state->getGlobalLinkTransform().translation().x(),
01007              link_state->getGlobalLinkTransform().translation().y(),
01008              link_state->getGlobalLinkTransform().translation().z());
01009     logDebug("Joint: %s",link_state->getParentJointState()->getName().c_str());
01010     */
01011     const JointModel *pjm = link->getParentJointModel();
01012     if (pjm->getVariableCount() > 0)
01013     {
01014       unsigned int joint_index = group->getVariableGroupIndex(pjm->getName());
01015       if (pjm->getType() == robot_model::JointModel::REVOLUTE)
01016       {
01017         joint_transform = reference_transform * getGlobalLinkTransform(link);
01018         joint_axis = joint_transform.rotation() * static_cast<const robot_model::RevoluteJointModel*>(pjm)->getAxis();
01019         jacobian.block<3,1>(0,joint_index) = jacobian.block<3,1>(0,joint_index) + joint_axis.cross(point_transform - joint_transform.translation());
01020         jacobian.block<3,1>(3,joint_index) = jacobian.block<3,1>(3,joint_index) + joint_axis;
01021       }
01022       else
01023         if (pjm->getType() == robot_model::JointModel::PRISMATIC)
01024         {
01025           joint_transform = reference_transform * getGlobalLinkTransform(link);
01026           joint_axis = joint_transform * static_cast<const robot_model::PrismaticJointModel*>(pjm)->getAxis();
01027           jacobian.block<3,1>(0,joint_index) = jacobian.block<3,1>(0,joint_index) + joint_axis;
01028         }
01029         else
01030           if (pjm->getType() == robot_model::JointModel::PLANAR)
01031           {
01032             joint_transform = reference_transform * getGlobalLinkTransform(link);
01033             joint_axis = joint_transform * Eigen::Vector3d(1.0,0.0,0.0);
01034             jacobian.block<3,1>(0,joint_index) = jacobian.block<3,1>(0,joint_index) + joint_axis;
01035             joint_axis = joint_transform*Eigen::Vector3d(0.0,1.0,0.0);
01036             jacobian.block<3,1>(0,joint_index+1) = jacobian.block<3,1>(0,joint_index+1) + joint_axis;
01037             joint_axis = joint_transform*Eigen::Vector3d(0.0,0.0,1.0);
01038             jacobian.block<3,1>(0,joint_index+2) = jacobian.block<3,1>(0,joint_index+2) + joint_axis.cross(point_transform - joint_transform.translation());
01039             jacobian.block<3,1>(3,joint_index+2) = jacobian.block<3,1>(3,joint_index+2) + joint_axis;
01040           }
01041           else
01042             logError("Unknown type of joint in Jacobian computation");
01043     }
01044     if (pjm == root_joint_model)
01045       break;
01046     link = pjm->getParentLinkModel();
01047   }
01048   if (use_quaternion_representation) 
01049   { // Quaternion representation
01050     // From "Advanced Dynamics and Motion Simulation" by Paul Mitiguy
01051     // d/dt ( [w] ) = 1/2 * [ -x -y -z ]  * [ omega_1 ]
01052     //        [x]           [  w -z  y ]    [ omega_2 ]
01053     //        [y]           [  z  w -x ]    [ omega_3 ]
01054     //        [z]           [ -y  x  w ]
01055     Eigen::Quaterniond q(link_transform.rotation());
01056     double w = q.w(), x = q.x(), y = q.y(), z = q.z();
01057     Eigen::MatrixXd quaternion_update_matrix(4,3);
01058     quaternion_update_matrix << -x, -y, -z,
01059                                  w, -z,  y,
01060                                  z,  w, -x,
01061                                 -y,  x,  w;
01062     jacobian.block(3,0,4,columns) = 0.5*quaternion_update_matrix*jacobian.block(3,0, 3, columns);
01063   }
01064   return true;
01065 }
01066 
01067 bool moveit::core::RobotState::setFromDiffIK(const JointModelGroup *jmg, const Eigen::VectorXd &twist, const std::string &tip,
01068                                              double dt, const GroupStateValidityCallbackFn &constraint)
01069 {
01070   Eigen::VectorXd qdot;
01071   computeVariableVelocity(jmg, qdot, twist, getLinkModel(tip));
01072   return integrateVariableVelocity(jmg, qdot, dt, constraint);
01073 }
01074 
01075 bool moveit::core::RobotState::setFromDiffIK(const JointModelGroup *jmg, const geometry_msgs::Twist &twist, const std::string &tip,
01076                                              double dt, const GroupStateValidityCallbackFn &constraint)
01077 {
01078   Eigen::Matrix<double, 6, 1> t;
01079   tf::twistMsgToEigen(twist, t);
01080   return setFromDiffIK(jmg, t, tip, dt, constraint);
01081 }
01082 
01083 void moveit::core::RobotState::computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot,
01084                                                        const Eigen::VectorXd &twist, const LinkModel *tip) const
01085 {
01086   //Get the Jacobian of the group at the current configuration
01087   Eigen::MatrixXd J(6, jmg->getVariableCount());
01088   Eigen::Vector3d reference_point(0.0, 0.0, 0.0);
01089   getJacobian(jmg, tip, reference_point, J, false);
01090   
01091   //Rotate the jacobian to the end-effector frame
01092   Eigen::Affine3d eMb = getGlobalLinkTransform(tip).inverse();
01093   Eigen::MatrixXd eWb = Eigen::ArrayXXd::Zero(6, 6);
01094   eWb.block(0, 0, 3, 3) = eMb.matrix().block(0, 0, 3, 3);
01095   eWb.block(3, 3, 3, 3) = eMb.matrix().block(0, 0, 3, 3);
01096   J = eWb * J;
01097 
01098   //Do the Jacobian moore-penrose pseudo-inverse
01099   Eigen::JacobiSVD<Eigen::MatrixXd> svdOfJ(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
01100   const Eigen::MatrixXd U = svdOfJ.matrixU();
01101   const Eigen::MatrixXd V = svdOfJ.matrixV();
01102   const Eigen::VectorXd S = svdOfJ.singularValues();
01103   
01104   Eigen::VectorXd Sinv = S;
01105   static const double pinvtoler = std::numeric_limits<float>::epsilon();
01106   double maxsv = 0.0 ;
01107   for (std::size_t i = 0; i < static_cast<std::size_t>(S.rows()); ++i)
01108     if (fabs(S(i)) > maxsv) maxsv = fabs(S(i));
01109   for (std::size_t i = 0; i < static_cast<std::size_t>(S.rows()); ++i)
01110   {
01111     //Those singular values smaller than a percentage of the maximum singular value are removed
01112     if (fabs(S(i)) > maxsv * pinvtoler)
01113       Sinv(i) = 1.0 / S(i);
01114     else Sinv(i) = 0.0;
01115   }
01116   Eigen::MatrixXd Jinv = (V * Sinv.asDiagonal() * U.transpose());
01117   
01118   // Compute joint velocity
01119   qdot = Jinv * twist;
01120 }
01121 
01122 bool moveit::core::RobotState::integrateVariableVelocity(const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt,
01123                                                          const GroupStateValidityCallbackFn &constraint)
01124 {
01125   Eigen::VectorXd q(jmg->getVariableCount());
01126   copyJointGroupPositions(jmg, q);
01127   q = q + dt * qdot;
01128   setJointGroupPositions(jmg, q);
01129   enforceBounds(jmg);
01130   
01131   if (constraint)
01132   {
01133     std::vector<double> values;
01134     copyJointGroupPositions(jmg, values);
01135     return constraint(this, jmg, &values[0]);
01136   }
01137   else
01138     return true;
01139 }
01140 
01141 bool moveit::core::RobotState::setFromIK(const JointModelGroup *jmg, const geometry_msgs::Pose &pose,
01142                                          unsigned int attempts, double timeout,
01143                                          const GroupStateValidityCallbackFn &constraint,
01144                                          const kinematics::KinematicsQueryOptions &options)
01145 {
01146   const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
01147   if (!solver)
01148   {
01149     logError("No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
01150     return false;
01151   }
01152   return setFromIK(jmg, pose, solver->getTipFrame(), attempts, timeout, constraint, options);
01153 }
01154 
01155 bool moveit::core::RobotState::setFromIK(const JointModelGroup *jmg, const geometry_msgs::Pose &pose, const std::string &tip,
01156                                          unsigned int attempts, double timeout,
01157                                          const GroupStateValidityCallbackFn &constraint,
01158                                          const kinematics::KinematicsQueryOptions &options)
01159 {
01160   Eigen::Affine3d mat;
01161   tf::poseMsgToEigen(pose, mat);
01162   static std::vector<double> consistency_limits;
01163   return setFromIK(jmg, mat, tip, consistency_limits, attempts, timeout, constraint, options);
01164 }
01165 
01166 bool moveit::core::RobotState::setFromIK(const JointModelGroup *jmg, const Eigen::Affine3d &pose,
01167                                          unsigned int attempts, double timeout,
01168                                          const GroupStateValidityCallbackFn &constraint,
01169                                          const kinematics::KinematicsQueryOptions &options)
01170 {
01171   const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
01172   if (!solver)
01173   {
01174     logError("No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
01175     return false;
01176   }
01177   static std::vector<double> consistency_limits;
01178   return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, attempts, timeout, constraint, options);
01179 }
01180 
01181 bool moveit::core::RobotState::setFromIK(const JointModelGroup *jmg, const Eigen::Affine3d &pose_in, const std::string &tip_in,
01182                                          unsigned int attempts, double timeout,
01183                                          const GroupStateValidityCallbackFn &constraint,
01184                                          const kinematics::KinematicsQueryOptions &options)
01185 {
01186   static std::vector<double> consistency_limits;
01187   return setFromIK(jmg, pose_in, tip_in, consistency_limits, attempts, timeout, constraint, options);
01188 }
01189 
01190 namespace moveit
01191 {
01192 namespace core
01193 {
01194 namespace
01195 {
01196 bool ikCallbackFnAdapter(RobotState *state, const JointModelGroup *group, const GroupStateValidityCallbackFn &constraint,
01197                          const geometry_msgs::Pose &, const std::vector<double> &ik_sol, moveit_msgs::MoveItErrorCodes &error_code)
01198 {
01199   const std::vector<unsigned int> &bij = group->getKinematicsSolverJointBijection();
01200   std::vector<double> solution(bij.size());
01201   for (std::size_t i = 0 ; i < bij.size() ; ++i)
01202     solution[bij[i]] = ik_sol[i];
01203   if (constraint(state, group, &solution[0]))
01204     error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
01205   else
01206     error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
01207   return true;
01208 }
01209 }
01210 }
01211 }
01212 
01213 bool moveit::core::RobotState::setToIKSolverFrame(Eigen::Affine3d &pose, const kinematics::KinematicsBaseConstPtr& solver)
01214 {
01215   return setToIKSolverFrame( pose, solver->getBaseFrame() );
01216 }
01217 
01218 bool moveit::core::RobotState::setToIKSolverFrame(Eigen::Affine3d &pose, const std::string &ik_frame)
01219 {
01220   // Bring the pose to the frame of the IK solver
01221   if (!Transforms::sameFrame(ik_frame, robot_model_->getModelFrame()))
01222   {
01223     const LinkModel *lm = getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame);
01224     if (!lm)
01225       return false;
01226     pose = getGlobalLinkTransform(lm).inverse() * pose;
01227   }
01228   return true;
01229 }
01230 
01231 bool moveit::core::RobotState::setFromIK(const JointModelGroup *jmg, const Eigen::Affine3d &pose_in, const std::string &tip_in,
01232                                          const std::vector<double> &consistency_limits_in, unsigned int attempts, double timeout,
01233                                          const GroupStateValidityCallbackFn &constraint, const kinematics::KinematicsQueryOptions &options)
01234 {
01235   // Convert from single pose and tip to vectors
01236   EigenSTL::vector_Affine3d poses;
01237   poses.push_back(pose_in);
01238 
01239   std::vector<std::string> tips;
01240   tips.push_back(tip_in);
01241 
01242   std::vector<std::vector<double> > consistency_limits;
01243   consistency_limits.push_back(consistency_limits_in);
01244 
01245   return setFromIK(jmg, poses, tips, consistency_limits, attempts, timeout, constraint, options);
01246 }
01247 
01248 bool moveit::core::RobotState::setFromIK(const JointModelGroup *jmg, const EigenSTL::vector_Affine3d &poses_in, const std::vector<std::string> &tips_in,
01249                                          unsigned int attempts, double timeout,
01250                                          const GroupStateValidityCallbackFn &constraint, const kinematics::KinematicsQueryOptions &options)
01251 {
01252   static const std::vector<std::vector<double> > consistency_limits;
01253   return setFromIK(jmg, poses_in, tips_in, consistency_limits, attempts, timeout, constraint, options);
01254 }
01255 
01256 bool moveit::core::RobotState::setFromIK(const JointModelGroup *jmg, const EigenSTL::vector_Affine3d &poses_in, const std::vector<std::string> &tips_in,
01257                                          const std::vector<std::vector<double> > &consistency_limit_sets,
01258                                          unsigned int attempts, double timeout,
01259                                          const GroupStateValidityCallbackFn &constraint, const kinematics::KinematicsQueryOptions &options)
01260 {
01261   // Error check
01262   if (poses_in.size() != tips_in.size())
01263   {
01264     logError("moveit.robot_state: Number of poses must be the same as number of tips");
01265     return false;
01266   }
01267 
01268   // Load solver
01269   const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
01270 
01271   // Check if this jmg has a solver
01272   bool valid_solver = true;
01273   if(!solver)
01274   {
01275     valid_solver = false;
01276   }
01277   // Check if this jmg's IK solver can handle multiple tips (non-chain solver)
01278   else if (poses_in.size() > 1)
01279   {
01280     std::string error_msg;
01281     if(!solver->supportsGroup(jmg, &error_msg))
01282     {
01283       logError("moveit.robot_state: Kinematics solver %s does not support joint group %s.  Error: %s",
01284         typeid(*solver).name(), jmg->getName().c_str(), error_msg.c_str());
01285       valid_solver = false;
01286     }
01287   }
01288 
01289   if (!valid_solver)
01290   {
01291     // Check if there are subgroups that can solve this for us (non-chains)
01292     if (poses_in.size() > 1)
01293     {
01294       // Forward to setFromIKSubgroups() to allow different subgroup IK solvers to work together
01295       return setFromIKSubgroups(jmg, poses_in, tips_in, consistency_limit_sets, attempts, timeout, constraint, options);
01296     }
01297     else
01298     {
01299       logError("moveit.robot_state: No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
01300       return false;
01301     }
01302   }
01303 
01304   // Check that no, or only one set of consistency limits have been passed in, and choose that one
01305   std::vector<double> consistency_limits;
01306   if (consistency_limit_sets.size() > 1)
01307   {
01308     logError("moveit.robot_state: Invalid number (%d) of sets of consistency limits for a setFromIK request that is being solved by a single IK solver",
01309       consistency_limit_sets.size());
01310     return false;
01311   }
01312   else if (consistency_limit_sets.size() == 1)
01313     consistency_limits = consistency_limit_sets[0];
01314 
01315   const std::vector<std::string> &solver_tip_frames = solver->getTipFrames();
01316 
01317   // Track which possible tips frames we have filled in so far
01318   std::vector<bool> tip_frames_used (solver_tip_frames.size(), false);
01319 
01320   // Create vector to hold the output frames in the same order as solver_tip_frames
01321   std::vector<geometry_msgs::Pose> ik_queries(solver_tip_frames.size());
01322 
01323   // Bring each pose to the frame of the IK solver
01324   for (std::size_t i = 0; i < poses_in.size(); ++i)
01325   {
01326     // Make non-const
01327     Eigen::Affine3d pose = poses_in[i];
01328     std::string tip = tips_in[i];
01329 
01330     // Remove extra slash
01331     if (!tip.empty() && tip[0] == '/')
01332       tip = tip.substr(1);
01333 
01334     // bring the pose to the frame of the IK solver
01335     if (!setToIKSolverFrame(pose, solver))
01336       return false;
01337 
01338     // try all of the solver's possible tip frames to see if they uniquely align with any of our passed in tip frames
01339     bool found_valid_frame = false;
01340     std::size_t tip_id; // our current index
01341     for (tip_id = 0; tip_id < solver_tip_frames.size(); ++tip_id)
01342     {
01343       // Check if this tip frame is already accounted for
01344       if (tip_frames_used[tip_id] == true)
01345         continue; // already has a pose
01346 
01347       // check if the tip frame can be transformed via fixed transforms to the frame known to the IK solver
01348       std::string tip_frame = solver_tip_frames[tip_id];
01349 
01350       // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings more often that we need to
01351       if (!tip_frame.empty() && tip_frame[0] == '/')
01352         tip_frame = tip_frame.substr(1);
01353 
01354       if (tip != tip_frame)
01355       {
01356         if (hasAttachedBody(tip))
01357         {
01358           const AttachedBody *ab = getAttachedBody(tip);
01359           const EigenSTL::vector_Affine3d &ab_trans = ab->getFixedTransforms();
01360           if (ab_trans.size() != 1)
01361           {
01362             logError("moveit.robot_state: Cannot use an attached body with multiple geometries as a reference frame.");
01363             return false;
01364           }
01365           tip = ab->getAttachedLinkName();
01366           pose = pose * ab_trans[0].inverse();
01367         }
01368         if (tip != tip_frame)
01369         {
01370           const robot_model::LinkModel *lm = getLinkModel(tip);
01371           if (!lm)
01372             return false;
01373           const robot_model::LinkTransformMap &fixed_links = lm->getAssociatedFixedTransforms();
01374           for (robot_model::LinkTransformMap::const_iterator it = fixed_links.begin() ; it != fixed_links.end() ; ++it)
01375             if (Transforms::sameFrame(it->first->getName(), tip_frame))
01376             {
01377               tip = tip_frame;
01378               pose = pose * it->second;
01379               break;
01380             }
01381         }
01382 
01383       } // end if tip
01384 
01385         // Check if this tip frame works
01386       if (tip == tip_frame)
01387       {
01388         found_valid_frame = true;
01389         break;
01390       }
01391 
01392     } // end for solver_tip_frames
01393 
01394     // Make sure one of the tip frames worked
01395     if (!found_valid_frame)
01396     {
01397       logError("moveit.robot_state: Cannot compute IK for tip reference frame '%s'", tip.c_str());
01398       return false;
01399     }
01400 
01401     // Remove that tip from the list of available tip frames because each can only have one pose
01402     tip_frames_used[tip_id] = true;
01403 
01404     // Convert Eigen pose to geometry_msgs pose
01405     geometry_msgs::Pose ik_query;
01406     tf::poseEigenToMsg(pose, ik_query);
01407 
01408     // Save into vectors
01409     ik_queries[tip_id] = ik_query;
01410   } // end for poses_in
01411 
01412   // Create poses for all remaining tips a solver expects, even if not passed into this function
01413   for (std::size_t tip_id = 0; tip_id < solver_tip_frames.size(); ++tip_id)
01414   {
01415     // Check if this tip frame is already accounted for
01416     if (tip_frames_used[tip_id] == true)
01417       continue; // already has a pose
01418 
01419     // Process this tip
01420     std::string tip_frame = solver_tip_frames[tip_id];
01421 
01422     // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings more often that we need to
01423     if (!tip_frame.empty() && tip_frame[0] == '/')
01424       tip_frame = tip_frame.substr(1);
01425 
01426     // Get the pose of a different EE tip link
01427     Eigen::Affine3d current_pose = getGlobalLinkTransform(tip_frame);
01428 
01429     // bring the pose to the frame of the IK solver
01430     if (!setToIKSolverFrame(current_pose, solver))
01431       return false;
01432 
01433     // Convert Eigen pose to geometry_msgs pose
01434     geometry_msgs::Pose ik_query;
01435     tf::poseEigenToMsg(current_pose, ik_query);
01436 
01437     // Save into vectors - but this needs to be ordered in the same order as the IK solver expects its tip frames
01438     ik_queries[tip_id] = ik_query;
01439 
01440     // Remove that tip from the list of available tip frames because each can only have one pose
01441     tip_frames_used[tip_id] = true;
01442   }
01443 
01444   // if no timeout has been specified, use the default one
01445   if (timeout < std::numeric_limits<double>::epsilon())
01446     timeout = jmg->getDefaultIKTimeout();
01447 
01448   if (attempts == 0)
01449     attempts = jmg->getDefaultIKAttempts();
01450 
01451   // set callback function
01452   kinematics::KinematicsBase::IKCallbackFn ik_callback_fn;
01453   if (constraint)
01454     ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
01455 
01456   // Bijection
01457   const std::vector<unsigned int> &bij = jmg->getKinematicsSolverJointBijection();
01458 
01459   bool first_seed = true;
01460   std::vector<double> initial_values;
01461   for (unsigned int st = 0 ; st < attempts ; ++st)
01462   {
01463     std::vector<double> seed(bij.size());
01464 
01465     // the first seed is the current robot state joint values
01466     if (first_seed)
01467     {
01468       first_seed = false;
01469       copyJointGroupPositions(jmg, initial_values);
01470       for (std::size_t i = 0 ; i < bij.size() ; ++i)
01471         seed[i] = initial_values[bij[i]];
01472     }
01473     else
01474     {
01475       logDebug("moveit.robot_state: Rerunning IK solver with random joint positions");
01476 
01477       // sample a random seed
01478       random_numbers::RandomNumberGenerator &rng = getRandomNumberGenerator();
01479       std::vector<double> random_values;
01480       jmg->getVariableRandomPositions(rng, random_values);
01481       for (std::size_t i = 0 ; i < bij.size() ; ++i)
01482         seed[i] = random_values[bij[i]];
01483       
01484       if (options.lock_redundant_joints)
01485       {
01486         std::vector<unsigned int> red_joints;
01487         solver->getRedundantJoints(red_joints);
01488         copyJointGroupPositions(jmg, initial_values);
01489         for(std::size_t i = 0 ; i < red_joints.size(); ++i)
01490           seed[red_joints[i]] = initial_values[bij[red_joints[i]]];
01491       }
01492     }
01493     
01494     // compute the IK solution
01495     std::vector<double> ik_sol;
01496     moveit_msgs::MoveItErrorCodes error;
01497 
01498     if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options, this))
01499     {
01500       std::vector<double> solution(bij.size());
01501       for (std::size_t i = 0 ; i < bij.size() ; ++i)
01502         solution[bij[i]] = ik_sol[i];
01503       setJointGroupPositions(jmg, solution);
01504       return true;
01505     }
01506   }
01507   return false;
01508 }
01509 
01510 bool moveit::core::RobotState::setFromIKSubgroups(const JointModelGroup *jmg, const EigenSTL::vector_Affine3d &poses_in, 
01511                                                   const std::vector<std::string> &tips_in,
01512                                                   const std::vector<std::vector<double> > &consistency_limits,
01513                                                   unsigned int attempts, double timeout,
01514                                                   const GroupStateValidityCallbackFn &constraint, const kinematics::KinematicsQueryOptions &options)
01515 {
01516   // Assume we have already ran setFromIK() and those checks
01517 
01518   // Get containing subgroups
01519   std::vector<const JointModelGroup*> sub_groups;
01520   jmg->getSubgroups(sub_groups);
01521 
01522   // Error check
01523   if (poses_in.size() != sub_groups.size())
01524   {
01525     logError("Number of poses must be the same as number of sub-groups");
01526     return false;
01527   }
01528 
01529   if (tips_in.size() != sub_groups.size())
01530   {
01531     logError("Number of tip names must be the same as number of sub-groups");
01532     return false;
01533   }
01534 
01535   if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
01536   {
01537     logError("Number of consistency limit vectors must be the same as number of sub-groups");
01538     return false;
01539   }
01540   
01541   for (std::size_t i = 0 ; i < consistency_limits.size() ; ++i)
01542   {
01543     if (consistency_limits[i].size() != sub_groups[i]->getVariableCount())
01544     {
01545       logError("Number of joints in consistency_limits is %u but it should be should be %u", (unsigned int)i, sub_groups[i]->getVariableCount());
01546       return false;
01547     }
01548   }
01549 
01550   // Populate list of kin solvers for the various subgroups
01551   std::vector<kinematics::KinematicsBaseConstPtr> solvers;
01552   for (std::size_t i = 0; i < poses_in.size() ; ++i)
01553   {
01554     kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
01555     if (!solver)
01556     {
01557       logError("Could not find solver for group '%s'", sub_groups[i]->getName().c_str());
01558       return false;
01559     }
01560     solvers.push_back(solver);
01561   }
01562   
01563   // Make non-const versions
01564   EigenSTL::vector_Affine3d transformed_poses = poses_in;
01565   std::vector<std::string> tip_names = tips_in;
01566 
01567   // Each each pose's tip frame naming
01568   for (std::size_t i = 0 ; i < poses_in.size() ; ++i)
01569   {
01570     Eigen::Affine3d &pose = transformed_poses[i];
01571     std::string &tip = tip_names[i];
01572 
01573     // bring the pose to the frame of the IK solver
01574     if (!setToIKSolverFrame(pose, solvers[i]))
01575       return false;
01576 
01577     // see if the tip frame can be transformed via fixed transforms to the frame known to the IK solver
01578     std::string tip_frame = solvers[i]->getTipFrame();
01579 
01580     // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings more often that we need to
01581     if (!tip_frame.empty() && tip_frame[0] == '/')
01582       tip_frame = tip_frame.substr(1);
01583 
01584     if (tip != tip_frame)
01585     {
01586       if (hasAttachedBody(tip))
01587       {
01588         const AttachedBody *ab = getAttachedBody(tip);
01589         const EigenSTL::vector_Affine3d &ab_trans = ab->getFixedTransforms();
01590         if (ab_trans.size() != 1)
01591         {
01592           logError("Cannot use an attached body with multiple geometries as a reference frame.");
01593           return false;
01594         }
01595         tip = ab->getAttachedLinkName();
01596         pose = pose * ab_trans[0].inverse();
01597       }
01598       if (tip != tip_frame)
01599       {
01600         const robot_model::LinkModel *lm = getLinkModel(tip);
01601         if (!lm)
01602           return false;
01603         const robot_model::LinkTransformMap &fixed_links = lm->getAssociatedFixedTransforms();
01604         for (robot_model::LinkTransformMap::const_iterator it = fixed_links.begin() ; it != fixed_links.end() ; ++it)
01605           if (it->first->getName() == tip_frame)
01606           {
01607             tip = tip_frame;
01608             pose = pose * it->second;
01609             break;
01610           }
01611       }
01612     }
01613 
01614     if (tip != tip_frame)
01615     {
01616       logError("Cannot compute IK for tip reference frame '%s'", tip.c_str());
01617       return false;
01618     }
01619   }
01620 
01621   // Convert Eigen poses to geometry_msg format
01622   std::vector<geometry_msgs::Pose> ik_queries(poses_in.size());
01623   kinematics::KinematicsBase::IKCallbackFn ik_callback_fn;
01624   if (constraint)
01625     ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
01626 
01627   for (std::size_t i = 0; i < transformed_poses.size() ; ++i)
01628   {
01629     Eigen::Quaterniond quat(transformed_poses[i].rotation());
01630     Eigen::Vector3d point(transformed_poses[i].translation());
01631     ik_queries[i].position.x = point.x();
01632     ik_queries[i].position.y = point.y();
01633     ik_queries[i].position.z = point.z();
01634     ik_queries[i].orientation.x = quat.x();
01635     ik_queries[i].orientation.y = quat.y();
01636     ik_queries[i].orientation.z = quat.z();
01637     ik_queries[i].orientation.w = quat.w();
01638   }
01639 
01640   if (attempts == 0)
01641     attempts = jmg->getDefaultIKAttempts();
01642 
01643   // if no timeout has been specified, use the default one
01644   if (timeout < std::numeric_limits<double>::epsilon())
01645     timeout = jmg->getDefaultIKTimeout();
01646 
01647   bool first_seed = true;
01648   for (unsigned int st = 0 ; st < attempts ; ++st)
01649   {
01650     bool found_solution = true;
01651     for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
01652     {
01653       const std::vector<unsigned int>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
01654       std::vector<double> seed(bij.size());
01655        // the first seed is the initial state
01656       if (first_seed)
01657       {
01658         std::vector<double> initial_values;
01659         copyJointGroupPositions(sub_groups[sg], initial_values);
01660         for (std::size_t i = 0 ; i < bij.size() ; ++i)
01661           seed[i] = initial_values[bij[i]];
01662       }
01663       else
01664       {
01665         // sample a random seed
01666         random_numbers::RandomNumberGenerator &rng = getRandomNumberGenerator();
01667         std::vector<double> random_values;
01668         sub_groups[sg]->getVariableRandomPositions(rng, random_values);
01669         for (std::size_t i = 0 ; i < bij.size() ; ++i)
01670           seed[i] = random_values[bij[i]];
01671       }
01672 
01673       // compute the IK solution
01674       std::vector<double> ik_sol;
01675       moveit_msgs::MoveItErrorCodes error;
01676       const std::vector<double> &climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
01677       if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, timeout, climits, ik_sol, error))
01678       {
01679         std::vector<double> solution(bij.size());
01680         for (std::size_t i = 0 ; i < bij.size() ; ++i)
01681           solution[bij[i]] = ik_sol[i];
01682         setJointGroupPositions(sub_groups[sg], solution);
01683       }
01684       else
01685       {
01686         found_solution = false;
01687         break;
01688       }
01689       logDebug("IK attempt: %d of %d", st, attempts);
01690     }
01691     if (found_solution)
01692     {
01693       std::vector<double> full_solution;
01694       copyJointGroupPositions(jmg, full_solution);
01695       if (constraint ? constraint(this, jmg, &full_solution[0]) : true)
01696       {
01697         logDebug("Found IK solution");
01698         return true;
01699       }
01700     }
01701   }
01702 }
01703 
01704 double moveit::core::RobotState::computeCartesianPath(const JointModelGroup *group, std::vector<RobotStatePtr> &traj, const LinkModel *link,
01705                                                       const Eigen::Vector3d &direction, bool global_reference_frame, double distance, double max_step, double jump_threshold,
01706                                                       const GroupStateValidityCallbackFn &validCallback,
01707                                                       const kinematics::KinematicsQueryOptions &options)
01708 {
01709   //this is the Cartesian pose we start from, and have to move in the direction indicated
01710   const Eigen::Affine3d &start_pose = getGlobalLinkTransform(link);
01711   
01712   //the direction can be in the local reference frame (in which case we rotate it)
01713   const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.rotation() * direction;
01714 
01715   //The target pose is built by applying a translation to the start pose for the desired direction and distance
01716   Eigen::Affine3d target_pose = start_pose;
01717   target_pose.translation() += rotated_direction * distance;
01718 
01719   //call computeCartesianPath for the computed target pose in the global reference frame
01720   return (distance * computeCartesianPath(group, traj, link, target_pose, true, max_step, jump_threshold, validCallback, options));
01721 }
01722 
01723 double moveit::core::RobotState::computeCartesianPath(const JointModelGroup *group, std::vector<RobotStatePtr> &traj, const LinkModel *link,
01724                                                       const Eigen::Affine3d &target, bool global_reference_frame, double max_step, double jump_threshold,
01725                                                       const GroupStateValidityCallbackFn &validCallback,
01726                                                       const kinematics::KinematicsQueryOptions &options)
01727 {
01728   const std::vector<const JointModel*> &cjnt = group->getContinuousJointModels();
01729   // make sure that continuous joints wrap
01730   for (std::size_t i = 0 ; i < cjnt.size() ; ++i)
01731     enforceBounds(cjnt[i]);
01732 
01733   // this is the Cartesian pose we start from, and we move in the direction indicated
01734   Eigen::Affine3d start_pose = getGlobalLinkTransform(link);
01735 
01736   // the target can be in the local reference frame (in which case we rotate it)
01737   Eigen::Affine3d rotated_target = global_reference_frame ? target : start_pose * target;
01738   
01739   bool test_joint_space_jump = jump_threshold > 0.0;
01740   
01741   // decide how many steps we will need for this trajectory
01742   double distance = (rotated_target.translation() - start_pose.translation()).norm();
01743   unsigned int steps = (test_joint_space_jump ? 5 : 1) + (unsigned int)floor(distance / max_step);
01744   
01745   traj.clear();
01746   traj.push_back(RobotStatePtr(new RobotState(*this)));
01747   
01748   std::vector<double> dist_vector;
01749   double total_dist = 0.0;
01750   
01751   double last_valid_percentage = 0.0;
01752   Eigen::Quaterniond start_quaternion(start_pose.rotation());
01753   Eigen::Quaterniond target_quaternion(rotated_target.rotation());
01754   for (unsigned int i = 1; i <= steps ; ++i)
01755   {
01756     double percentage = (double)i / (double)steps;
01757 
01758     Eigen::Affine3d pose(start_quaternion.slerp(percentage, target_quaternion));
01759     pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
01760 
01761     if (setFromIK(group, pose, link->getName(), 1, 0.0, validCallback, options))
01762     {
01763       traj.push_back(RobotStatePtr(new RobotState(*this)));
01764 
01765       // compute the distance to the previous point (infinity norm)
01766       if (test_joint_space_jump)
01767       {
01768         double dist_prev_point = traj.back()->distance(*traj[traj.size() - 2], group);
01769         dist_vector.push_back(dist_prev_point);
01770         total_dist += dist_prev_point;
01771       }
01772     }
01773     else
01774       break;
01775     last_valid_percentage = percentage;
01776   }
01777 
01778   if (test_joint_space_jump)
01779   {
01780     // compute the average distance between the states we looked at
01781     double thres = jump_threshold * (total_dist / (double)dist_vector.size());
01782     for (std::size_t i = 0 ; i < dist_vector.size() ; ++i)
01783       if (dist_vector[i] > thres)
01784       {
01785         logDebug("Truncating Cartesian path due to detected jump in joint-space distance");
01786         last_valid_percentage = (double)i / (double)steps;
01787         traj.resize(i);
01788         break;
01789       }
01790   }
01791 
01792   return last_valid_percentage;
01793 }
01794 
01795 double moveit::core::RobotState::computeCartesianPath(const JointModelGroup *group, std::vector<RobotStatePtr> &traj, const LinkModel *link,
01796                                                       const EigenSTL::vector_Affine3d &waypoints, bool global_reference_frame, double max_step, double jump_threshold,
01797                                                       const GroupStateValidityCallbackFn &validCallback,
01798                                                       const kinematics::KinematicsQueryOptions &options)
01799 {
01800   double percentage_solved = 0.0;
01801   for (std::size_t i = 0; i < waypoints.size(); ++i)
01802   {
01803     std::vector<RobotStatePtr> waypoint_traj;
01804     double wp_percentage_solved = computeCartesianPath(group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, jump_threshold, validCallback, options);
01805     if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
01806     {
01807       percentage_solved = (double)(i + 1) / (double)waypoints.size();
01808       std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
01809       if(i > 0 && !waypoint_traj.empty())
01810         std::advance(start, 1);
01811       traj.insert(traj.end(), start, waypoint_traj.end());
01812     }
01813     else
01814     {
01815       percentage_solved += wp_percentage_solved / (double)waypoints.size();
01816       std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
01817       if(i > 0 && !waypoint_traj.empty())
01818         std::advance(start, 1);
01819       traj.insert(traj.end(), start, waypoint_traj.end());
01820       break;
01821     }
01822   }
01823   
01824   return percentage_solved;
01825 }
01826 
01827 namespace
01828 {
01829 static inline void updateAABB(const Eigen::Affine3d &t, const Eigen::Vector3d &e, std::vector<double> &aabb)
01830 {
01831   Eigen::Vector3d v = e / 2.0;
01832   Eigen::Vector3d c2 = t * v;
01833   v = -v;
01834   Eigen::Vector3d c1 = t * v;
01835   if (aabb.empty())
01836   {
01837     aabb.resize(6);
01838     aabb[0] = c1.x();
01839     aabb[2] = c1.y();
01840     aabb[4] = c1.z();
01841     aabb[1] = c2.x();
01842     aabb[3] = c2.y();
01843     aabb[5] = c2.z();
01844   }
01845   else
01846   {
01847     if (aabb[0] > c1.x())
01848       aabb[0] = c1.x();
01849     if (aabb[2] > c1.y())
01850       aabb[2] = c1.y();
01851     if (aabb[4] > c1.z())
01852       aabb[4] = c1.z();
01853     if (aabb[1] < c2.x())
01854       aabb[1] = c2.x();
01855     if (aabb[3] < c2.y())
01856       aabb[3] = c2.y();
01857     if (aabb[5] < c2.z())
01858       aabb[5] = c2.z();
01859   }
01860 }
01861 }
01862 
01863 void robot_state::RobotState::computeAABB(std::vector<double> &aabb) const
01864 {
01865   BOOST_VERIFY(checkLinkTransforms());
01866   
01867   aabb.clear();
01868   std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
01869   for (std::size_t i = 0 ; i < links.size() ; ++i)
01870   {
01871     const Eigen::Affine3d &t = getGlobalLinkTransform(links[i]);
01872     const Eigen::Vector3d &e = links[i]->getShapeExtentsAtOrigin();
01873     updateAABB(t, e, aabb);
01874   }
01875   for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin() ; it != attached_body_map_.end() ; ++it)
01876   {
01877     const EigenSTL::vector_Affine3d &ts = it->second->getGlobalCollisionBodyTransforms();
01878     const std::vector<shapes::ShapeConstPtr> &ss = it->second->getShapes();
01879     for (std::size_t i = 0 ; i < ts.size() ; ++i)
01880     {
01881       Eigen::Vector3d e = shapes::computeShapeExtents(ss[i].get());
01882       updateAABB(ts[i], e, aabb);
01883     }
01884   }
01885   if (aabb.empty())
01886     aabb.resize(6, 0.0);
01887 }
01888 
01889 void moveit::core::RobotState::printStatePositions(std::ostream &out) const
01890 {
01891   const std::vector<std::string> &nm = robot_model_->getVariableNames();
01892   for (std::size_t i = 0 ; i < nm.size() ; ++i)
01893     out << nm[i] << "=" << position_[i] << std::endl;
01894 }
01895 
01896 void moveit::core::RobotState::printDirtyInfo(std::ostream &out) const
01897 {
01898   out << "  * Dirty Joint Transforms: " << std::endl;
01899   const std::vector<const JointModel*> &jm = robot_model_->getJointModels();
01900   for (std::size_t i = 0 ; i < jm.size() ; ++i)
01901     if (jm[i]->getVariableCount() > 0 && dirtyJointTransform(jm[i]))
01902       out << "    " << jm[i]->getName() << std::endl;
01903   out << "  * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL") << std::endl;
01904   out << "  * Dirty Collision Body Transforms: " << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->getName() : "NULL") << std::endl;
01905 }
01906 
01907 void moveit::core::RobotState::printStateInfo(std::ostream &out) const
01908 {
01909   out << "Robot State @" << this << std::endl;
01910   
01911   std::size_t n = robot_model_->getVariableCount();
01912   if (position_)
01913   {
01914     out << "  * Position: ";
01915     for (std::size_t i = 0 ; i < n ; ++i)
01916       out << position_[i] << " ";
01917     out << std::endl;
01918   }
01919   else
01920     out << "  * Position: NULL" << std::endl;
01921   
01922   if (velocity_)
01923   {
01924     out << "  * Velocity: ";
01925     for (std::size_t i = 0 ; i < n ; ++i)
01926       out << velocity_[i] << " ";
01927     out << std::endl;
01928   }
01929   else
01930     out << "  * Velocity: NULL" << std::endl;
01931 
01932   if (acceleration_)
01933   {
01934     out << "  * Acceleration: ";
01935     for (std::size_t i = 0 ; i < n ; ++i)
01936       out << acceleration_[i] << " ";
01937     out << std::endl;
01938   }
01939   else
01940     out << "  * Acceleration: NULL" << std::endl;
01941   
01942   out << "  * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL") << std::endl;
01943   out << "  * Dirty Collision Body Transforms: " << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->getName() : "NULL") << std::endl;
01944   
01945   printTransforms(out);
01946 }
01947 
01948 void moveit::core::RobotState::printTransform(const Eigen::Affine3d &transform, std::ostream &out) const
01949 {
01950   Eigen::Quaterniond q(transform.rotation());  
01951   out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " << transform.translation().z() << "], Q.xyzw = ["
01952       << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "]" << std::endl;
01953 }
01954 
01955 void moveit::core::RobotState::printTransforms(std::ostream &out) const
01956 {
01957   if (!variable_joint_transforms_)
01958   {
01959     out << "No transforms computed" << std::endl;
01960     return;
01961   }
01962   
01963   out << "Joint transforms:" << std::endl;
01964   const std::vector<const JointModel*> &jm = robot_model_->getJointModels();
01965   for (std::size_t i = 0 ; i < jm.size() ; ++i)
01966   {
01967     out << "  " << jm[i]->getName();
01968     const int idx = jm[i]->getJointIndex();
01969     if (dirty_joint_transforms_[idx])
01970       out << " [dirty]";
01971     out << ": ";
01972     printTransform(variable_joint_transforms_[idx], out);
01973   }
01974   
01975   out << "Link poses:" << std::endl;
01976   const std::vector<const LinkModel*> &lm = robot_model_->getLinkModels();
01977   for (std::size_t i = 0 ; i < lm.size() ; ++i)
01978   {
01979     out << "  " << lm[i]->getName() << ": ";
01980     printTransform(global_link_transforms_[lm[i]->getLinkIndex()], out);
01981   }
01982 }
01983 
01984 std::string moveit::core::RobotState::getStateTreeString(const std::string& prefix) const
01985 {
01986   std::stringstream ss;
01987   ss << "ROBOT: " << robot_model_->getName() << std::endl;
01988   getStateTreeJointString(ss, robot_model_->getRootJoint(), "   ", true);
01989   return ss.str();
01990 }
01991 
01992 namespace
01993 {
01994 void getPoseString(std::ostream& ss, const Eigen::Affine3d& pose, const std::string& pfx)
01995 {
01996   ss.precision(3);
01997   for (int y = 0 ; y < 4 ; ++y)
01998   {
01999     ss << pfx;
02000     for (int x = 0 ; x < 4 ; ++x)
02001     {
02002       ss << std::setw(8) << pose(y, x) << " ";
02003     }
02004     ss << std::endl;
02005   }
02006 }
02007 }
02008 
02009 void moveit::core::RobotState::getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const
02010 {
02011   std::string pfx = pfx0 + "+--";
02012   
02013   ss << pfx << "Joint: " << jm->getName() << std::endl;
02014 
02015   pfx = pfx0 + (last ? "   " : "|  ");
02016 
02017   for (std::size_t i = 0 ; i < jm->getVariableCount(); ++i)
02018   {
02019     ss.precision(3);
02020     ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] << std::endl;
02021   }
02022 
02023   const LinkModel* lm = jm->getChildLinkModel();
02024 
02025   ss << pfx << "Link: " << lm->getName() << std::endl;
02026   getPoseString(ss, lm->getJointOriginTransform(), pfx + "joint_origin:");
02027   if (variable_joint_transforms_)
02028   {
02029     getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx + "joint_variable:");
02030     getPoseString(ss, global_link_transforms_[lm->getLinkIndex()], pfx + "link_global:");
02031   }
02032   
02033   for (std::vector<const JointModel*>::const_iterator it = lm->getChildJointModels().begin() ; it != lm->getChildJointModels().end() ; ++it)
02034     getStateTreeJointString(ss, *it, pfx, it + 1 == lm->getChildJointModels().end());
02035 }
02036 
02037 
02038 std::ostream& moveit::core::operator<<(std::ostream &out, const RobotState &s)
02039 {
02040   s.printStateInfo(out);
02041   return out;  
02042 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53