00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <moveit/robot_state/robot_state.h>
00039 #include <moveit/transforms/transforms.h>
00040 #include <geometric_shapes/shape_operations.h>
00041 #include <eigen_conversions/eigen_msg.h>
00042 #include <moveit/backtrace/backtrace.h>
00043 #include <moveit/profiler/profiler.h>
00044 #include <boost/bind.hpp>
00045
00046 moveit::core::RobotState::RobotState(const RobotModelConstPtr& robot_model)
00047 : robot_model_(robot_model)
00048 , has_velocity_(false)
00049 , has_acceleration_(false)
00050 , has_effort_(false)
00051 , dirty_link_transforms_(robot_model_->getRootJoint())
00052 , dirty_collision_body_transforms_(NULL)
00053 , rng_(NULL)
00054 {
00055 allocMemory();
00056
00057
00058 const int nr_doubles_for_dirty_joint_transforms =
00059 1 + robot_model_->getJointModelCount() / (sizeof(double) / sizeof(unsigned char));
00060 memset(dirty_joint_transforms_, 1, sizeof(double) * nr_doubles_for_dirty_joint_transforms);
00061 }
00062
00063 moveit::core::RobotState::RobotState(const RobotState& other) : rng_(NULL)
00064 {
00065 robot_model_ = other.robot_model_;
00066 allocMemory();
00067 copyFrom(other);
00068 }
00069
00070 moveit::core::RobotState::~RobotState()
00071 {
00072 clearAttachedBodies();
00073 free(memory_);
00074 if (rng_)
00075 delete rng_;
00076 }
00077
00078 void moveit::core::RobotState::allocMemory(void)
00079 {
00080
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
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
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
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
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
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
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
00218 }
00219
00220 void moveit::core::RobotState::setToRandomPositions(const JointModelGroup* group)
00221 {
00222
00223
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
00241
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
00259
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);
00276 setVariablePositions(m);
00277 return r;
00278 }
00279
00280 void moveit::core::RobotState::setToDefaultValues()
00281 {
00282 robot_model_->getVariableDefaultPositions(position_);
00283
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
00292 memcpy(position_, position, robot_model_->getVariableCount() * sizeof(double));
00293
00294
00295
00296
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
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
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
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
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();
00651
00652
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
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
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
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
00684
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
00691
00692
00693
00694
00695
00696 }
00697
00698
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
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
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
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];
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
01130
01131
01132
01133
01134
01135 Eigen::Vector3d joint_axis;
01136 Eigen::Affine3d joint_transform;
01137
01138 while (link)
01139 {
01140
01141
01142
01143
01144
01145
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 {
01186
01187
01188
01189
01190
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
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
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
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
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
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
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
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
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
01412 const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
01413
01414
01415 bool valid_solver = true;
01416 if (!solver)
01417 {
01418 valid_solver = false;
01419 }
01420
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
01435 if (poses_in.size() > 1)
01436 {
01437
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
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
01462 std::vector<bool> tip_frames_used(solver_tip_frames.size(), false);
01463
01464
01465 std::vector<geometry_msgs::Pose> ik_queries(solver_tip_frames.size());
01466
01467
01468 for (std::size_t i = 0; i < poses_in.size(); ++i)
01469 {
01470
01471 Eigen::Affine3d pose = poses_in[i];
01472 std::string pose_frame = tips_in[i];
01473
01474
01475 if (!pose_frame.empty() && pose_frame[0] == '/')
01476 pose_frame = pose_frame.substr(1);
01477
01478
01479 if (!setToIKSolverFrame(pose, solver))
01480 return false;
01481
01482
01483
01484 bool found_valid_frame = false;
01485 std::size_t solver_tip_id;
01486 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
01487 {
01488
01489 if (tip_frames_used[solver_tip_id] == true)
01490 continue;
01491
01492
01493 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
01494
01495
01496
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 }
01530
01531
01532 if (pose_frame == solver_tip_frame)
01533 {
01534 found_valid_frame = true;
01535 break;
01536 }
01537
01538 }
01539
01540
01541 if (!found_valid_frame)
01542 {
01543 logError("moveit.robot_state: Cannot compute IK for query pose reference frame '%s'", pose_frame.c_str());
01544 return false;
01545 }
01546
01547
01548 tip_frames_used[solver_tip_id] = true;
01549
01550
01551 geometry_msgs::Pose ik_query;
01552 tf::poseEigenToMsg(pose, ik_query);
01553
01554
01555 ik_queries[solver_tip_id] = ik_query;
01556 }
01557
01558
01559 for (std::size_t solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
01560 {
01561
01562 if (tip_frames_used[solver_tip_id] == true)
01563 continue;
01564
01565
01566 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
01567
01568
01569
01570 if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
01571 solver_tip_frame = solver_tip_frame.substr(1);
01572
01573
01574 Eigen::Affine3d current_pose = getGlobalLinkTransform(solver_tip_frame);
01575
01576
01577 if (!setToIKSolverFrame(current_pose, solver))
01578 return false;
01579
01580
01581 geometry_msgs::Pose ik_query;
01582 tf::poseEigenToMsg(current_pose, ik_query);
01583
01584
01585 ik_queries[solver_tip_id] = ik_query;
01586
01587
01588 tip_frames_used[solver_tip_id] = true;
01589 }
01590
01591
01592 if (timeout < std::numeric_limits<double>::epsilon())
01593 timeout = jmg->getDefaultIKTimeout();
01594
01595 if (attempts == 0)
01596 attempts = jmg->getDefaultIKAttempts();
01597
01598
01599 kinematics::KinematicsBase::IKCallbackFn ik_callback_fn;
01600 if (constraint)
01601 ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
01602
01603
01604 const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();
01605
01606 bool first_seed = true;
01607 std::vector<double> initial_values;
01608 for (unsigned int st = 0; st < attempts; ++st)
01609 {
01610 std::vector<double> seed(bij.size());
01611
01612
01613 if (first_seed)
01614 {
01615 first_seed = false;
01616 copyJointGroupPositions(jmg, initial_values);
01617 for (std::size_t i = 0; i < bij.size(); ++i)
01618 seed[i] = initial_values[bij[i]];
01619 }
01620 else
01621 {
01622 logDebug("moveit.robot_state: Rerunning IK solver with random joint positions");
01623
01624
01625 random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator();
01626 std::vector<double> random_values;
01627 jmg->getVariableRandomPositions(rng, random_values);
01628 for (std::size_t i = 0; i < bij.size(); ++i)
01629 seed[i] = random_values[bij[i]];
01630
01631 if (options.lock_redundant_joints)
01632 {
01633 std::vector<unsigned int> red_joints;
01634 solver->getRedundantJoints(red_joints);
01635 copyJointGroupPositions(jmg, initial_values);
01636 for (std::size_t i = 0; i < red_joints.size(); ++i)
01637 seed[red_joints[i]] = initial_values[bij[red_joints[i]]];
01638 }
01639 }
01640
01641
01642 std::vector<double> ik_sol;
01643 moveit_msgs::MoveItErrorCodes error;
01644
01645 if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options,
01646 this))
01647 {
01648 std::vector<double> solution(bij.size());
01649 for (std::size_t i = 0; i < bij.size(); ++i)
01650 solution[bij[i]] = ik_sol[i];
01651 setJointGroupPositions(jmg, solution);
01652 return true;
01653 }
01654 }
01655 return false;
01656 }
01657
01658 bool moveit::core::RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::vector_Affine3d& poses_in,
01659 const std::vector<std::string>& tips_in,
01660 const std::vector<std::vector<double> >& consistency_limits,
01661 unsigned int attempts, double timeout,
01662 const GroupStateValidityCallbackFn& constraint,
01663 const kinematics::KinematicsQueryOptions& options)
01664 {
01665
01666
01667
01668 std::vector<const JointModelGroup*> sub_groups;
01669 jmg->getSubgroups(sub_groups);
01670
01671
01672 if (poses_in.size() != sub_groups.size())
01673 {
01674 logError("Number of poses (%u) must be the same as number of sub-groups (%u)", poses_in.size(), sub_groups.size());
01675 return false;
01676 }
01677
01678 if (tips_in.size() != sub_groups.size())
01679 {
01680 logError("Number of tip names (%u) must be same as number of sub-groups (%u)", tips_in.size(), sub_groups.size());
01681 return false;
01682 }
01683
01684 if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
01685 {
01686 logError("Number of consistency limit vectors must be the same as number of sub-groups");
01687 return false;
01688 }
01689
01690 for (std::size_t i = 0; i < consistency_limits.size(); ++i)
01691 {
01692 if (consistency_limits[i].size() != sub_groups[i]->getVariableCount())
01693 {
01694 logError("Number of joints in consistency_limits is %u but it should be should be %u", (unsigned int)i,
01695 sub_groups[i]->getVariableCount());
01696 return false;
01697 }
01698 }
01699
01700
01701 std::vector<kinematics::KinematicsBaseConstPtr> solvers;
01702 for (std::size_t i = 0; i < poses_in.size(); ++i)
01703 {
01704 kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
01705 if (!solver)
01706 {
01707 logError("Could not find solver for group '%s'", sub_groups[i]->getName().c_str());
01708 return false;
01709 }
01710 solvers.push_back(solver);
01711 }
01712
01713
01714 EigenSTL::vector_Affine3d transformed_poses = poses_in;
01715 std::vector<std::string> pose_frames = tips_in;
01716
01717
01718 for (std::size_t i = 0; i < poses_in.size(); ++i)
01719 {
01720 Eigen::Affine3d& pose = transformed_poses[i];
01721 std::string& pose_frame = pose_frames[i];
01722
01723
01724 if (!setToIKSolverFrame(pose, solvers[i]))
01725 return false;
01726
01727
01728 std::string solver_tip_frame = solvers[i]->getTipFrame();
01729
01730
01731
01732 if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
01733 solver_tip_frame = solver_tip_frame.substr(1);
01734
01735 if (pose_frame != solver_tip_frame)
01736 {
01737 if (hasAttachedBody(pose_frame))
01738 {
01739 const AttachedBody* ab = getAttachedBody(pose_frame);
01740 const EigenSTL::vector_Affine3d& ab_trans = ab->getFixedTransforms();
01741 if (ab_trans.size() != 1)
01742 {
01743 logError("Cannot use an attached body with multiple geometries as a reference frame.");
01744 return false;
01745 }
01746 pose_frame = ab->getAttachedLinkName();
01747 pose = pose * ab_trans[0].inverse();
01748 }
01749 if (pose_frame != solver_tip_frame)
01750 {
01751 const robot_model::LinkModel* lm = getLinkModel(pose_frame);
01752 if (!lm)
01753 return false;
01754 const robot_model::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
01755 for (robot_model::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
01756 if (it->first->getName() == solver_tip_frame)
01757 {
01758 pose_frame = solver_tip_frame;
01759 pose = pose * it->second;
01760 break;
01761 }
01762 }
01763 }
01764
01765 if (pose_frame != solver_tip_frame)
01766 {
01767 logError("Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(),
01768 solver_tip_frame.c_str());
01769 return false;
01770 }
01771 }
01772
01773
01774 std::vector<geometry_msgs::Pose> ik_queries(poses_in.size());
01775 kinematics::KinematicsBase::IKCallbackFn ik_callback_fn;
01776 if (constraint)
01777 ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
01778
01779 for (std::size_t i = 0; i < transformed_poses.size(); ++i)
01780 {
01781 Eigen::Quaterniond quat(transformed_poses[i].rotation());
01782 Eigen::Vector3d point(transformed_poses[i].translation());
01783 ik_queries[i].position.x = point.x();
01784 ik_queries[i].position.y = point.y();
01785 ik_queries[i].position.z = point.z();
01786 ik_queries[i].orientation.x = quat.x();
01787 ik_queries[i].orientation.y = quat.y();
01788 ik_queries[i].orientation.z = quat.z();
01789 ik_queries[i].orientation.w = quat.w();
01790 }
01791
01792 if (attempts == 0)
01793 attempts = jmg->getDefaultIKAttempts();
01794
01795
01796 if (timeout < std::numeric_limits<double>::epsilon())
01797 timeout = jmg->getDefaultIKTimeout();
01798
01799 bool first_seed = true;
01800 for (unsigned int st = 0; st < attempts; ++st)
01801 {
01802 bool found_solution = true;
01803 for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
01804 {
01805 const std::vector<unsigned int>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
01806 std::vector<double> seed(bij.size());
01807
01808 if (first_seed)
01809 {
01810 std::vector<double> initial_values;
01811 copyJointGroupPositions(sub_groups[sg], initial_values);
01812 for (std::size_t i = 0; i < bij.size(); ++i)
01813 seed[i] = initial_values[bij[i]];
01814 }
01815 else
01816 {
01817
01818 random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator();
01819 std::vector<double> random_values;
01820 sub_groups[sg]->getVariableRandomPositions(rng, random_values);
01821 for (std::size_t i = 0; i < bij.size(); ++i)
01822 seed[i] = random_values[bij[i]];
01823 }
01824
01825
01826 std::vector<double> ik_sol;
01827 moveit_msgs::MoveItErrorCodes error;
01828 const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
01829 if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, timeout, climits, ik_sol, error))
01830 {
01831 std::vector<double> solution(bij.size());
01832 for (std::size_t i = 0; i < bij.size(); ++i)
01833 solution[bij[i]] = ik_sol[i];
01834 setJointGroupPositions(sub_groups[sg], solution);
01835 }
01836 else
01837 {
01838 found_solution = false;
01839 break;
01840 }
01841 logDebug("IK attempt: %d of %d", st, attempts);
01842 }
01843 if (found_solution)
01844 {
01845 std::vector<double> full_solution;
01846 copyJointGroupPositions(jmg, full_solution);
01847 if (constraint ? constraint(this, jmg, &full_solution[0]) : true)
01848 {
01849 logDebug("Found IK solution");
01850 return true;
01851 }
01852 }
01853 }
01854 return false;
01855 }
01856
01857 double moveit::core::RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
01858 const LinkModel* link, const Eigen::Vector3d& direction,
01859 bool global_reference_frame, double distance, double max_step,
01860 double jump_threshold,
01861 const GroupStateValidityCallbackFn& validCallback,
01862 const kinematics::KinematicsQueryOptions& options)
01863 {
01864
01865 const Eigen::Affine3d& start_pose = getGlobalLinkTransform(link);
01866
01867
01868 const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.rotation() * direction;
01869
01870
01871 Eigen::Affine3d target_pose = start_pose;
01872 target_pose.translation() += rotated_direction * distance;
01873
01874
01875 return (distance *
01876 computeCartesianPath(group, traj, link, target_pose, true, max_step, jump_threshold, validCallback, options));
01877 }
01878
01879 double moveit::core::RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
01880 const LinkModel* link, const Eigen::Affine3d& target,
01881 bool global_reference_frame, double max_step,
01882 double jump_threshold,
01883 const GroupStateValidityCallbackFn& validCallback,
01884 const kinematics::KinematicsQueryOptions& options)
01885 {
01886 const std::vector<const JointModel*>& cjnt = group->getContinuousJointModels();
01887
01888 for (std::size_t i = 0; i < cjnt.size(); ++i)
01889 enforceBounds(cjnt[i]);
01890
01891
01892 Eigen::Affine3d start_pose = getGlobalLinkTransform(link);
01893
01894
01895 Eigen::Affine3d rotated_target = global_reference_frame ? target : start_pose * target;
01896
01897 bool test_joint_space_jump = jump_threshold > 0.0;
01898
01899
01900 double distance = (rotated_target.translation() - start_pose.translation()).norm();
01901 unsigned int steps = (test_joint_space_jump ? 5 : 1) + (unsigned int)floor(distance / max_step);
01902
01903 traj.clear();
01904 traj.push_back(RobotStatePtr(new RobotState(*this)));
01905
01906 std::vector<double> dist_vector;
01907 double total_dist = 0.0;
01908
01909 double last_valid_percentage = 0.0;
01910 Eigen::Quaterniond start_quaternion(start_pose.rotation());
01911 Eigen::Quaterniond target_quaternion(rotated_target.rotation());
01912 for (unsigned int i = 1; i <= steps; ++i)
01913 {
01914 double percentage = (double)i / (double)steps;
01915
01916 Eigen::Affine3d pose(start_quaternion.slerp(percentage, target_quaternion));
01917 pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
01918
01919 if (setFromIK(group, pose, link->getName(), 1, 0.0, validCallback, options))
01920 {
01921 traj.push_back(RobotStatePtr(new RobotState(*this)));
01922
01923
01924 if (test_joint_space_jump)
01925 {
01926 double dist_prev_point = traj.back()->distance(*traj[traj.size() - 2], group);
01927 dist_vector.push_back(dist_prev_point);
01928 total_dist += dist_prev_point;
01929 }
01930 }
01931 else
01932 break;
01933 last_valid_percentage = percentage;
01934 }
01935
01936 if (test_joint_space_jump && !dist_vector.empty())
01937 {
01938
01939 double thres = jump_threshold * (total_dist / (double)dist_vector.size());
01940 for (std::size_t i = 0; i < dist_vector.size(); ++i)
01941 if (dist_vector[i] > thres)
01942 {
01943 logDebug("Truncating Cartesian path due to detected jump in joint-space distance");
01944 last_valid_percentage = (double)i / (double)steps;
01945 traj.resize(i);
01946 break;
01947 }
01948 }
01949
01950 return last_valid_percentage;
01951 }
01952
01953 double moveit::core::RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
01954 const LinkModel* link, const EigenSTL::vector_Affine3d& waypoints,
01955 bool global_reference_frame, double max_step,
01956 double jump_threshold,
01957 const GroupStateValidityCallbackFn& validCallback,
01958 const kinematics::KinematicsQueryOptions& options)
01959 {
01960 double percentage_solved = 0.0;
01961 for (std::size_t i = 0; i < waypoints.size(); ++i)
01962 {
01963 std::vector<RobotStatePtr> waypoint_traj;
01964 double wp_percentage_solved = computeCartesianPath(group, waypoint_traj, link, waypoints[i], global_reference_frame,
01965 max_step, jump_threshold, validCallback, options);
01966 if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
01967 {
01968 percentage_solved = (double)(i + 1) / (double)waypoints.size();
01969 std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
01970 if (i > 0 && !waypoint_traj.empty())
01971 std::advance(start, 1);
01972 traj.insert(traj.end(), start, waypoint_traj.end());
01973 }
01974 else
01975 {
01976 percentage_solved += wp_percentage_solved / (double)waypoints.size();
01977 std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
01978 if (i > 0 && !waypoint_traj.empty())
01979 std::advance(start, 1);
01980 traj.insert(traj.end(), start, waypoint_traj.end());
01981 break;
01982 }
01983 }
01984
01985 return percentage_solved;
01986 }
01987
01988 namespace
01989 {
01990 static inline void updateAABB(const Eigen::Affine3d& t, const Eigen::Vector3d& e, std::vector<double>& aabb)
01991 {
01992 Eigen::Vector3d v = e / 2.0;
01993 Eigen::Vector3d c2 = t * v;
01994 v = -v;
01995 Eigen::Vector3d c1 = t * v;
01996 if (aabb.empty())
01997 {
01998 aabb.resize(6);
01999 aabb[0] = c1.x();
02000 aabb[2] = c1.y();
02001 aabb[4] = c1.z();
02002 aabb[1] = c2.x();
02003 aabb[3] = c2.y();
02004 aabb[5] = c2.z();
02005 }
02006 else
02007 {
02008 if (aabb[0] > c1.x())
02009 aabb[0] = c1.x();
02010 if (aabb[2] > c1.y())
02011 aabb[2] = c1.y();
02012 if (aabb[4] > c1.z())
02013 aabb[4] = c1.z();
02014 if (aabb[1] < c2.x())
02015 aabb[1] = c2.x();
02016 if (aabb[3] < c2.y())
02017 aabb[3] = c2.y();
02018 if (aabb[5] < c2.z())
02019 aabb[5] = c2.z();
02020 }
02021 }
02022 }
02023
02024 void robot_state::RobotState::computeAABB(std::vector<double>& aabb) const
02025 {
02026 BOOST_VERIFY(checkLinkTransforms());
02027
02028 aabb.clear();
02029 std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
02030 for (std::size_t i = 0; i < links.size(); ++i)
02031 {
02032 const Eigen::Affine3d& t = getGlobalLinkTransform(links[i]);
02033 const Eigen::Vector3d& e = links[i]->getShapeExtentsAtOrigin();
02034 updateAABB(t, e, aabb);
02035 }
02036 for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
02037 it != attached_body_map_.end(); ++it)
02038 {
02039 const EigenSTL::vector_Affine3d& ts = it->second->getGlobalCollisionBodyTransforms();
02040 const std::vector<shapes::ShapeConstPtr>& ss = it->second->getShapes();
02041 for (std::size_t i = 0; i < ts.size(); ++i)
02042 {
02043 Eigen::Vector3d e = shapes::computeShapeExtents(ss[i].get());
02044 updateAABB(ts[i], e, aabb);
02045 }
02046 }
02047 if (aabb.empty())
02048 aabb.resize(6, 0.0);
02049 }
02050
02051 void moveit::core::RobotState::printStatePositions(std::ostream& out) const
02052 {
02053 const std::vector<std::string>& nm = robot_model_->getVariableNames();
02054 for (std::size_t i = 0; i < nm.size(); ++i)
02055 out << nm[i] << "=" << position_[i] << std::endl;
02056 }
02057
02058 void moveit::core::RobotState::printDirtyInfo(std::ostream& out) const
02059 {
02060 out << " * Dirty Joint Transforms: " << std::endl;
02061 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
02062 for (std::size_t i = 0; i < jm.size(); ++i)
02063 if (jm[i]->getVariableCount() > 0 && dirtyJointTransform(jm[i]))
02064 out << " " << jm[i]->getName() << std::endl;
02065 out << " * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL")
02066 << std::endl;
02067 out << " * Dirty Collision Body Transforms: "
02068 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->getName() : "NULL") << std::endl;
02069 }
02070
02071 void moveit::core::RobotState::printStateInfo(std::ostream& out) const
02072 {
02073 out << "Robot State @" << this << std::endl;
02074
02075 std::size_t n = robot_model_->getVariableCount();
02076 if (position_)
02077 {
02078 out << " * Position: ";
02079 for (std::size_t i = 0; i < n; ++i)
02080 out << position_[i] << " ";
02081 out << std::endl;
02082 }
02083 else
02084 out << " * Position: NULL" << std::endl;
02085
02086 if (velocity_)
02087 {
02088 out << " * Velocity: ";
02089 for (std::size_t i = 0; i < n; ++i)
02090 out << velocity_[i] << " ";
02091 out << std::endl;
02092 }
02093 else
02094 out << " * Velocity: NULL" << std::endl;
02095
02096 if (acceleration_)
02097 {
02098 out << " * Acceleration: ";
02099 for (std::size_t i = 0; i < n; ++i)
02100 out << acceleration_[i] << " ";
02101 out << std::endl;
02102 }
02103 else
02104 out << " * Acceleration: NULL" << std::endl;
02105
02106 out << " * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL")
02107 << std::endl;
02108 out << " * Dirty Collision Body Transforms: "
02109 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->getName() : "NULL") << std::endl;
02110
02111 printTransforms(out);
02112 }
02113
02114 void moveit::core::RobotState::printTransform(const Eigen::Affine3d& transform, std::ostream& out) const
02115 {
02116 Eigen::Quaterniond q(transform.rotation());
02117 out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
02118 << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w()
02119 << "]" << std::endl;
02120 }
02121
02122 void moveit::core::RobotState::printTransforms(std::ostream& out) const
02123 {
02124 if (!variable_joint_transforms_)
02125 {
02126 out << "No transforms computed" << std::endl;
02127 return;
02128 }
02129
02130 out << "Joint transforms:" << std::endl;
02131 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
02132 for (std::size_t i = 0; i < jm.size(); ++i)
02133 {
02134 out << " " << jm[i]->getName();
02135 const int idx = jm[i]->getJointIndex();
02136 if (dirty_joint_transforms_[idx])
02137 out << " [dirty]";
02138 out << ": ";
02139 printTransform(variable_joint_transforms_[idx], out);
02140 }
02141
02142 out << "Link poses:" << std::endl;
02143 const std::vector<const LinkModel*>& lm = robot_model_->getLinkModels();
02144 for (std::size_t i = 0; i < lm.size(); ++i)
02145 {
02146 out << " " << lm[i]->getName() << ": ";
02147 printTransform(global_link_transforms_[lm[i]->getLinkIndex()], out);
02148 }
02149 }
02150
02151 std::string moveit::core::RobotState::getStateTreeString(const std::string& prefix) const
02152 {
02153 std::stringstream ss;
02154 ss << "ROBOT: " << robot_model_->getName() << std::endl;
02155 getStateTreeJointString(ss, robot_model_->getRootJoint(), " ", true);
02156 return ss.str();
02157 }
02158
02159 namespace
02160 {
02161 void getPoseString(std::ostream& ss, const Eigen::Affine3d& pose, const std::string& pfx)
02162 {
02163 ss.precision(3);
02164 for (int y = 0; y < 4; ++y)
02165 {
02166 ss << pfx;
02167 for (int x = 0; x < 4; ++x)
02168 {
02169 ss << std::setw(8) << pose(y, x) << " ";
02170 }
02171 ss << std::endl;
02172 }
02173 }
02174 }
02175
02176 void moveit::core::RobotState::getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0,
02177 bool last) const
02178 {
02179 std::string pfx = pfx0 + "+--";
02180
02181 ss << pfx << "Joint: " << jm->getName() << std::endl;
02182
02183 pfx = pfx0 + (last ? " " : "| ");
02184
02185 for (std::size_t i = 0; i < jm->getVariableCount(); ++i)
02186 {
02187 ss.precision(3);
02188 ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] << std::endl;
02189 }
02190
02191 const LinkModel* lm = jm->getChildLinkModel();
02192
02193 ss << pfx << "Link: " << lm->getName() << std::endl;
02194 getPoseString(ss, lm->getJointOriginTransform(), pfx + "joint_origin:");
02195 if (variable_joint_transforms_)
02196 {
02197 getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx + "joint_variable:");
02198 getPoseString(ss, global_link_transforms_[lm->getLinkIndex()], pfx + "link_global:");
02199 }
02200
02201 for (std::vector<const JointModel*>::const_iterator it = lm->getChildJointModels().begin();
02202 it != lm->getChildJointModels().end(); ++it)
02203 getStateTreeJointString(ss, *it, pfx, it + 1 == lm->getChildJointModels().end());
02204 }
02205
02206 std::ostream& moveit::core::operator<<(std::ostream& out, const RobotState& s)
02207 {
02208 s.printStateInfo(out);
02209 return out;
02210 }