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()]);
807 if ((idx = frame.find(
'/')) != std::string::npos)
809 std::string
object{ frame.substr(0, idx) };
813 if (!body->hasSubframeTransform(frame))
824 return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
829 const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
838 const std::vector<const JointModel*>& jm =
group->getActiveJointModels();
847 const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
861 for (
const JointModel* jm : robot_model_->getActiveJointModels())
881 std::pair<double, const JointModel*>
884 double distance = std::numeric_limits<double>::max();
896 std::vector<double> lower_bounds(bounds.size()), upper_bounds(bounds.size());
897 for (std::size_t j = 0; j < bounds.size(); ++j)
899 lower_bounds[j] = bounds[j].min_position_;
900 upper_bounds[j] = bounds[j].max_position_;
902 double new_distance = joint->distance(joint_values, &lower_bounds[0]);
908 new_distance = joint->distance(joint_values, &upper_bounds[0]);
915 return std::make_pair(
distance, index);
920 const std::vector<const JointModel*>& jm =
group->getActiveJointModels();
923 const int idx = joint_id->getFirstVariableIndex();
924 const std::vector<VariableBounds>& bounds = joint_id->getVariableBounds();
927 for (std::size_t var_id = 0; var_id < joint_id->getVariableCount(); ++var_id)
929 const double dtheta = std::abs(*(position_ + idx + var_id) - *(other.
getVariablePositions() + idx + var_id));
931 if (dtheta > dt * bounds[var_id].max_velocity_)
944 const int idx = joint->getFirstVariableIndex();
945 d += joint->getDistanceFactor() * joint->distance(position_ + idx, other.position_ + idx);
952 moveit::core::checkInterpolationParamBounds(LOGGER, t);
955 memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() *
sizeof(
unsigned char));
956 state.dirty_link_transforms_ = state.robot_model_->getRootJoint();
961 moveit::core::checkInterpolationParamBounds(LOGGER, t);
965 const int idx = joint->getFirstVariableIndex();
966 joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
968 state.updateMimicJoints(joint_group);
973 attached_body_update_callback_ = callback;
978 return attached_body_map_.find(
id) != attached_body_map_.end();
983 const auto it = attached_body_map_.find(
id);
984 if (it == attached_body_map_.end())
986 RCLCPP_ERROR(LOGGER,
"Attached body '%s' not found",
id.c_str());
990 return it->second.get();
999 if (attached_body_update_callback_)
1000 attached_body_update_callback_(attached_body.get(),
true);
1001 attached_body_map_[attached_body->getName()] = std::move(attached_body);
1006 attachBody(std::unique_ptr<AttachedBody>(attached_body));
1010 const std::vector<shapes::ShapeConstPtr>&
shapes,
1011 const EigenSTL::vector_Isometry3d& shape_poses,
const std::set<std::string>& touch_links,
1012 const std::string& link,
const trajectory_msgs::msg::JointTrajectory& detach_posture,
1015 attachBody(std::make_unique<AttachedBody>(robot_model_->getLinkModel(link),
id, pose,
shapes, shape_poses,
1016 touch_links, detach_posture, subframe_poses));
1021 attached_bodies.clear();
1022 attached_bodies.reserve(attached_body_map_.size());
1023 for (
const auto& it : attached_body_map_)
1024 attached_bodies.push_back(it.second.get());
1029 attached_bodies.clear();
1030 for (
const auto& it : attached_body_map_)
1031 if (
group->hasLinkModel(it.second->getAttachedLinkName()))
1032 attached_bodies.push_back(it.second.get());
1037 attached_bodies.clear();
1038 for (
const auto& it : attached_body_map_)
1039 if (it.second->getAttachedLink() == link_model)
1040 attached_bodies.push_back(it.second.get());
1045 for (
const auto& it : attached_body_map_)
1047 if (attached_body_update_callback_)
1048 attached_body_update_callback_(it.second.get(),
false);
1050 attached_body_map_.clear();
1055 for (
auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1057 if (it->second->getAttachedLink() != link)
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 for (
auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1072 if (!
group->hasLinkModel(it->second->getAttachedLinkName()))
1076 if (attached_body_update_callback_)
1077 attached_body_update_callback_(it->second.get(),
false);
1078 const auto del = it++;
1079 attached_body_map_.erase(del);
1085 const auto it = attached_body_map_.find(
id);
1086 if (it != attached_body_map_.end())
1088 if (attached_body_update_callback_)
1089 attached_body_update_callback_(it->second.get(),
false);
1090 attached_body_map_.erase(it);
1110 *frame_found = found;
1112 RCLCPP_WARN(LOGGER,
"getFrameTransform() did not find a frame with name %s.",
frame_id.c_str());
1118 bool& frame_found)
const
1123 static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
1124 if (
frame_id == robot_model_->getModelFrame())
1126 robot_link = robot_model_->getRootLink();
1128 return IDENTITY_TRANSFORM;
1130 if ((robot_link = robot_model_->getLinkModel(
frame_id, &frame_found)))
1132 assert(checkLinkTransforms());
1133 return global_link_transforms_[robot_link->
getLinkIndex()];
1135 robot_link =
nullptr;
1138 const auto jt = attached_body_map_.find(
frame_id);
1139 if (jt != attached_body_map_.end())
1141 const Eigen::Isometry3d& transform = jt->second->getGlobalPose();
1142 robot_link = jt->second->getAttachedLink();
1144 assert(checkLinkTransforms());
1149 for (
const auto& body : attached_body_map_)
1151 const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(
frame_id, &frame_found);
1154 robot_link = body.second->getAttachedLink();
1155 assert(checkLinkTransforms());
1160 robot_link =
nullptr;
1161 frame_found =
false;
1162 return IDENTITY_TRANSFORM;
1169 if (robot_model_->hasLinkModel(
frame_id))
1173 const auto it = attached_body_map_.find(
frame_id);
1174 if (it != attached_body_map_.end())
1175 return !it->second->getGlobalCollisionBodyTransforms().empty();
1178 for (
const auto& body : attached_body_map_)
1180 if (body.second->hasSubframeTransform(
frame_id))
1187 const std_msgs::msg::ColorRGBA& color,
const std::string& ns,
1188 const rclcpp::Duration& dur,
bool include_attached)
const
1190 std::size_t cur_num = arr.markers.size();
1192 unsigned int id = cur_num;
1193 for (std::size_t i = cur_num; i < arr.markers.size(); ++i, ++id)
1195 arr.markers[i].ns = ns;
1196 arr.markers[i].id = id;
1197 arr.markers[i].lifetime = dur;
1198 arr.markers[i].color = color;
1203 bool include_attached)
const
1205 rclcpp::Clock clock;
1206 for (
const std::string& link_name : link_names)
1208 RCLCPP_DEBUG(LOGGER,
"Trying to get marker for link '%s'", link_name.c_str());
1209 const LinkModel* link_model = robot_model_->getLinkModel(link_name);
1212 if (include_attached)
1213 for (
const auto& it : attached_body_map_)
1214 if (it.second->getAttachedLink() == link_model)
1216 for (std::size_t j = 0; j < it.second->getShapes().size(); ++j)
1218 visualization_msgs::msg::Marker att_mark;
1219 att_mark.header.frame_id = robot_model_->getModelFrame();
1220 att_mark.header.stamp = clock.now();
1221 if (shapes::constructMarkerFromShape(it.second->getShapes()[j].get(), att_mark))
1224 if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<float>::epsilon())
1226 att_mark.pose = tf2::toMsg(it.second->getGlobalCollisionBodyTransforms()[j]);
1227 arr.markers.push_back(att_mark);
1235 for (std::size_t j = 0; j < link_model->
getShapes().size(); ++j)
1237 visualization_msgs::msg::Marker mark;
1238 mark.header.frame_id = robot_model_->getModelFrame();
1239 mark.header.stamp = clock.now();
1243 if (mesh_resource.empty() || link_model->
getShapes().size() > 1)
1245 if (!shapes::constructMarkerFromShape(link_model->
getShapes()[j].get(), mark))
1248 if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<float>::epsilon())
1255 mark.type = mark.MESH_RESOURCE;
1256 mark.mesh_use_embedded_materials =
false;
1257 mark.mesh_resource = mesh_resource;
1260 mark.scale.x = mesh_scale[0];
1261 mark.scale.y = mesh_scale[1];
1262 mark.scale.z = mesh_scale[2];
1266 arr.markers.push_back(mark);
1274 Eigen::MatrixXd result;
1276 throw Exception(
"Unable to compute Jacobian");
1281 const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
1282 bool use_quaternion_representation)
const
1284 assert(checkLinkTransforms());
1286 if (!
group->isChain())
1288 RCLCPP_ERROR(LOGGER,
"The group '%s' is not a chain. Cannot compute Jacobian.",
group->getName().c_str());
1294 RCLCPP_ERROR(LOGGER,
"Link name '%s' does not exist in the chain '%s' or is not a child for this chain",
1302 Eigen::Isometry3d reference_transform =
1304 int rows = use_quaternion_representation ? 7 : 6;
1305 int columns =
group->getVariableCount();
1306 jacobian = Eigen::MatrixXd::Zero(rows, columns);
1310 Eigen::Vector3d point_transform = link_transform * reference_point_position;
1318 Eigen::Isometry3d joint_transform;
1336 unsigned int joint_index =
group->getVariableGroupIndex(pjm->
getName());
1342 jacobian.block<3, 1>(0, joint_index) =
1343 jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation());
1344 jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis;
1349 jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1354 jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1356 jacobian.block<3, 1>(0, joint_index + 1) = jacobian.block<3, 1>(0, joint_index + 1) + joint_axis;
1358 jacobian.block<3, 1>(0, joint_index + 2) = jacobian.block<3, 1>(0, joint_index + 2) +
1359 joint_axis.cross(point_transform - joint_transform.translation());
1360 jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis;
1363 RCLCPP_ERROR(LOGGER,
"Unknown type of joint in Jacobian computation");
1365 if (pjm == root_joint_model)
1369 if (use_quaternion_representation)
1376 Eigen::Quaterniond q(link_transform.linear());
1377 double w = q.w(),
x = q.x(),
y = q.y(),
z = q.z();
1378 Eigen::MatrixXd quaternion_update_matrix(4, 3);
1379 quaternion_update_matrix << -
x, -
y, -
z,
w, -
z,
y,
z,
w, -
x, -
y,
x,
w;
1380 jacobian.block(3, 0, 4, columns) = 0.5 * quaternion_update_matrix * jacobian.block(3, 0, 3, columns);
1388 Eigen::VectorXd qdot;
1396 Eigen::Matrix<double, 6, 1> t;
1397 tf2::fromMsg(twist, t);
1402 const Eigen::VectorXd& twist,
const LinkModel* tip)
const
1411 Eigen::MatrixXd e_wb = Eigen::ArrayXXd::Zero(6, 6);
1412 e_wb.block(0, 0, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1413 e_wb.block(3, 3, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1417 Eigen::JacobiSVD<Eigen::MatrixXd> svd_of_j(j, Eigen::ComputeThinU | Eigen::ComputeThinV);
1418 const Eigen::MatrixXd& u = svd_of_j.matrixU();
1419 const Eigen::MatrixXd& v = svd_of_j.matrixV();
1420 const Eigen::VectorXd& s = svd_of_j.singularValues();
1422 Eigen::VectorXd sinv = s;
1423 static const double PINVTOLER = std::numeric_limits<float>::epsilon();
1425 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1426 if (fabs(s(i)) > maxsv)
1428 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1431 if (fabs(s(i)) > maxsv * PINVTOLER)
1432 sinv(i) = 1.0 / s(i);
1436 Eigen::MatrixXd jinv = (v * sinv.asDiagonal() * u.transpose());
1439 qdot = jinv * twist;
1453 std::vector<double> values;
1455 return constraint(
this, jmg, &values[0]);
1469 RCLCPP_ERROR(LOGGER,
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1472 return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint,
options, cost_function);
1480 Eigen::Isometry3d mat;
1481 tf2::fromMsg(pose, mat);
1482 static std::vector<double> consistency_limits;
1483 return setFromIK(jmg, mat, tip, consistency_limits, timeout, constraint,
options, cost_function);
1494 RCLCPP_ERROR(LOGGER,
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1497 static std::vector<double> consistency_limits;
1498 return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, timeout, constraint,
options, cost_function);
1506 static std::vector<double> consistency_limits;
1507 return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint,
options, cost_function);
1514 const std::vector<double>& ik_sol, moveit_msgs::msg::MoveItErrorCodes& error_code)
1516 const std::vector<size_t>& bij =
group->getKinematicsSolverJointBijection();
1517 std::vector<double> solution(bij.size());
1518 for (std::size_t i = 0; i < bij.size(); ++i)
1519 solution[bij[i]] = ik_sol[i];
1520 if (constraint(state,
group, &solution[0]))
1521 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
1523 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
1539 getLinkModel((!ik_frame.empty() && ik_frame[0] ==
'/') ? ik_frame.substr(1) : ik_frame);
1542 RCLCPP_ERROR(LOGGER,
"The following IK frame does not exist: %s", ik_frame.c_str());
1551 const std::vector<double>& consistency_limits_in,
double timeout,
1557 EigenSTL::vector_Isometry3d poses;
1558 poses.push_back(pose_in);
1560 std::vector<std::string> tips;
1561 tips.push_back(tip_in);
1563 std::vector<std::vector<double> > consistency_limits;
1564 consistency_limits.push_back(consistency_limits_in);
1566 return setFromIK(jmg, poses, tips, consistency_limits, timeout, constraint,
options, cost_function);
1570 const std::vector<std::string>& tips_in,
double timeout,
1575 const std::vector<std::vector<double> > consistency_limits;
1576 return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint,
options, cost_function);
1580 const std::vector<std::string>& tips_in,
1581 const std::vector<std::vector<double> >& consistency_limit_sets,
double timeout,
1587 if (poses_in.size() != tips_in.size())
1589 RCLCPP_ERROR(LOGGER,
"Number of poses must be the same as number of tips");
1597 bool valid_solver =
true;
1600 valid_solver =
false;
1603 else if (poses_in.size() > 1)
1605 std::string error_msg;
1606 if (!solver->supportsGroup(jmg, &error_msg))
1609 RCLCPP_ERROR(LOGGER,
"Kinematics solver %s does not support joint group %s. Error: %s",
1610 typeid(solver_ref).
name(), jmg->
getName().c_str(), error_msg.c_str());
1611 valid_solver =
false;
1618 if (poses_in.size() > 1)
1625 RCLCPP_ERROR(LOGGER,
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1631 std::vector<double> consistency_limits;
1632 if (consistency_limit_sets.size() > 1)
1634 RCLCPP_ERROR(LOGGER,
1635 "Invalid number (%zu) of sets of consistency limits for a setFromIK request "
1636 "that is being solved by a single IK solver",
1637 consistency_limit_sets.size());
1640 else if (consistency_limit_sets.size() == 1)
1641 consistency_limits = consistency_limit_sets[0];
1643 const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
1646 std::vector<bool> tip_frames_used(solver_tip_frames.size(),
false);
1649 std::vector<geometry_msgs::msg::Pose> ik_queries(solver_tip_frames.size());
1652 for (std::size_t i = 0; i < poses_in.size(); ++i)
1655 Eigen::Isometry3d pose = poses_in[i];
1656 std::string pose_frame = tips_in[i];
1659 if (!pose_frame.empty() && pose_frame[0] ==
'/')
1660 pose_frame = pose_frame.substr(1);
1668 bool found_valid_frame =
false;
1669 std::size_t solver_tip_id;
1670 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1673 if (tip_frames_used[solver_tip_id])
1677 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1681 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1682 solver_tip_frame = solver_tip_frame.substr(1);
1684 if (pose_frame != solver_tip_frame)
1689 pose_frame = body->getAttachedLinkName();
1690 pose = pose * body->getPose().inverse();
1692 if (pose_frame != solver_tip_frame)
1697 RCLCPP_ERROR(LOGGER,
"The following Pose Frame does not exist: %s", pose_frame.c_str());
1701 for (
const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
1704 pose_frame = solver_tip_frame;
1705 pose = pose * fixed_link.second;
1713 if (pose_frame == solver_tip_frame)
1715 found_valid_frame =
true;
1722 if (!found_valid_frame)
1724 RCLCPP_ERROR(LOGGER,
"Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str());
1726 std::stringstream ss;
1727 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1728 ss << solver_tip_frames[solver_tip_id] <<
", ";
1729 RCLCPP_ERROR(LOGGER,
"Available tip frames: [%s]", ss.str().c_str());
1734 tip_frames_used[solver_tip_id] =
true;
1737 geometry_msgs::msg::Pose ik_query;
1738 ik_query = tf2::toMsg(pose);
1741 ik_queries[solver_tip_id] = ik_query;
1745 for (std::size_t solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1748 if (tip_frames_used[solver_tip_id])
1752 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1756 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1757 solver_tip_frame = solver_tip_frame.substr(1);
1767 geometry_msgs::msg::Pose ik_query;
1768 ik_query = tf2::toMsg(current_pose);
1771 ik_queries[solver_tip_id] = ik_query;
1774 tip_frames_used[solver_tip_id] =
true;
1778 if (timeout < std::numeric_limits<double>::epsilon())
1784 ik_callback_fn = ik_callback_fn = [
this, jmg, constraint](
const geometry_msgs::msg::Pose& pose,
1785 const std::vector<double>& joints,
1786 moveit_msgs::msg::MoveItErrorCodes& error_code) {
1787 ikCallbackFnAdapter(
this, jmg, constraint, pose, joints, error_code);
1793 std::vector<double> initial_values;
1795 std::vector<double> seed(bij.size());
1796 for (std::size_t i = 0; i < bij.size(); ++i)
1797 seed[i] = initial_values[bij[i]];
1800 std::vector<double> ik_sol;
1801 moveit_msgs::msg::MoveItErrorCodes error;
1803 if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, cost_function,
1806 std::vector<double> solution(bij.size());
1807 for (std::size_t i = 0; i < bij.size(); ++i)
1808 solution[bij[i]] = ik_sol[i];
1816 const std::vector<std::string>& tips_in,
1817 const std::vector<std::vector<double> >& consistency_limits,
double timeout,
1824 std::vector<const JointModelGroup*> sub_groups;
1828 if (poses_in.size() != sub_groups.size())
1830 RCLCPP_ERROR(LOGGER,
"Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(),
1835 if (tips_in.size() != sub_groups.size())
1837 RCLCPP_ERROR(LOGGER,
"Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(),
1842 if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
1844 RCLCPP_ERROR(LOGGER,
"Number of consistency limit vectors must be the same as number of sub-groups");
1848 for (std::size_t i = 0; i < consistency_limits.size(); ++i)
1852 RCLCPP_ERROR(LOGGER,
"Number of joints in consistency_limits is %zu but it should be should be %u", i,
1859 std::vector<kinematics::KinematicsBaseConstPtr> solvers;
1860 for (std::size_t i = 0; i < poses_in.size(); ++i)
1862 kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
1865 RCLCPP_ERROR(LOGGER,
"Could not find solver for group '%s'", sub_groups[i]->getName().c_str());
1868 solvers.push_back(solver);
1872 EigenSTL::vector_Isometry3d transformed_poses = poses_in;
1873 std::vector<std::string> pose_frames = tips_in;
1876 for (std::size_t i = 0; i < poses_in.size(); ++i)
1878 ASSERT_ISOMETRY(transformed_poses[i])
1879 Eigen::Isometry3d& pose = transformed_poses[i];
1880 std::string& pose_frame = pose_frames[i];
1887 std::string solver_tip_frame = solvers[i]->getTipFrame();
1891 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1892 solver_tip_frame = solver_tip_frame.substr(1);
1894 if (pose_frame != solver_tip_frame)
1900 pose = pose * body->
getPose().inverse();
1902 if (pose_frame != solver_tip_frame)
1909 for (
const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
1910 if (fixed_link.first->getName() == solver_tip_frame)
1912 pose_frame = solver_tip_frame;
1913 pose = pose * fixed_link.second;
1919 if (pose_frame != solver_tip_frame)
1921 RCLCPP_ERROR(LOGGER,
"Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(),
1922 solver_tip_frame.c_str());
1928 std::vector<geometry_msgs::msg::Pose> ik_queries(poses_in.size());
1931 ik_callback_fn = [
this, jmg, constraint](
const geometry_msgs::msg::Pose pose,
const std::vector<double>& joints,
1932 moveit_msgs::msg::MoveItErrorCodes& error_code) {
1933 ikCallbackFnAdapter(
this, jmg, constraint, pose, joints, error_code);
1936 for (std::size_t i = 0; i < transformed_poses.size(); ++i)
1938 Eigen::Quaterniond quat(transformed_poses[i].linear());
1940 ik_queries[i].position.x = point.x();
1941 ik_queries[i].position.y = point.y();
1942 ik_queries[i].position.z = point.z();
1943 ik_queries[i].orientation.x = quat.x();
1944 ik_queries[i].orientation.y = quat.y();
1945 ik_queries[i].orientation.z = quat.z();
1946 ik_queries[i].orientation.w = quat.w();
1950 if (timeout < std::numeric_limits<double>::epsilon())
1953 auto start = std::chrono::system_clock::now();
1956 bool first_seed =
true;
1957 unsigned int attempts = 0;
1961 RCLCPP_DEBUG(LOGGER,
"IK attempt: %d", attempts);
1962 bool found_solution =
true;
1963 for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
1965 const std::vector<size_t>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
1966 std::vector<double> seed(bij.size());
1970 std::vector<double> initial_values;
1972 for (std::size_t i = 0; i < bij.size(); ++i)
1973 seed[i] = initial_values[bij[i]];
1979 std::vector<double> random_values;
1980 sub_groups[sg]->getVariableRandomPositions(rng, random_values);
1981 for (std::size_t i = 0; i < bij.size(); ++i)
1982 seed[i] = random_values[bij[i]];
1986 std::vector<double> ik_sol;
1987 moveit_msgs::msg::MoveItErrorCodes error;
1988 const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
1989 if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, (timeout - elapsed) / sub_groups.size(), climits, ik_sol,
1992 std::vector<double> solution(bij.size());
1993 for (std::size_t i = 0; i < bij.size(); ++i)
1994 solution[bij[i]] = ik_sol[i];
1999 found_solution =
false;
2005 std::vector<double> full_solution;
2007 if (constraint ? constraint(
this, jmg, &full_solution[0]) :
true)
2009 RCLCPP_DEBUG(LOGGER,
"Found IK solution");
2013 elapsed = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start).count();
2015 }
while (elapsed < timeout);
2021 bool global_reference_frame,
double distance,
double max_step,
2031 const LinkModel* link,
const Eigen::Isometry3d& target,
2032 bool global_reference_frame,
double max_step,
double jump_threshold_factor,
2043 bool global_reference_frame,
double max_step,
double jump_threshold_factor,
2054 assert(checkLinkTransforms());
2057 std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
2062 transform.translate(link->getCenteredBoundingBoxOffset());
2065 for (
const auto& it : attached_body_map_)
2067 const EigenSTL::vector_Isometry3d& transforms = it.second->getGlobalCollisionBodyTransforms();
2068 const std::vector<shapes::ShapeConstPtr>&
shapes = it.second->getShapes();
2069 for (std::size_t i = 0; i < transforms.size(); ++i)
2077 aabb.resize(6, 0.0);
2078 if (!bounding_box.isEmpty())
2082 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data(), 3) = bounding_box.min();
2083 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data() + 1, 3) = bounding_box.max();
2089 const std::vector<std::string>& nm = robot_model_->getVariableNames();
2090 for (std::size_t i = 0; i < nm.size(); ++i)
2091 out << nm[i] <<
"=" << position_[i] <<
'\n';
2105 if (joint->getVariableCount() > 1)
2118 out <<
" " << std::fixed << std::setprecision(5) << bound.
min_position_ <<
"\t";
2120 double step = delta / 20.0;
2122 bool marker_shown =
false;
2126 if (!marker_shown && current_value < value)
2129 marker_shown =
true;
2138 out <<
" \t" << std::fixed << std::setprecision(5) << bound.
max_position_ <<
" \t" << joint->getName()
2139 <<
" current: " << std::fixed << std::setprecision(5) << current_value <<
'\n';
2148 out <<
" * Dirty Joint Transforms: \n";
2149 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2152 out <<
" " << joint->getName() <<
'\n';
2153 out <<
" * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->
getName() :
"NULL") <<
'\n';
2154 out <<
" * Dirty Collision Body Transforms: "
2155 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->
getName() :
"NULL\n");
2160 out <<
"Robot State @" <<
this <<
'\n';
2162 std::size_t n = robot_model_->getVariableCount();
2165 out <<
" * Position: ";
2166 for (std::size_t i = 0; i < n; ++i)
2167 out << position_[i] <<
" ";
2171 out <<
" * Position: NULL\n";
2175 out <<
" * Velocity: ";
2176 for (std::size_t i = 0; i < n; ++i)
2177 out << velocity_[i] <<
" ";
2181 out <<
" * Velocity: NULL\n";
2185 out <<
" * Acceleration: ";
2186 for (std::size_t i = 0; i < n; ++i)
2187 out << acceleration_[i] <<
" ";
2191 out <<
" * Acceleration: NULL\n";
2193 out <<
" * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->
getName() :
"NULL\n");
2194 out <<
" * Dirty Collision Body Transforms: "
2195 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->
getName() :
"NULL\n");
2202 if (checkIsometry(transform, CHECK_ISOMETRY_PRECISION,
false))
2204 Eigen::Quaterniond q(transform.linear());
2205 out <<
"T.xyz = [" << transform.translation().x() <<
", " << transform.translation().y() <<
", "
2206 << transform.translation().z() <<
"], Q.xyzw = [" << q.x() <<
", " << q.y() <<
", " << q.z() <<
", " << q.w()
2211 out <<
"[NON-ISOMETRY] "
2212 << transform.matrix().format(
2213 Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols,
", ",
"; ",
"",
"",
"[",
"]"));
2220 if (!variable_joint_transforms_)
2222 out <<
"No transforms computed\n";
2226 out <<
"Joint transforms:\n";
2227 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2230 out <<
" " << joint->getName();
2231 const int idx = joint->getJointIndex();
2232 if (dirty_joint_transforms_[idx])
2238 out <<
"Link poses:\n";
2239 const std::vector<const LinkModel*>& link_model = robot_model_->getLinkModels();
2240 for (
const LinkModel* link : link_model)
2242 out <<
" " << link->getName() <<
": ";
2243 printTransform(global_link_transforms_[link->getLinkIndex()], out);
2249 std::stringstream ss;
2250 ss <<
"ROBOT: " << robot_model_->getName() <<
'\n';
2251 getStateTreeJointString(ss, robot_model_->getRootJoint(),
" ",
true);
2257 void getPoseString(std::ostream& ss,
const Eigen::Isometry3d& pose,
const std::string& pfx)
2260 for (
int y = 0;
y < 4; ++
y)
2263 for (
int x = 0;
x < 4; ++
x)
2265 ss << std::setw(8) << pose(
y,
x) <<
" ";
2272 void RobotState::getStateTreeJointString(std::ostream& ss,
const JointModel* jm,
const std::string& pfx0,
2275 std::string pfx = pfx0 +
"+--";
2277 ss << pfx <<
"Joint: " << jm->getName() <<
'\n';
2279 pfx = pfx0 + (last ?
" " :
"| ");
2281 for (std::size_t i = 0; i < jm->getVariableCount(); ++i)
2284 ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] <<
'\n';
2287 const LinkModel* link_model = jm->getChildLinkModel();
2289 ss << pfx <<
"Link: " << link_model->
getName() <<
'\n';
2290 getPoseString(ss, link_model->getJointOriginTransform(), pfx +
"joint_origin:");
2291 if (variable_joint_transforms_)
2293 getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx +
"joint_variable:");
2294 getPoseString(ss, global_link_transforms_[link_model->getLinkIndex()], pfx +
"link_global:");
2297 for (std::vector<const JointModel*>::const_iterator it = link_model->getChildJointModels().begin();
2298 it != link_model->getChildJointModels().end(); ++it)
2299 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 LinkModel * getAttachedLink() const
Get the model 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.