38 #include <geometric_shapes/check_isometry.h>
39 #include <geometric_shapes/shape_operations.h>
43 #include <rclcpp/clock.hpp>
44 #include <rclcpp/logger.hpp>
45 #include <rclcpp/logging.hpp>
46 #include <rclcpp/time.hpp>
47 #include <tf2_eigen/tf2_eigen.hpp>
58 static const rclcpp::Logger LOGGER = rclcpp::get_logger(
"moveit_robot_state.robot_state");
61 : robot_model_(robot_model)
62 , has_velocity_(false)
63 , has_acceleration_(false)
65 , dirty_link_transforms_(nullptr)
66 , dirty_collision_body_transforms_(nullptr)
69 if (robot_model ==
nullptr)
71 throw std::invalid_argument(
"RobotState cannot be constructed with nullptr RobotModelConstPtr");
74 dirty_link_transforms_ = robot_model_->getRootJoint();
81 robot_model_ = other.robot_model_;
94 void RobotState::allocMemory()
96 static_assert((
sizeof(Eigen::Isometry3d) / EIGEN_MAX_ALIGN_BYTES) * EIGEN_MAX_ALIGN_BYTES ==
sizeof(Eigen::Isometry3d),
97 "sizeof(Eigen::Isometry3d) should be a multiple of EIGEN_MAX_ALIGN_BYTES");
99 constexpr
unsigned int extra_alignment_bytes = EIGEN_MAX_ALIGN_BYTES - 1;
101 const int nr_doubles_for_dirty_joint_transforms =
102 1 + robot_model_->getJointModelCount() / (
sizeof(double) /
sizeof(
unsigned char));
104 sizeof(Eigen::Isometry3d) * (robot_model_->getJointModelCount() + robot_model_->getLinkModelCount() +
105 robot_model_->getLinkGeometryCount()) +
106 sizeof(
double) * (robot_model_->getVariableCount() * 3 + nr_doubles_for_dirty_joint_transforms) +
107 extra_alignment_bytes;
108 memory_ = malloc(bytes);
113 variable_joint_transforms_ =
reinterpret_cast<Eigen::Isometry3d*
>(((uintptr_t)memory_ + extra_alignment_bytes) &
114 ~(uintptr_t)extra_alignment_bytes);
115 global_link_transforms_ = variable_joint_transforms_ + robot_model_->getJointModelCount();
116 global_collision_body_transforms_ = global_link_transforms_ + robot_model_->getLinkModelCount();
117 dirty_joint_transforms_ =
118 reinterpret_cast<unsigned char*
>(global_collision_body_transforms_ + robot_model_->getLinkGeometryCount());
119 position_ =
reinterpret_cast<double*
>(dirty_joint_transforms_) + nr_doubles_for_dirty_joint_transforms;
120 velocity_ = position_ + robot_model_->getVariableCount();
122 effort_ = acceleration_ = velocity_ + robot_model_->getVariableCount();
125 void RobotState::initTransforms()
128 const int nr_doubles_for_dirty_joint_transforms =
129 1 + robot_model_->getJointModelCount() / (
sizeof(double) /
sizeof(
unsigned char));
130 memset(dirty_joint_transforms_, 1,
sizeof(
double) * nr_doubles_for_dirty_joint_transforms);
133 for (
size_t i = 0, end = robot_model_->getJointModelCount() + robot_model_->getLinkModelCount() +
134 robot_model_->getLinkGeometryCount();
136 variable_joint_transforms_[i].makeAffine();
146 void RobotState::copyFrom(
const RobotState& other)
148 has_velocity_ = other.has_velocity_;
149 has_acceleration_ = other.has_acceleration_;
150 has_effort_ = other.has_effort_;
152 dirty_collision_body_transforms_ = other.dirty_collision_body_transforms_;
153 dirty_link_transforms_ = other.dirty_link_transforms_;
155 if (dirty_link_transforms_ == robot_model_->getRootJoint())
158 memcpy(position_, other.position_,
159 robot_model_->getVariableCount() *
sizeof(
double) *
160 (1 + (has_velocity_ ? 1 : 0) + ((has_acceleration_ || has_effort_) ? 1 : 0)));
167 const int nr_doubles_for_dirty_joint_transforms =
168 1 + robot_model_->getJointModelCount() / (
sizeof(double) /
sizeof(
unsigned char));
170 sizeof(Eigen::Isometry3d) * (robot_model_->getJointModelCount() + robot_model_->getLinkModelCount() +
171 robot_model_->getLinkGeometryCount()) +
173 (robot_model_->getVariableCount() * (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) +
174 ((has_acceleration_ || has_effort_) ? 1 : 0)) +
175 nr_doubles_for_dirty_joint_transforms);
176 memcpy((
void*)variable_joint_transforms_, (
void*)other.variable_joint_transforms_, bytes);
181 for (
const auto& attached_body : other.attached_body_map_)
182 attachBody(std::make_unique<AttachedBody>(*attached_body.second));
185 bool RobotState::checkJointTransforms(
const JointModel* joint)
const
189 RCLCPP_WARN(LOGGER,
"Returning dirty joint transforms for joint '%s'", joint->getName().c_str());
195 bool RobotState::checkLinkTransforms()
const
199 RCLCPP_WARN(LOGGER,
"Returning dirty link transforms");
205 bool RobotState::checkCollisionTransforms()
const
209 RCLCPP_WARN(LOGGER,
"Returning dirty collision body transforms");
215 void RobotState::markVelocity()
219 has_velocity_ =
true;
220 memset(velocity_, 0,
sizeof(
double) * robot_model_->getVariableCount());
224 void RobotState::markAcceleration()
226 if (!has_acceleration_)
228 has_acceleration_ =
true;
230 memset(acceleration_, 0,
sizeof(
double) * robot_model_->getVariableCount());
234 void RobotState::markEffort()
238 has_acceleration_ =
false;
240 memset(effort_, 0,
sizeof(
double) * robot_model_->getVariableCount());
246 has_velocity_ =
false;
252 has_acceleration_ =
false;
264 has_velocity_ =
false;
269 has_acceleration_ =
false;
287 robot_model_->getVariableRandomPositions(rng, position_);
288 memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() *
sizeof(
unsigned char));
289 dirty_link_transforms_ = robot_model_->getRootJoint();
302 const std::vector<const JointModel*>& joints =
group->getActiveJointModels();
304 joint->getVariableRandomPositions(rng, position_ + joint->getFirstVariableIndex());
305 updateMimicJoints(
group);
309 const std::vector<double>& distances)
318 const std::vector<double>& distances,
319 random_numbers::RandomNumberGenerator& rng)
321 const std::vector<const JointModel*>& joints =
group->getActiveJointModels();
322 assert(distances.size() == joints.size());
323 for (std::size_t i = 0; i < joints.size(); ++i)
325 const int idx = joints[i]->getFirstVariableIndex();
326 joints[i]->getVariableRandomPositionsNearBy(rng, position_ + joints[i]->getFirstVariableIndex(),
327 seed.position_ + idx, distances[i]);
329 updateMimicJoints(
group);
341 random_numbers::RandomNumberGenerator& rng)
343 const std::vector<const JointModel*>& joints =
group->getActiveJointModels();
346 const int idx = joint->getFirstVariableIndex();
347 joint->getVariableRandomPositionsNearBy(rng, position_ + joint->getFirstVariableIndex(), seed.position_ + idx,
350 updateMimicJoints(
group);
355 std::map<std::string, double> m;
356 bool r =
group->getVariableDefaultPositions(
name, m);
363 robot_model_->getVariableDefaultPositions(position_);
365 memset(velocity_, 0,
sizeof(
double) * 2 * robot_model_->getVariableCount());
366 memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() *
sizeof(
unsigned char));
367 dirty_link_transforms_ = robot_model_->getRootJoint();
373 memcpy(position_, position, robot_model_->getVariableCount() *
sizeof(
double));
378 memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() *
sizeof(
unsigned char));
379 dirty_link_transforms_ = robot_model_->getRootJoint();
384 for (
const std::pair<const std::string, double>& it : variable_map)
386 const int index = robot_model_->getVariableIndex(it.first);
387 position_[index] = it.second;
388 const JointModel* jm = robot_model_->getJointOfVariable(index);
389 markDirtyJointTransforms(jm);
390 updateMimicJoint(jm);
394 void RobotState::getMissingKeys(
const std::map<std::string, double>& variable_map,
395 std::vector<std::string>& missing_variables)
const
397 missing_variables.clear();
398 const std::vector<std::string>& nm = robot_model_->getVariableNames();
399 for (
const std::string& variable_name : nm)
400 if (variable_map.find(variable_name) == variable_map.end())
401 if (robot_model_->getJointOfVariable(variable_name)->getMimic() ==
nullptr)
402 missing_variables.push_back(variable_name);
406 std::vector<std::string>& missing_variables)
409 getMissingKeys(variable_map, missing_variables);
413 const std::vector<double>& variable_position)
415 for (std::size_t i = 0; i < variable_names.size(); ++i)
417 const int index = robot_model_->getVariableIndex(variable_names[i]);
418 position_[index] = variable_position[i];
419 const JointModel* jm = robot_model_->getJointOfVariable(index);
420 markDirtyJointTransforms(jm);
421 updateMimicJoint(jm);
428 for (
const std::pair<const std::string, double>& it : variable_map)
429 velocity_[robot_model_->getVariableIndex(it.first)] = it.second;
433 std::vector<std::string>& missing_variables)
436 getMissingKeys(variable_map, missing_variables);
440 const std::vector<double>& variable_velocity)
443 assert(variable_names.size() == variable_velocity.size());
444 for (std::size_t i = 0; i < variable_names.size(); ++i)
445 velocity_[robot_model_->getVariableIndex(variable_names[i])] = variable_velocity[i];
451 for (
const std::pair<const std::string, double>& it : variable_map)
452 acceleration_[robot_model_->getVariableIndex(it.first)] = it.second;
456 std::vector<std::string>& missing_variables)
459 getMissingKeys(variable_map, missing_variables);
463 const std::vector<double>& variable_acceleration)
466 assert(variable_names.size() == variable_acceleration.size());
467 for (std::size_t i = 0; i < variable_names.size(); ++i)
468 acceleration_[robot_model_->getVariableIndex(variable_names[i])] = variable_acceleration[i];
474 for (
const std::pair<const std::string, double>& it : variable_map)
475 effort_[robot_model_->getVariableIndex(it.first)] = it.second;
479 std::vector<std::string>& missing_variables)
482 getMissingKeys(variable_map, missing_variables);
486 const std::vector<double>& variable_effort)
489 assert(variable_names.size() == variable_effort.size());
490 for (std::size_t i = 0; i < variable_names.size(); ++i)
491 effort_[robot_model_->getVariableIndex(variable_names[i])] = variable_effort[i];
498 for (
size_t i = 0; i < robot_model_->getVariableCount(); ++i)
505 if (has_acceleration_)
507 RCLCPP_ERROR(LOGGER,
"Unable to set joint efforts because array is being used for accelerations");
517 const std::vector<int>& il =
group->getVariableIndexList();
518 if (
group->isContiguousWithinState())
519 memcpy(position_ + il[0], gstate,
group->getVariableCount() *
sizeof(
double));
522 for (std::size_t i = 0; i < il.size(); ++i)
523 position_[il[i]] = gstate[i];
525 updateMimicJoints(
group);
530 const std::vector<int>& il =
group->getVariableIndexList();
531 for (std::size_t i = 0; i < il.size(); ++i)
532 position_[il[i]] = values(i);
533 updateMimicJoints(
group);
538 assert(gstate.size() ==
group->getActiveVariableCount());
543 i += jm->getVariableCount();
545 updateMimicJoints(
group);
550 assert(values.size() ==
group->getActiveVariableCount());
555 i += jm->getVariableCount();
557 updateMimicJoints(
group);
562 const std::vector<int>& il =
group->getVariableIndexList();
563 if (
group->isContiguousWithinState())
564 memcpy(gstate, position_ + il[0],
group->getVariableCount() *
sizeof(
double));
566 for (std::size_t i = 0; i < il.size(); ++i)
567 gstate[i] = position_[il[i]];
572 const std::vector<int>& il =
group->getVariableIndexList();
573 values.resize(il.size());
574 for (std::size_t i = 0; i < il.size(); ++i)
575 values(i) = position_[il[i]];
581 const std::vector<int>& il =
group->getVariableIndexList();
582 if (
group->isContiguousWithinState())
583 memcpy(velocity_ + il[0], gstate,
group->getVariableCount() *
sizeof(
double));
586 for (std::size_t i = 0; i < il.size(); ++i)
587 velocity_[il[i]] = gstate[i];
594 const std::vector<int>& il =
group->getVariableIndexList();
595 for (std::size_t i = 0; i < il.size(); ++i)
596 velocity_[il[i]] = values(i);
601 const std::vector<int>& il =
group->getVariableIndexList();
602 if (
group->isContiguousWithinState())
603 memcpy(gstate, velocity_ + il[0],
group->getVariableCount() *
sizeof(
double));
605 for (std::size_t i = 0; i < il.size(); ++i)
606 gstate[i] = velocity_[il[i]];
611 const std::vector<int>& il =
group->getVariableIndexList();
612 values.resize(il.size());
613 for (std::size_t i = 0; i < il.size(); ++i)
614 values(i) = velocity_[il[i]];
620 const std::vector<int>& il =
group->getVariableIndexList();
621 if (
group->isContiguousWithinState())
622 memcpy(acceleration_ + il[0], gstate,
group->getVariableCount() *
sizeof(
double));
625 for (std::size_t i = 0; i < il.size(); ++i)
626 acceleration_[il[i]] = gstate[i];
633 const std::vector<int>& il =
group->getVariableIndexList();
634 for (std::size_t i = 0; i < il.size(); ++i)
635 acceleration_[il[i]] = values(i);
640 const std::vector<int>& il =
group->getVariableIndexList();
641 if (
group->isContiguousWithinState())
642 memcpy(gstate, acceleration_ + il[0],
group->getVariableCount() *
sizeof(
double));
644 for (std::size_t i = 0; i < il.size(); ++i)
645 gstate[i] = acceleration_[il[i]];
650 const std::vector<int>& il =
group->getVariableIndexList();
651 values.resize(il.size());
652 for (std::size_t i = 0; i < il.size(); ++i)
653 values(i) = acceleration_[il[i]];
661 memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() *
sizeof(
unsigned char));
662 dirty_link_transforms_ = robot_model_->getRootJoint();
671 if (dirty_link_transforms_ !=
nullptr)
674 if (dirty_collision_body_transforms_ !=
nullptr)
677 dirty_collision_body_transforms_ =
nullptr;
681 const EigenSTL::vector_Isometry3d& ot = link->getCollisionOriginTransforms();
682 const std::vector<int>& ot_id = link->areCollisionOriginTransformsIdentity();
683 const int index_co = link->getFirstCollisionBodyTransformIndex();
684 const int index_l = link->getLinkIndex();
685 for (std::size_t j = 0, end = ot.size(); j != end; ++j)
688 global_collision_body_transforms_[index_co + j] = global_link_transforms_[index_l];
690 global_collision_body_transforms_[index_co + j].affine().noalias() =
691 global_link_transforms_[index_l].affine() * ot[j].matrix();
699 if (dirty_link_transforms_ !=
nullptr)
701 updateLinkTransformsInternal(dirty_link_transforms_);
702 if (dirty_collision_body_transforms_)
703 dirty_collision_body_transforms_ =
704 robot_model_->getCommonRoot(dirty_collision_body_transforms_, dirty_link_transforms_);
706 dirty_collision_body_transforms_ = dirty_link_transforms_;
707 dirty_link_transforms_ =
nullptr;
711 void RobotState::updateLinkTransformsInternal(
const JointModel* start)
715 int idx_link = link->getLinkIndex();
720 if (link->parentJointIsFixed())
721 global_link_transforms_[idx_link].affine().noalias() =
722 global_link_transforms_[idx_parent].affine() * link->getJointOriginTransform().matrix();
725 if (link->jointOriginTransformIsIdentity())
726 global_link_transforms_[idx_link].affine().noalias() =
727 global_link_transforms_[idx_parent].affine() *
getJointTransform(link->getParentJointModel()).matrix();
729 global_link_transforms_[idx_link].affine().noalias() =
730 global_link_transforms_[idx_parent].affine() * link->getJointOriginTransform().matrix() *
736 if (link->jointOriginTransformIsIdentity())
737 global_link_transforms_[idx_link] =
getJointTransform(link->getParentJointModel());
739 global_link_transforms_[idx_link].affine().noalias() =
740 link->getJointOriginTransform().affine() *
getJointTransform(link->getParentJointModel()).matrix();
745 for (
const auto& attached_body : attached_body_map_)
746 attached_body.second->computeTransform(
747 global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]);
755 if (dirty_collision_body_transforms_)
756 dirty_collision_body_transforms_ =
757 robot_model_->getCommonRoot(dirty_collision_body_transforms_, link->
getParentJointModel());
761 global_link_transforms_[link->
getLinkIndex()] = transform;
766 updateLinkTransformsInternal(joint);
775 child_link = parent_link;
790 updateLinkTransformsInternal(joint);
797 for (
const auto& attached_body : attached_body_map_)
799 global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]);
808 RCLCPP_ERROR(LOGGER,
"Unable to find link for frame '%s'", frame.c_str());
809 return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
814 const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
823 const std::vector<const JointModel*>& jm =
group->getActiveJointModels();
832 const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
846 for (
const JointModel* jm : robot_model_->getActiveJointModels())
866 std::pair<double, const JointModel*>
869 double distance = std::numeric_limits<double>::max();
881 std::vector<double> lower_bounds(bounds.size()), upper_bounds(bounds.size());
882 for (std::size_t j = 0; j < bounds.size(); ++j)
884 lower_bounds[j] = bounds[j].min_position_;
885 upper_bounds[j] = bounds[j].max_position_;
887 double new_distance = joint->distance(joint_values, &lower_bounds[0]);
893 new_distance = joint->distance(joint_values, &upper_bounds[0]);
900 return std::make_pair(
distance, index);
905 const std::vector<const JointModel*>& jm =
group->getActiveJointModels();
908 const int idx = joint_id->getFirstVariableIndex();
909 const std::vector<VariableBounds>& bounds = joint_id->getVariableBounds();
912 for (std::size_t var_id = 0; var_id < joint_id->getVariableCount(); ++var_id)
914 const double dtheta = std::abs(*(position_ + idx + var_id) - *(other.
getVariablePositions() + idx + var_id));
916 if (dtheta > dt * bounds[var_id].max_velocity_)
929 const int idx = joint->getFirstVariableIndex();
930 d += joint->getDistanceFactor() * joint->distance(position_ + idx, other.position_ + idx);
937 moveit::core::checkInterpolationParamBounds(LOGGER, t);
940 memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() *
sizeof(
unsigned char));
941 state.dirty_link_transforms_ = state.robot_model_->getRootJoint();
946 moveit::core::checkInterpolationParamBounds(LOGGER, t);
950 const int idx = joint->getFirstVariableIndex();
951 joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
953 state.updateMimicJoints(joint_group);
958 attached_body_update_callback_ = callback;
963 return attached_body_map_.find(
id) != attached_body_map_.end();
968 const auto it = attached_body_map_.find(
id);
969 if (it == attached_body_map_.end())
971 RCLCPP_ERROR(LOGGER,
"Attached body '%s' not found",
id.c_str());
975 return it->second.get();
984 if (attached_body_update_callback_)
985 attached_body_update_callback_(attached_body.get(),
true);
986 attached_body_map_[attached_body->getName()] = std::move(attached_body);
991 attachBody(std::unique_ptr<AttachedBody>(attached_body));
995 const std::vector<shapes::ShapeConstPtr>&
shapes,
996 const EigenSTL::vector_Isometry3d& shape_poses,
const std::set<std::string>& touch_links,
997 const std::string& link,
const trajectory_msgs::msg::JointTrajectory& detach_posture,
1000 attachBody(std::make_unique<AttachedBody>(robot_model_->getLinkModel(link),
id, pose,
shapes, shape_poses,
1001 touch_links, detach_posture, subframe_poses));
1006 attached_bodies.clear();
1007 attached_bodies.reserve(attached_body_map_.size());
1008 for (
const auto& it : attached_body_map_)
1009 attached_bodies.push_back(it.second.get());
1014 attached_bodies.clear();
1015 for (
const auto& it : attached_body_map_)
1016 if (
group->hasLinkModel(it.second->getAttachedLinkName()))
1017 attached_bodies.push_back(it.second.get());
1022 attached_bodies.clear();
1023 for (
const auto& it : attached_body_map_)
1024 if (it.second->getAttachedLink() == link_model)
1025 attached_bodies.push_back(it.second.get());
1030 for (
const auto& it : attached_body_map_)
1032 if (attached_body_update_callback_)
1033 attached_body_update_callback_(it.second.get(),
false);
1035 attached_body_map_.clear();
1040 for (
auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1042 if (it->second->getAttachedLink() != link)
1046 if (attached_body_update_callback_)
1047 attached_body_update_callback_(it->second.get(),
false);
1048 const auto del = it++;
1049 attached_body_map_.erase(del);
1055 for (
auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1057 if (!
group->hasLinkModel(it->second->getAttachedLinkName()))
1061 if (attached_body_update_callback_)
1062 attached_body_update_callback_(it->second.get(),
false);
1063 const auto del = it++;
1064 attached_body_map_.erase(del);
1070 const auto it = attached_body_map_.find(
id);
1071 if (it != attached_body_map_.end())
1073 if (attached_body_update_callback_)
1074 attached_body_update_callback_(it->second.get(),
false);
1075 attached_body_map_.erase(it);
1095 *frame_found = found;
1097 RCLCPP_WARN(LOGGER,
"getFrameTransform() did not find a frame with name %s.",
frame_id.c_str());
1103 bool& frame_found)
const
1108 static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
1109 if (
frame_id == robot_model_->getModelFrame())
1111 robot_link = robot_model_->getRootLink();
1113 return IDENTITY_TRANSFORM;
1115 if ((robot_link = robot_model_->getLinkModel(
frame_id, &frame_found)))
1117 assert(checkLinkTransforms());
1118 return global_link_transforms_[robot_link->
getLinkIndex()];
1120 robot_link =
nullptr;
1123 const auto jt = attached_body_map_.find(
frame_id);
1124 if (jt != attached_body_map_.end())
1126 const Eigen::Isometry3d& transform = jt->second->getGlobalPose();
1127 robot_link = jt->second->getAttachedLink();
1129 assert(checkLinkTransforms());
1134 for (
const auto& body : attached_body_map_)
1136 const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(
frame_id, &frame_found);
1139 robot_link = body.second->getAttachedLink();
1140 assert(checkLinkTransforms());
1145 robot_link =
nullptr;
1146 frame_found =
false;
1147 return IDENTITY_TRANSFORM;
1154 if (robot_model_->hasLinkModel(
frame_id))
1158 const auto it = attached_body_map_.find(
frame_id);
1159 if (it != attached_body_map_.end())
1160 return !it->second->getGlobalCollisionBodyTransforms().empty();
1163 for (
const auto& body : attached_body_map_)
1165 if (body.second->hasSubframeTransform(
frame_id))
1172 const std_msgs::msg::ColorRGBA& color,
const std::string& ns,
1173 const rclcpp::Duration& dur,
bool include_attached)
const
1175 std::size_t cur_num = arr.markers.size();
1177 unsigned int id = cur_num;
1178 for (std::size_t i = cur_num; i < arr.markers.size(); ++i, ++id)
1180 arr.markers[i].ns = ns;
1181 arr.markers[i].id = id;
1182 arr.markers[i].lifetime = dur;
1183 arr.markers[i].color = color;
1188 bool include_attached)
const
1190 rclcpp::Clock clock;
1191 for (
const std::string& link_name : link_names)
1193 RCLCPP_DEBUG(LOGGER,
"Trying to get marker for link '%s'", link_name.c_str());
1194 const LinkModel* link_model = robot_model_->getLinkModel(link_name);
1197 if (include_attached)
1198 for (
const auto& it : attached_body_map_)
1199 if (it.second->getAttachedLink() == link_model)
1201 for (std::size_t j = 0; j < it.second->getShapes().size(); ++j)
1203 visualization_msgs::msg::Marker att_mark;
1204 att_mark.header.frame_id = robot_model_->getModelFrame();
1205 att_mark.header.stamp = clock.now();
1206 if (shapes::constructMarkerFromShape(it.second->getShapes()[j].get(), att_mark))
1209 if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<float>::epsilon())
1211 att_mark.pose = tf2::toMsg(it.second->getGlobalCollisionBodyTransforms()[j]);
1212 arr.markers.push_back(att_mark);
1220 for (std::size_t j = 0; j < link_model->
getShapes().size(); ++j)
1222 visualization_msgs::msg::Marker mark;
1223 mark.header.frame_id = robot_model_->getModelFrame();
1224 mark.header.stamp = clock.now();
1228 if (mesh_resource.empty() || link_model->
getShapes().size() > 1)
1230 if (!shapes::constructMarkerFromShape(link_model->
getShapes()[j].get(), mark))
1233 if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<float>::epsilon())
1240 mark.type = mark.MESH_RESOURCE;
1241 mark.mesh_use_embedded_materials =
false;
1242 mark.mesh_resource = mesh_resource;
1245 mark.scale.x = mesh_scale[0];
1246 mark.scale.y = mesh_scale[1];
1247 mark.scale.z = mesh_scale[2];
1251 arr.markers.push_back(mark);
1259 Eigen::MatrixXd result;
1261 throw Exception(
"Unable to compute Jacobian");
1266 const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
1267 bool use_quaternion_representation)
const
1269 assert(checkLinkTransforms());
1271 if (!
group->isChain())
1273 RCLCPP_ERROR(LOGGER,
"The group '%s' is not a chain. Cannot compute Jacobian.",
group->getName().c_str());
1279 RCLCPP_ERROR(LOGGER,
"Link name '%s' does not exist in the chain '%s' or is not a child for this chain",
1287 Eigen::Isometry3d reference_transform =
1289 int rows = use_quaternion_representation ? 7 : 6;
1290 int columns =
group->getVariableCount();
1291 jacobian = Eigen::MatrixXd::Zero(rows, columns);
1295 Eigen::Vector3d point_transform = link_transform * reference_point_position;
1303 Eigen::Isometry3d joint_transform;
1321 unsigned int joint_index =
group->getVariableGroupIndex(pjm->
getName());
1327 jacobian.block<3, 1>(0, joint_index) =
1328 jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation());
1329 jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis;
1334 jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1339 jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1341 jacobian.block<3, 1>(0, joint_index + 1) = jacobian.block<3, 1>(0, joint_index + 1) + joint_axis;
1343 jacobian.block<3, 1>(0, joint_index + 2) = jacobian.block<3, 1>(0, joint_index + 2) +
1344 joint_axis.cross(point_transform - joint_transform.translation());
1345 jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis;
1348 RCLCPP_ERROR(LOGGER,
"Unknown type of joint in Jacobian computation");
1350 if (pjm == root_joint_model)
1354 if (use_quaternion_representation)
1361 Eigen::Quaterniond q(link_transform.linear());
1362 double w = q.w(),
x = q.x(),
y = q.y(),
z = q.z();
1363 Eigen::MatrixXd quaternion_update_matrix(4, 3);
1364 quaternion_update_matrix << -
x, -
y, -
z,
w, -
z,
y,
z,
w, -
x, -
y,
x,
w;
1365 jacobian.block(3, 0, 4, columns) = 0.5 * quaternion_update_matrix * jacobian.block(3, 0, 3, columns);
1373 Eigen::VectorXd qdot;
1381 Eigen::Matrix<double, 6, 1> t;
1382 tf2::fromMsg(twist, t);
1387 const Eigen::VectorXd& twist,
const LinkModel* tip)
const
1396 Eigen::MatrixXd e_wb = Eigen::ArrayXXd::Zero(6, 6);
1397 e_wb.block(0, 0, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1398 e_wb.block(3, 3, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1402 Eigen::JacobiSVD<Eigen::MatrixXd> svd_of_j(j, Eigen::ComputeThinU | Eigen::ComputeThinV);
1403 const Eigen::MatrixXd& u = svd_of_j.matrixU();
1404 const Eigen::MatrixXd& v = svd_of_j.matrixV();
1405 const Eigen::VectorXd& s = svd_of_j.singularValues();
1407 Eigen::VectorXd sinv = s;
1408 static const double PINVTOLER = std::numeric_limits<float>::epsilon();
1410 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1411 if (fabs(s(i)) > maxsv)
1413 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1416 if (fabs(s(i)) > maxsv * PINVTOLER)
1417 sinv(i) = 1.0 / s(i);
1421 Eigen::MatrixXd jinv = (v * sinv.asDiagonal() * u.transpose());
1424 qdot = jinv * twist;
1438 std::vector<double> values;
1440 return constraint(
this, jmg, &values[0]);
1454 RCLCPP_ERROR(LOGGER,
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1457 return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint,
options, cost_function);
1465 Eigen::Isometry3d mat;
1466 tf2::fromMsg(pose, mat);
1467 static std::vector<double> consistency_limits;
1468 return setFromIK(jmg, mat, tip, consistency_limits, timeout, constraint,
options, cost_function);
1479 RCLCPP_ERROR(LOGGER,
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1482 static std::vector<double> consistency_limits;
1483 return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, timeout, constraint,
options, cost_function);
1491 static std::vector<double> consistency_limits;
1492 return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint,
options, cost_function);
1499 const std::vector<double>& ik_sol, moveit_msgs::msg::MoveItErrorCodes& error_code)
1501 const std::vector<size_t>& bij =
group->getKinematicsSolverJointBijection();
1502 std::vector<double> solution(bij.size());
1503 for (std::size_t i = 0; i < bij.size(); ++i)
1504 solution[bij[i]] = ik_sol[i];
1505 if (constraint(state,
group, &solution[0]))
1506 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
1508 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
1524 getLinkModel((!ik_frame.empty() && ik_frame[0] ==
'/') ? ik_frame.substr(1) : ik_frame);
1527 RCLCPP_ERROR(LOGGER,
"The following IK frame does not exist: %s", ik_frame.c_str());
1536 const std::vector<double>& consistency_limits_in,
double timeout,
1542 EigenSTL::vector_Isometry3d poses;
1543 poses.push_back(pose_in);
1545 std::vector<std::string> tips;
1546 tips.push_back(tip_in);
1548 std::vector<std::vector<double> > consistency_limits;
1549 consistency_limits.push_back(consistency_limits_in);
1551 return setFromIK(jmg, poses, tips, consistency_limits, timeout, constraint,
options, cost_function);
1555 const std::vector<std::string>& tips_in,
double timeout,
1560 const std::vector<std::vector<double> > consistency_limits;
1561 return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint,
options, cost_function);
1565 const std::vector<std::string>& tips_in,
1566 const std::vector<std::vector<double> >& consistency_limit_sets,
double timeout,
1572 if (poses_in.size() != tips_in.size())
1574 RCLCPP_ERROR(LOGGER,
"Number of poses must be the same as number of tips");
1582 bool valid_solver =
true;
1585 valid_solver =
false;
1588 else if (poses_in.size() > 1)
1590 std::string error_msg;
1591 if (!solver->supportsGroup(jmg, &error_msg))
1594 RCLCPP_ERROR(LOGGER,
"Kinematics solver %s does not support joint group %s. Error: %s",
1595 typeid(solver_ref).
name(), jmg->
getName().c_str(), error_msg.c_str());
1596 valid_solver =
false;
1603 if (poses_in.size() > 1)
1610 RCLCPP_ERROR(LOGGER,
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1616 std::vector<double> consistency_limits;
1617 if (consistency_limit_sets.size() > 1)
1619 RCLCPP_ERROR(LOGGER,
1620 "Invalid number (%zu) of sets of consistency limits for a setFromIK request "
1621 "that is being solved by a single IK solver",
1622 consistency_limit_sets.size());
1625 else if (consistency_limit_sets.size() == 1)
1626 consistency_limits = consistency_limit_sets[0];
1628 const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
1631 std::vector<bool> tip_frames_used(solver_tip_frames.size(),
false);
1634 std::vector<geometry_msgs::msg::Pose> ik_queries(solver_tip_frames.size());
1637 for (std::size_t i = 0; i < poses_in.size(); ++i)
1640 Eigen::Isometry3d pose = poses_in[i];
1641 std::string pose_frame = tips_in[i];
1644 if (!pose_frame.empty() && pose_frame[0] ==
'/')
1645 pose_frame = pose_frame.substr(1);
1653 bool found_valid_frame =
false;
1654 std::size_t solver_tip_id;
1655 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1658 if (tip_frames_used[solver_tip_id])
1662 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1666 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1667 solver_tip_frame = solver_tip_frame.substr(1);
1669 if (pose_frame != solver_tip_frame)
1674 pose_frame = body->getAttachedLinkName();
1675 pose = pose * body->getPose().inverse();
1677 if (pose_frame != solver_tip_frame)
1682 RCLCPP_ERROR(LOGGER,
"The following Pose Frame does not exist: %s", pose_frame.c_str());
1686 for (
const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
1689 pose_frame = solver_tip_frame;
1690 pose = pose * fixed_link.second;
1698 if (pose_frame == solver_tip_frame)
1700 found_valid_frame =
true;
1707 if (!found_valid_frame)
1709 RCLCPP_ERROR(LOGGER,
"Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str());
1711 std::stringstream ss;
1712 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1713 ss << solver_tip_frames[solver_tip_id] <<
", ";
1714 RCLCPP_ERROR(LOGGER,
"Available tip frames: [%s]", ss.str().c_str());
1719 tip_frames_used[solver_tip_id] =
true;
1722 geometry_msgs::msg::Pose ik_query;
1723 ik_query = tf2::toMsg(pose);
1726 ik_queries[solver_tip_id] = ik_query;
1730 for (std::size_t solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1733 if (tip_frames_used[solver_tip_id])
1737 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1741 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1742 solver_tip_frame = solver_tip_frame.substr(1);
1752 geometry_msgs::msg::Pose ik_query;
1753 ik_query = tf2::toMsg(current_pose);
1756 ik_queries[solver_tip_id] = ik_query;
1759 tip_frames_used[solver_tip_id] =
true;
1763 if (timeout < std::numeric_limits<double>::epsilon())
1769 ik_callback_fn = ik_callback_fn = [
this, jmg, constraint](
const geometry_msgs::msg::Pose& pose,
1770 const std::vector<double>& joints,
1771 moveit_msgs::msg::MoveItErrorCodes& error_code) {
1772 ikCallbackFnAdapter(
this, jmg, constraint, pose, joints, error_code);
1778 std::vector<double> initial_values;
1780 std::vector<double> seed(bij.size());
1781 for (std::size_t i = 0; i < bij.size(); ++i)
1782 seed[i] = initial_values[bij[i]];
1785 std::vector<double> ik_sol;
1786 moveit_msgs::msg::MoveItErrorCodes error;
1788 if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, cost_function,
1791 std::vector<double> solution(bij.size());
1792 for (std::size_t i = 0; i < bij.size(); ++i)
1793 solution[bij[i]] = ik_sol[i];
1801 const std::vector<std::string>& tips_in,
1802 const std::vector<std::vector<double> >& consistency_limits,
double timeout,
1809 std::vector<const JointModelGroup*> sub_groups;
1813 if (poses_in.size() != sub_groups.size())
1815 RCLCPP_ERROR(LOGGER,
"Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(),
1820 if (tips_in.size() != sub_groups.size())
1822 RCLCPP_ERROR(LOGGER,
"Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(),
1827 if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
1829 RCLCPP_ERROR(LOGGER,
"Number of consistency limit vectors must be the same as number of sub-groups");
1833 for (std::size_t i = 0; i < consistency_limits.size(); ++i)
1837 RCLCPP_ERROR(LOGGER,
"Number of joints in consistency_limits is %zu but it should be should be %u", i,
1844 std::vector<kinematics::KinematicsBaseConstPtr> solvers;
1845 for (std::size_t i = 0; i < poses_in.size(); ++i)
1847 kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
1850 RCLCPP_ERROR(LOGGER,
"Could not find solver for group '%s'", sub_groups[i]->getName().c_str());
1853 solvers.push_back(solver);
1857 EigenSTL::vector_Isometry3d transformed_poses = poses_in;
1858 std::vector<std::string> pose_frames = tips_in;
1861 for (std::size_t i = 0; i < poses_in.size(); ++i)
1863 ASSERT_ISOMETRY(transformed_poses[i])
1864 Eigen::Isometry3d& pose = transformed_poses[i];
1865 std::string& pose_frame = pose_frames[i];
1872 std::string solver_tip_frame = solvers[i]->getTipFrame();
1876 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1877 solver_tip_frame = solver_tip_frame.substr(1);
1879 if (pose_frame != solver_tip_frame)
1885 pose = pose * body->
getPose().inverse();
1887 if (pose_frame != solver_tip_frame)
1894 for (
const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
1895 if (fixed_link.first->getName() == solver_tip_frame)
1897 pose_frame = solver_tip_frame;
1898 pose = pose * fixed_link.second;
1904 if (pose_frame != solver_tip_frame)
1906 RCLCPP_ERROR(LOGGER,
"Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(),
1907 solver_tip_frame.c_str());
1913 std::vector<geometry_msgs::msg::Pose> ik_queries(poses_in.size());
1916 ik_callback_fn = [
this, jmg, constraint](
const geometry_msgs::msg::Pose pose,
const std::vector<double>& joints,
1917 moveit_msgs::msg::MoveItErrorCodes& error_code) {
1918 ikCallbackFnAdapter(
this, jmg, constraint, pose, joints, error_code);
1921 for (std::size_t i = 0; i < transformed_poses.size(); ++i)
1923 Eigen::Quaterniond quat(transformed_poses[i].linear());
1925 ik_queries[i].position.x = point.x();
1926 ik_queries[i].position.y = point.y();
1927 ik_queries[i].position.z = point.z();
1928 ik_queries[i].orientation.x = quat.x();
1929 ik_queries[i].orientation.y = quat.y();
1930 ik_queries[i].orientation.z = quat.z();
1931 ik_queries[i].orientation.w = quat.w();
1935 if (timeout < std::numeric_limits<double>::epsilon())
1938 auto start = std::chrono::system_clock::now();
1941 bool first_seed =
true;
1942 unsigned int attempts = 0;
1946 RCLCPP_DEBUG(LOGGER,
"IK attempt: %d", attempts);
1947 bool found_solution =
true;
1948 for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
1950 const std::vector<size_t>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
1951 std::vector<double> seed(bij.size());
1955 std::vector<double> initial_values;
1957 for (std::size_t i = 0; i < bij.size(); ++i)
1958 seed[i] = initial_values[bij[i]];
1964 std::vector<double> random_values;
1965 sub_groups[sg]->getVariableRandomPositions(rng, random_values);
1966 for (std::size_t i = 0; i < bij.size(); ++i)
1967 seed[i] = random_values[bij[i]];
1971 std::vector<double> ik_sol;
1972 moveit_msgs::msg::MoveItErrorCodes error;
1973 const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
1974 if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, (timeout - elapsed) / sub_groups.size(), climits, ik_sol,
1977 std::vector<double> solution(bij.size());
1978 for (std::size_t i = 0; i < bij.size(); ++i)
1979 solution[bij[i]] = ik_sol[i];
1984 found_solution =
false;
1990 std::vector<double> full_solution;
1992 if (constraint ? constraint(
this, jmg, &full_solution[0]) :
true)
1994 RCLCPP_DEBUG(LOGGER,
"Found IK solution");
1998 elapsed = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start).count();
2000 }
while (elapsed < timeout);
2006 bool global_reference_frame,
double distance,
double max_step,
2016 const LinkModel* link,
const Eigen::Isometry3d& target,
2017 bool global_reference_frame,
double max_step,
double jump_threshold_factor,
2028 bool global_reference_frame,
double max_step,
double jump_threshold_factor,
2039 assert(checkLinkTransforms());
2042 std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
2047 transform.translate(link->getCenteredBoundingBoxOffset());
2050 for (
const auto& it : attached_body_map_)
2052 const EigenSTL::vector_Isometry3d& transforms = it.second->getGlobalCollisionBodyTransforms();
2053 const std::vector<shapes::ShapeConstPtr>&
shapes = it.second->getShapes();
2054 for (std::size_t i = 0; i < transforms.size(); ++i)
2062 aabb.resize(6, 0.0);
2063 if (!bounding_box.isEmpty())
2067 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data(), 3) = bounding_box.min();
2068 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data() + 1, 3) = bounding_box.max();
2074 const std::vector<std::string>& nm = robot_model_->getVariableNames();
2075 for (std::size_t i = 0; i < nm.size(); ++i)
2076 out << nm[i] <<
"=" << position_[i] <<
'\n';
2090 if (joint->getVariableCount() > 1)
2103 out <<
" " << std::fixed << std::setprecision(5) << bound.
min_position_ <<
"\t";
2105 double step = delta / 20.0;
2107 bool marker_shown =
false;
2111 if (!marker_shown && current_value < value)
2114 marker_shown =
true;
2123 out <<
" \t" << std::fixed << std::setprecision(5) << bound.
max_position_ <<
" \t" << joint->getName()
2124 <<
" current: " << std::fixed << std::setprecision(5) << current_value <<
'\n';
2133 out <<
" * Dirty Joint Transforms: \n";
2134 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2137 out <<
" " << joint->getName() <<
'\n';
2138 out <<
" * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->
getName() :
"NULL") <<
'\n';
2139 out <<
" * Dirty Collision Body Transforms: "
2140 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->
getName() :
"NULL\n");
2145 out <<
"Robot State @" <<
this <<
'\n';
2147 std::size_t n = robot_model_->getVariableCount();
2150 out <<
" * Position: ";
2151 for (std::size_t i = 0; i < n; ++i)
2152 out << position_[i] <<
" ";
2156 out <<
" * Position: NULL\n";
2160 out <<
" * Velocity: ";
2161 for (std::size_t i = 0; i < n; ++i)
2162 out << velocity_[i] <<
" ";
2166 out <<
" * Velocity: NULL\n";
2170 out <<
" * Acceleration: ";
2171 for (std::size_t i = 0; i < n; ++i)
2172 out << acceleration_[i] <<
" ";
2176 out <<
" * Acceleration: NULL\n";
2178 out <<
" * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->
getName() :
"NULL\n");
2179 out <<
" * Dirty Collision Body Transforms: "
2180 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->
getName() :
"NULL\n");
2187 if (checkIsometry(transform, CHECK_ISOMETRY_PRECISION,
false))
2189 Eigen::Quaterniond q(transform.linear());
2190 out <<
"T.xyz = [" << transform.translation().x() <<
", " << transform.translation().y() <<
", "
2191 << transform.translation().z() <<
"], Q.xyzw = [" << q.x() <<
", " << q.y() <<
", " << q.z() <<
", " << q.w()
2196 out <<
"[NON-ISOMETRY] "
2197 << transform.matrix().format(
2198 Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols,
", ",
"; ",
"",
"",
"[",
"]"));
2205 if (!variable_joint_transforms_)
2207 out <<
"No transforms computed\n";
2211 out <<
"Joint transforms:\n";
2212 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2215 out <<
" " << joint->getName();
2216 const int idx = joint->getJointIndex();
2217 if (dirty_joint_transforms_[idx])
2223 out <<
"Link poses:\n";
2224 const std::vector<const LinkModel*>& link_model = robot_model_->getLinkModels();
2225 for (
const LinkModel* link : link_model)
2227 out <<
" " << link->getName() <<
": ";
2228 printTransform(global_link_transforms_[link->getLinkIndex()], out);
2234 std::stringstream ss;
2235 ss <<
"ROBOT: " << robot_model_->getName() <<
'\n';
2236 getStateTreeJointString(ss, robot_model_->getRootJoint(),
" ",
true);
2242 void getPoseString(std::ostream& ss,
const Eigen::Isometry3d& pose,
const std::string& pfx)
2245 for (
int y = 0;
y < 4; ++
y)
2248 for (
int x = 0;
x < 4; ++
x)
2250 ss << std::setw(8) << pose(
y,
x) <<
" ";
2257 void RobotState::getStateTreeJointString(std::ostream& ss,
const JointModel* jm,
const std::string& pfx0,
2260 std::string pfx = pfx0 +
"+--";
2262 ss << pfx <<
"Joint: " << jm->getName() <<
'\n';
2264 pfx = pfx0 + (last ?
" " :
"| ");
2266 for (std::size_t i = 0; i < jm->getVariableCount(); ++i)
2269 ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] <<
'\n';
2272 const LinkModel* link_model = jm->getChildLinkModel();
2274 ss << pfx <<
"Link: " << link_model->
getName() <<
'\n';
2275 getPoseString(ss, link_model->getJointOriginTransform(), pfx +
"joint_origin:");
2276 if (variable_joint_transforms_)
2278 getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx +
"joint_variable:");
2279 getPoseString(ss, global_link_transforms_[link_model->getLinkIndex()], pfx +
"link_global:");
2282 for (std::vector<const JointModel*>::const_iterator it = link_model->getChildJointModels().begin();
2283 it != link_model->getChildJointModels().end(); ++it)
2284 getStateTreeJointString(ss, *it, pfx, it + 1 == link_model->getChildJointModels().end());
Provides an interface for kinematics solvers.
std::function< double(const geometry_msgs::msg::Pose &, const moveit::core::RobotState &, moveit::core::JointModelGroup const *, const std::vector< double > &)> IKCostFn
Signature for a cost function used to evaluate IK solutions.
std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
This may be thrown if unrecoverable errors occur.
Represents an axis-aligned bounding box.
void extendWithTransformedBox(const Eigen::Isometry3d &transform, const Eigen::Vector3d &box)
Extend with a box transformed by the given transform.
Object defining bodies that can be attached to robot links.
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
const Eigen::Isometry3d & getPose() const
Get the pose of the attached body relative to the parent link.
static Distance computeCartesianPath(RobotState *start_state, const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, const LinkModel *link, const Eigen::Vector3d &translation, bool global_reference_frame, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn())
Compute the sequence of joint values that correspond to a straight Cartesian path for a particular li...
double getDefaultIKTimeout() const
Get the default IK timeout.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
const std::string & getName() const
Get the name of the joint group.
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
void getSubgroups(std::vector< const JointModelGroup * > &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
const std::vector< size_t > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
const LinkModel * getParentLinkModel() const
Get the link that this joint connects to. The robot is assumed to start with a joint,...
size_t getJointIndex() const
Get the index of this joint within the robot model.
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
size_t getFirstVariableIndex() const
Get the index of this joint's first variable within the full robot state.
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
const std::string & getName() const
Get the name of the joint.
const std::vector< const LinkModel * > & getDescendantLinkModels() const
Get all the link models that descend from this joint, in the kinematic tree.
virtual void computeTransform(const double *joint_values, Eigen::Isometry3d &transf) const =0
Given the joint values for a joint, compute the corresponding transform. The computed transform is gu...
JointType getType() const
Get the type of joint.
A link from the robot. Contains the constant transform applied to the link and its geometry.
int getFirstCollisionBodyTransformIndex() const
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms. The returned transforms are ...
const Eigen::Isometry3d & getVisualMeshOrigin() const
Get the transform for the visual mesh origin.
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
const Eigen::Vector3d & getVisualMeshScale() const
Get the scale of the mesh resource for this link.
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
const std::string & getName() const
The name of this link.
const Eigen::Isometry3d & getJointOriginTransform() const
When transforms are computed for this link, they are usually applied to the link's origin....
const LinkModel * getParentLinkModel() const
Get the link model whose child this link is (through some joint). There may not always be a parent li...
const std::vector< const JointModel * > & getChildJointModels() const
A link may have 0 or more child joints. From those joints there will certainly be other descendant li...
size_t getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
const std::string & getVisualMeshFilename() const
Get the filename of the mesh resource used for visual display of this link.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
void zeroVelocities()
Set all velocities to 0.0.
RobotState & operator=(const RobotState &other)
Copy operator.
void setJointEfforts(const JointModel *joint, const double *effort)
void setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback)
void printTransforms(std::ostream &out=std::cout) const
void printTransform(const Eigen::Isometry3d &transform, std::ostream &out=std::cout) const
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Isometry3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
Compute the Jacobian with reference to a particular point on a given link, for a specified group.
const double * getJointPositions(const std::string &joint_name) const
std::pair< double, const JointModel * > getMinDistanceToPositionBounds() const
Get the minimm distance from this state to the bounds. The minimum distance and the joint for which t...
void copyJointGroupVelocities(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
void setJointGroupAccelerations(const std::string &joint_group_name, const double *gstate)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
void setJointGroupVelocities(const std::string &joint_group_name, const double *gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel *tip) const
Given a twist for a particular link (tip), compute the corresponding velocity for every variable and ...
void computeAABB(std::vector< double > &aabb) const
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx,...
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)....
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
void harmonizePositions()
Call harmonizePosition() for all joints / all joints in group / given joint.
void setVariableVelocities(const double *velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
bool dirtyCollisionBodyTransforms() const
void zeroAccelerations()
Set all accelerations to 0.0.
bool isValidVelocityMove(const RobotState &other, const JointModelGroup *group, double dt) const
Check that the time to move between two waypoints is sufficient given velocity limits and time step.
bool setFromDiffIK(const JointModelGroup *group, const Eigen::VectorXd &twist, const std::string &tip, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Set the joint values from a Cartesian velocity applied during a time dt.
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully....
void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup *jmg, std::ostream &out=std::cout) const
Output to console the current state of the robot's joint limits.
void dropDynamics()
Reduce RobotState to kinematic information (remove velocity, acceleration and effort,...
bool dirtyJointTransform(const JointModel *joint) const
void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &seed, double distance)
Set all joints in group to random values near the value in seed. distance is the maximum amount each ...
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
void interpolate(const RobotState &to, double t, RobotState &state) const
double distance(const RobotState &other) const
Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints.
std::string getStateTreeString() const
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
bool satisfiesBounds(double margin=0.0) const
void setVariableEffort(const double *effort)
Given an array with effort values for all variables, set those values as the effort in this state.
void setJointGroupActivePositions(const JointModelGroup *group, const std::vector< double > &gstate)
Given positions for the variables of active joints that make up a group, in the order found in the gr...
bool dirtyLinkTransforms() const
void printStateInfo(std::ostream &out=std::cout) const
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
void update(bool force=false)
Update all transforms.
void dropAccelerations()
Remove accelerations from this state (this differs from setting them to zero)
void getRobotMarkers(visualization_msgs::msg::MarkerArray &arr, const std::vector< std::string > &link_names, const std_msgs::msg::ColorRGBA &color, const std::string &ns, const rclcpp::Duration &dur, bool include_attached=false) const
Get a MarkerArray that fully describes the robot markers for a given robot.
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return nullptr if not found.
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double >> &consistency_limits, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solv...
bool setToIKSolverFrame(Eigen::Isometry3d &pose, const kinematics::KinematicsBaseConstPtr &solver)
Transform pose from the robot model's base frame to the reference frame of the IK solver.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void printStatePositions(std::ostream &out=std::cout) const
std::size_t getVariableCount() const
Get the number of variables that make up this state.
void printDirtyInfo(std::ostream &out=std::cout) const
void setVariableAccelerations(const double *acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
void copyJointGroupAccelerations(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
RobotState(const RobotModelConstPtr &robot_model)
A state can be constructed from a specified robot model. No values are initialized....
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Compute the sequence of joint values that correspond to a straight Cartesian path for a particular gr...
void dropEffort()
Remove effort values from this state (this differs from setting them to zero)
bool knowsFrameTransform(const std::string &frame_id) const
Check if a transformation matrix from the model frame (root of model) to frame frame_id is known.
bool integrateVariableVelocity(const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Given the velocities for the variables in this group (qdot) and an amount of time (dt),...
bool hasAttachedBody(const std::string &id) const
Check if an attached body named id exists in this state.
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
void invertVelocity()
Invert velocity if present.
const Eigen::Isometry3d & getFrameInfo(const std::string &frame_id, const LinkModel *&robot_link, bool &frame_found) const
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
void harmonizePosition(const JointModel *joint)
void clearAttachedBodies()
Clear all attached bodies. This calls delete on the AttachedBody instances, if needed.
void dropVelocities()
Remove velocities from this state (this differs from setting them to zero)
void zeroEffort()
Set all effort values to 0.0.
const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const std::string &frame) const
Get the latest link upwards the kinematic tree which is only connected via fixed joints.
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
bool setFromIK(const JointModelGroup *group, const geometry_msgs::msg::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn())
If the group this state corresponds to is a chain and a solver is available, then the joint values ca...
void setJointPositions(const std::string &joint_name, const double *position)
#define MOVEIT_CONSOLE_COLOR_RESET
#define MOVEIT_CONSOLE_COLOR_RED
Vec3fX< details::Vec3Data< double > > Vector3d
std::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
std::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Main namespace for MoveIt.
double distance(const urdf::Pose &transform)
A set of options for the kinematics solver.
Struct for containing jump_threshold.
Struct for containing max_step for computeCartesianPath.