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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Apr 23 2018 10:24:45