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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Dec 26 2017 03:33:17