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()]);
802 const LinkModel* RobotState::getLinkModelIncludingAttachedBodies(
const std::string& frame)
const
811 if (
const auto it = attached_body_map_.find(frame); it != attached_body_map_.end())
813 const auto& body{ it->second };
814 return body->getAttachedLink();
818 for (
const auto& it : attached_body_map_)
820 const auto& body{ it.second };
821 if (body->hasSubframeTransform(frame))
823 return body->getAttachedLink();
834 const LinkModel* link = getLinkModelIncludingAttachedBodies(frame);
836 return getRobotModel()->getRigidlyConnectedParentLinkModel(link, jmg);
841 const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
850 const std::vector<const JointModel*>& jm =
group->getActiveJointModels();
859 const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
873 for (
const JointModel* jm : robot_model_->getActiveJointModels())
893 std::pair<double, const JointModel*>
896 double distance = std::numeric_limits<double>::max();
908 std::vector<double> lower_bounds(bounds.size()), upper_bounds(bounds.size());
909 for (std::size_t j = 0; j < bounds.size(); ++j)
911 lower_bounds[j] = bounds[j].min_position_;
912 upper_bounds[j] = bounds[j].max_position_;
914 double new_distance = joint->distance(joint_values, &lower_bounds[0]);
920 new_distance = joint->distance(joint_values, &upper_bounds[0]);
927 return std::make_pair(
distance, index);
932 const std::vector<const JointModel*>& jm =
group->getActiveJointModels();
935 const int idx = joint_id->getFirstVariableIndex();
936 const std::vector<VariableBounds>& bounds = joint_id->getVariableBounds();
939 for (std::size_t var_id = 0; var_id < joint_id->getVariableCount(); ++var_id)
941 const double dtheta = std::abs(*(position_ + idx + var_id) - *(other.
getVariablePositions() + idx + var_id));
943 if (dtheta > dt * bounds[var_id].max_velocity_)
956 const int idx = joint->getFirstVariableIndex();
957 d += joint->getDistanceFactor() * joint->distance(position_ + idx, other.position_ + idx);
964 moveit::core::checkInterpolationParamBounds(LOGGER, t);
967 memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() *
sizeof(
unsigned char));
968 state.dirty_link_transforms_ = state.robot_model_->getRootJoint();
973 moveit::core::checkInterpolationParamBounds(LOGGER, t);
977 const int idx = joint->getFirstVariableIndex();
978 joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
980 state.updateMimicJoints(joint_group);
985 attached_body_update_callback_ = callback;
990 return attached_body_map_.find(
id) != attached_body_map_.end();
995 const auto it = attached_body_map_.find(
id);
996 if (it == attached_body_map_.end())
998 RCLCPP_ERROR(LOGGER,
"Attached body '%s' not found",
id.c_str());
1002 return it->second.get();
1011 if (attached_body_update_callback_)
1012 attached_body_update_callback_(attached_body.get(),
true);
1013 attached_body_map_[attached_body->getName()] = std::move(attached_body);
1018 attachBody(std::unique_ptr<AttachedBody>(attached_body));
1022 const std::vector<shapes::ShapeConstPtr>&
shapes,
1023 const EigenSTL::vector_Isometry3d& shape_poses,
const std::set<std::string>& touch_links,
1024 const std::string& link,
const trajectory_msgs::msg::JointTrajectory& detach_posture,
1027 attachBody(std::make_unique<AttachedBody>(robot_model_->getLinkModel(link),
id, pose,
shapes, shape_poses,
1028 touch_links, detach_posture, subframe_poses));
1033 attached_bodies.clear();
1034 attached_bodies.reserve(attached_body_map_.size());
1035 for (
const auto& it : attached_body_map_)
1036 attached_bodies.push_back(it.second.get());
1041 attached_bodies.clear();
1042 for (
const auto& it : attached_body_map_)
1043 if (
group->hasLinkModel(it.second->getAttachedLinkName()))
1044 attached_bodies.push_back(it.second.get());
1049 attached_bodies.clear();
1050 for (
const auto& it : attached_body_map_)
1051 if (it.second->getAttachedLink() == link_model)
1052 attached_bodies.push_back(it.second.get());
1057 for (
const auto& it : attached_body_map_)
1059 if (attached_body_update_callback_)
1060 attached_body_update_callback_(it.second.get(),
false);
1062 attached_body_map_.clear();
1067 for (
auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1069 if (it->second->getAttachedLink() != link)
1073 if (attached_body_update_callback_)
1074 attached_body_update_callback_(it->second.get(),
false);
1075 const auto del = it++;
1076 attached_body_map_.erase(del);
1082 for (
auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1084 if (!
group->hasLinkModel(it->second->getAttachedLinkName()))
1088 if (attached_body_update_callback_)
1089 attached_body_update_callback_(it->second.get(),
false);
1090 const auto del = it++;
1091 attached_body_map_.erase(del);
1097 const auto it = attached_body_map_.find(
id);
1098 if (it != attached_body_map_.end())
1100 if (attached_body_update_callback_)
1101 attached_body_update_callback_(it->second.get(),
false);
1102 attached_body_map_.erase(it);
1122 *frame_found = found;
1124 RCLCPP_WARN(LOGGER,
"getFrameTransform() did not find a frame with name %s.",
frame_id.c_str());
1130 bool& frame_found)
const
1135 static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
1136 if (
frame_id == robot_model_->getModelFrame())
1138 robot_link = robot_model_->getRootLink();
1140 return IDENTITY_TRANSFORM;
1142 if ((robot_link = robot_model_->getLinkModel(
frame_id, &frame_found)))
1144 assert(checkLinkTransforms());
1145 return global_link_transforms_[robot_link->
getLinkIndex()];
1147 robot_link =
nullptr;
1150 const auto jt = attached_body_map_.find(
frame_id);
1151 if (jt != attached_body_map_.end())
1153 const Eigen::Isometry3d& transform = jt->second->getGlobalPose();
1154 robot_link = jt->second->getAttachedLink();
1156 assert(checkLinkTransforms());
1161 for (
const auto& body : attached_body_map_)
1163 const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(
frame_id, &frame_found);
1166 robot_link = body.second->getAttachedLink();
1167 assert(checkLinkTransforms());
1172 robot_link =
nullptr;
1173 frame_found =
false;
1174 return IDENTITY_TRANSFORM;
1181 if (robot_model_->hasLinkModel(
frame_id))
1185 const auto it = attached_body_map_.find(
frame_id);
1186 if (it != attached_body_map_.end())
1187 return !it->second->getGlobalCollisionBodyTransforms().empty();
1190 for (
const auto& body : attached_body_map_)
1192 if (body.second->hasSubframeTransform(
frame_id))
1199 const std_msgs::msg::ColorRGBA& color,
const std::string& ns,
1200 const rclcpp::Duration& dur,
bool include_attached)
const
1202 std::size_t cur_num = arr.markers.size();
1204 unsigned int id = cur_num;
1205 for (std::size_t i = cur_num; i < arr.markers.size(); ++i, ++id)
1207 arr.markers[i].ns = ns;
1208 arr.markers[i].id = id;
1209 arr.markers[i].lifetime = dur;
1210 arr.markers[i].color = color;
1215 bool include_attached)
const
1217 rclcpp::Clock clock;
1218 for (
const std::string& link_name : link_names)
1220 RCLCPP_DEBUG(LOGGER,
"Trying to get marker for link '%s'", link_name.c_str());
1221 const LinkModel* link_model = robot_model_->getLinkModel(link_name);
1224 if (include_attached)
1225 for (
const auto& it : attached_body_map_)
1226 if (it.second->getAttachedLink() == link_model)
1228 for (std::size_t j = 0; j < it.second->getShapes().size(); ++j)
1230 visualization_msgs::msg::Marker att_mark;
1231 att_mark.header.frame_id = robot_model_->getModelFrame();
1232 att_mark.header.stamp = clock.now();
1233 if (shapes::constructMarkerFromShape(it.second->getShapes()[j].get(), att_mark))
1236 if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<float>::epsilon())
1238 att_mark.pose = tf2::toMsg(it.second->getGlobalCollisionBodyTransforms()[j]);
1239 arr.markers.push_back(att_mark);
1247 for (std::size_t j = 0; j < link_model->
getShapes().size(); ++j)
1249 visualization_msgs::msg::Marker mark;
1250 mark.header.frame_id = robot_model_->getModelFrame();
1251 mark.header.stamp = clock.now();
1255 if (mesh_resource.empty() || link_model->
getShapes().size() > 1)
1257 if (!shapes::constructMarkerFromShape(link_model->
getShapes()[j].get(), mark))
1260 if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<float>::epsilon())
1267 mark.type = mark.MESH_RESOURCE;
1268 mark.mesh_use_embedded_materials =
false;
1269 mark.mesh_resource = mesh_resource;
1272 mark.scale.x = mesh_scale[0];
1273 mark.scale.y = mesh_scale[1];
1274 mark.scale.z = mesh_scale[2];
1278 arr.markers.push_back(mark);
1286 Eigen::MatrixXd result;
1288 throw Exception(
"Unable to compute Jacobian");
1293 const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
1294 bool use_quaternion_representation)
const
1296 assert(checkLinkTransforms());
1298 if (!
group->isChain())
1300 RCLCPP_ERROR(LOGGER,
"The group '%s' is not a chain. Cannot compute Jacobian.",
group->getName().c_str());
1306 RCLCPP_ERROR(LOGGER,
"Link name '%s' does not exist in the chain '%s' or is not a child for this chain",
1314 Eigen::Isometry3d reference_transform =
1316 int rows = use_quaternion_representation ? 7 : 6;
1317 int columns =
group->getVariableCount();
1318 jacobian = Eigen::MatrixXd::Zero(rows, columns);
1322 Eigen::Vector3d point_transform = link_transform * reference_point_position;
1330 Eigen::Isometry3d joint_transform;
1348 unsigned int joint_index =
group->getVariableGroupIndex(pjm->
getName());
1354 jacobian.block<3, 1>(0, joint_index) =
1355 jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation());
1356 jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis;
1361 jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1366 jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1368 jacobian.block<3, 1>(0, joint_index + 1) = jacobian.block<3, 1>(0, joint_index + 1) + joint_axis;
1370 jacobian.block<3, 1>(0, joint_index + 2) = jacobian.block<3, 1>(0, joint_index + 2) +
1371 joint_axis.cross(point_transform - joint_transform.translation());
1372 jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis;
1375 RCLCPP_ERROR(LOGGER,
"Unknown type of joint in Jacobian computation");
1377 if (pjm == root_joint_model)
1381 if (use_quaternion_representation)
1388 Eigen::Quaterniond q(link_transform.linear());
1389 double w = q.w(),
x = q.x(),
y = q.y(),
z = q.z();
1390 Eigen::MatrixXd quaternion_update_matrix(4, 3);
1391 quaternion_update_matrix << -
x, -
y, -
z,
w, -
z,
y,
z,
w, -
x, -
y,
x,
w;
1392 jacobian.block(3, 0, 4, columns) = 0.5 * quaternion_update_matrix * jacobian.block(3, 0, 3, columns);
1400 Eigen::VectorXd qdot;
1408 Eigen::Matrix<double, 6, 1> t;
1409 tf2::fromMsg(twist, t);
1414 const Eigen::VectorXd& twist,
const LinkModel* tip)
const
1423 Eigen::MatrixXd e_wb = Eigen::ArrayXXd::Zero(6, 6);
1424 e_wb.block(0, 0, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1425 e_wb.block(3, 3, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1429 Eigen::JacobiSVD<Eigen::MatrixXd> svd_of_j(j, Eigen::ComputeThinU | Eigen::ComputeThinV);
1430 const Eigen::MatrixXd& u = svd_of_j.matrixU();
1431 const Eigen::MatrixXd& v = svd_of_j.matrixV();
1432 const Eigen::VectorXd& s = svd_of_j.singularValues();
1434 Eigen::VectorXd sinv = s;
1435 static const double PINVTOLER = std::numeric_limits<float>::epsilon();
1437 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1438 if (fabs(s(i)) > maxsv)
1440 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1443 if (fabs(s(i)) > maxsv * PINVTOLER)
1444 sinv(i) = 1.0 / s(i);
1448 Eigen::MatrixXd jinv = (v * sinv.asDiagonal() * u.transpose());
1451 qdot = jinv * twist;
1465 std::vector<double> values;
1467 return constraint(
this, jmg, &values[0]);
1481 RCLCPP_ERROR(LOGGER,
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1484 return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint,
options, cost_function);
1492 Eigen::Isometry3d mat;
1493 tf2::fromMsg(pose, mat);
1494 static std::vector<double> consistency_limits;
1495 return setFromIK(jmg, mat, tip, consistency_limits, timeout, constraint,
options, cost_function);
1506 RCLCPP_ERROR(LOGGER,
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1509 static std::vector<double> consistency_limits;
1510 return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, timeout, constraint,
options, cost_function);
1518 static std::vector<double> consistency_limits;
1519 return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint,
options, cost_function);
1526 const std::vector<double>& ik_sol, moveit_msgs::msg::MoveItErrorCodes& error_code)
1528 const std::vector<size_t>& bij =
group->getKinematicsSolverJointBijection();
1529 std::vector<double> solution(bij.size());
1530 for (std::size_t i = 0; i < bij.size(); ++i)
1531 solution[bij[i]] = ik_sol[i];
1532 if (constraint(state,
group, &solution[0]))
1533 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
1535 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
1551 getLinkModel((!ik_frame.empty() && ik_frame[0] ==
'/') ? ik_frame.substr(1) : ik_frame);
1554 RCLCPP_ERROR(LOGGER,
"The following IK frame does not exist: %s", ik_frame.c_str());
1563 const std::vector<double>& consistency_limits_in,
double timeout,
1569 EigenSTL::vector_Isometry3d poses;
1570 poses.push_back(pose_in);
1572 std::vector<std::string> tips;
1573 tips.push_back(tip_in);
1575 std::vector<std::vector<double> > consistency_limits;
1576 consistency_limits.push_back(consistency_limits_in);
1578 return setFromIK(jmg, poses, tips, consistency_limits, timeout, constraint,
options, cost_function);
1582 const std::vector<std::string>& tips_in,
double timeout,
1587 const std::vector<std::vector<double> > consistency_limits;
1588 return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint,
options, cost_function);
1592 const std::vector<std::string>& tips_in,
1593 const std::vector<std::vector<double> >& consistency_limit_sets,
double timeout,
1599 if (poses_in.size() != tips_in.size())
1601 RCLCPP_ERROR(LOGGER,
"Number of poses must be the same as number of tips");
1609 bool valid_solver =
true;
1612 valid_solver =
false;
1615 else if (poses_in.size() > 1)
1617 std::string error_msg;
1618 if (!solver->supportsGroup(jmg, &error_msg))
1621 RCLCPP_ERROR(LOGGER,
"Kinematics solver %s does not support joint group %s. Error: %s",
1622 typeid(solver_ref).
name(), jmg->
getName().c_str(), error_msg.c_str());
1623 valid_solver =
false;
1630 if (poses_in.size() > 1)
1637 RCLCPP_ERROR(LOGGER,
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1643 std::vector<double> consistency_limits;
1644 if (consistency_limit_sets.size() > 1)
1646 RCLCPP_ERROR(LOGGER,
1647 "Invalid number (%zu) of sets of consistency limits for a setFromIK request "
1648 "that is being solved by a single IK solver",
1649 consistency_limit_sets.size());
1652 else if (consistency_limit_sets.size() == 1)
1653 consistency_limits = consistency_limit_sets[0];
1655 const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
1658 std::vector<bool> tip_frames_used(solver_tip_frames.size(),
false);
1661 std::vector<geometry_msgs::msg::Pose> ik_queries(solver_tip_frames.size());
1664 for (std::size_t i = 0; i < poses_in.size(); ++i)
1667 Eigen::Isometry3d pose = poses_in[i];
1668 std::string pose_frame = tips_in[i];
1671 if (!pose_frame.empty() && pose_frame[0] ==
'/')
1672 pose_frame = pose_frame.substr(1);
1680 bool found_valid_frame =
false;
1681 std::size_t solver_tip_id;
1682 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1685 if (tip_frames_used[solver_tip_id])
1689 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1693 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1694 solver_tip_frame = solver_tip_frame.substr(1);
1696 if (pose_frame != solver_tip_frame)
1701 RCLCPP_ERROR_STREAM(LOGGER,
"The following Pose Frame does not exist: " << pose_frame);
1708 RCLCPP_ERROR_STREAM(LOGGER,
"The following Solver Tip Frame does not exist: " << solver_tip_frame);
1712 if (pose_parent == tip_parent)
1715 pose = pose * pose_parent_to_frame.inverse() * tip_parent_to_tip;
1716 found_valid_frame =
true;
1722 found_valid_frame =
true;
1728 if (!found_valid_frame)
1730 RCLCPP_ERROR(LOGGER,
"Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str());
1732 std::stringstream ss;
1733 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1734 ss << solver_tip_frames[solver_tip_id] <<
", ";
1735 RCLCPP_ERROR(LOGGER,
"Available tip frames: [%s]", ss.str().c_str());
1740 tip_frames_used[solver_tip_id] =
true;
1743 geometry_msgs::msg::Pose ik_query;
1744 ik_query = tf2::toMsg(pose);
1747 ik_queries[solver_tip_id] = ik_query;
1751 for (std::size_t solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1754 if (tip_frames_used[solver_tip_id])
1758 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1762 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1763 solver_tip_frame = solver_tip_frame.substr(1);
1773 geometry_msgs::msg::Pose ik_query;
1774 ik_query = tf2::toMsg(current_pose);
1777 ik_queries[solver_tip_id] = ik_query;
1780 tip_frames_used[solver_tip_id] =
true;
1784 if (timeout < std::numeric_limits<double>::epsilon())
1790 ik_callback_fn = ik_callback_fn = [
this, jmg, constraint](
const geometry_msgs::msg::Pose& pose,
1791 const std::vector<double>& joints,
1792 moveit_msgs::msg::MoveItErrorCodes& error_code) {
1793 ikCallbackFnAdapter(
this, jmg, constraint, pose, joints, error_code);
1799 std::vector<double> initial_values;
1801 std::vector<double> seed(bij.size());
1802 for (std::size_t i = 0; i < bij.size(); ++i)
1803 seed[i] = initial_values[bij[i]];
1806 std::vector<double> ik_sol;
1807 moveit_msgs::msg::MoveItErrorCodes error;
1809 if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, cost_function,
1812 std::vector<double> solution(bij.size());
1813 for (std::size_t i = 0; i < bij.size(); ++i)
1814 solution[bij[i]] = ik_sol[i];
1822 const std::vector<std::string>& tips_in,
1823 const std::vector<std::vector<double> >& consistency_limits,
double timeout,
1830 std::vector<const JointModelGroup*> sub_groups;
1834 if (poses_in.size() != sub_groups.size())
1836 RCLCPP_ERROR(LOGGER,
"Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(),
1841 if (tips_in.size() != sub_groups.size())
1843 RCLCPP_ERROR(LOGGER,
"Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(),
1848 if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
1850 RCLCPP_ERROR(LOGGER,
"Number of consistency limit vectors must be the same as number of sub-groups");
1854 for (std::size_t i = 0; i < consistency_limits.size(); ++i)
1858 RCLCPP_ERROR(LOGGER,
"Number of joints in consistency_limits[%zu] is %lu but it should be should be %u", i,
1865 std::vector<kinematics::KinematicsBaseConstPtr> solvers;
1866 for (std::size_t i = 0; i < poses_in.size(); ++i)
1868 kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
1871 RCLCPP_ERROR(LOGGER,
"Could not find solver for group '%s'", sub_groups[i]->getName().c_str());
1874 solvers.push_back(solver);
1878 EigenSTL::vector_Isometry3d transformed_poses = poses_in;
1879 std::vector<std::string> pose_frames = tips_in;
1882 for (std::size_t i = 0; i < poses_in.size(); ++i)
1884 ASSERT_ISOMETRY(transformed_poses[i])
1885 Eigen::Isometry3d& pose = transformed_poses[i];
1886 std::string& pose_frame = pose_frames[i];
1893 std::string solver_tip_frame = solvers[i]->getTipFrame();
1897 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1898 solver_tip_frame = solver_tip_frame.substr(1);
1900 if (pose_frame != solver_tip_frame)
1906 pose = pose * body->
getPose().inverse();
1908 if (pose_frame != solver_tip_frame)
1915 for (
const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
1916 if (fixed_link.first->getName() == solver_tip_frame)
1918 pose_frame = solver_tip_frame;
1919 pose = pose * fixed_link.second;
1925 if (pose_frame != solver_tip_frame)
1927 RCLCPP_ERROR(LOGGER,
"Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(),
1928 solver_tip_frame.c_str());
1934 std::vector<geometry_msgs::msg::Pose> ik_queries(poses_in.size());
1937 ik_callback_fn = [
this, jmg, constraint](
const geometry_msgs::msg::Pose pose,
const std::vector<double>& joints,
1938 moveit_msgs::msg::MoveItErrorCodes& error_code) {
1939 ikCallbackFnAdapter(
this, jmg, constraint, pose, joints, error_code);
1942 for (std::size_t i = 0; i < transformed_poses.size(); ++i)
1944 Eigen::Quaterniond quat(transformed_poses[i].linear());
1946 ik_queries[i].position.x = point.x();
1947 ik_queries[i].position.y = point.y();
1948 ik_queries[i].position.z = point.z();
1949 ik_queries[i].orientation.x = quat.x();
1950 ik_queries[i].orientation.y = quat.y();
1951 ik_queries[i].orientation.z = quat.z();
1952 ik_queries[i].orientation.w = quat.w();
1956 if (timeout < std::numeric_limits<double>::epsilon())
1959 auto start = std::chrono::system_clock::now();
1962 bool first_seed =
true;
1963 unsigned int attempts = 0;
1967 RCLCPP_DEBUG(LOGGER,
"IK attempt: %d", attempts);
1968 bool found_solution =
true;
1969 for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
1971 const std::vector<size_t>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
1972 std::vector<double> seed(bij.size());
1976 std::vector<double> initial_values;
1978 for (std::size_t i = 0; i < bij.size(); ++i)
1979 seed[i] = initial_values[bij[i]];
1985 std::vector<double> random_values;
1986 sub_groups[sg]->getVariableRandomPositions(rng, random_values);
1987 for (std::size_t i = 0; i < bij.size(); ++i)
1988 seed[i] = random_values[bij[i]];
1992 std::vector<double> ik_sol;
1993 moveit_msgs::msg::MoveItErrorCodes error;
1994 const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
1995 if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, (timeout - elapsed) / sub_groups.size(), climits, ik_sol,
1998 std::vector<double> solution(bij.size());
1999 for (std::size_t i = 0; i < bij.size(); ++i)
2000 solution[bij[i]] = ik_sol[i];
2005 found_solution =
false;
2011 std::vector<double> full_solution;
2013 if (constraint ? constraint(
this, jmg, &full_solution[0]) :
true)
2015 RCLCPP_DEBUG(LOGGER,
"Found IK solution");
2019 elapsed = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start).count();
2021 }
while (elapsed < timeout);
2027 bool global_reference_frame,
double distance,
double max_step,
2037 const LinkModel* link,
const Eigen::Isometry3d& target,
2038 bool global_reference_frame,
double max_step,
double jump_threshold_factor,
2049 bool global_reference_frame,
double max_step,
double jump_threshold_factor,
2060 assert(checkLinkTransforms());
2063 std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
2068 transform.translate(link->getCenteredBoundingBoxOffset());
2071 for (
const auto& it : attached_body_map_)
2073 const EigenSTL::vector_Isometry3d& transforms = it.second->getGlobalCollisionBodyTransforms();
2074 const std::vector<shapes::ShapeConstPtr>&
shapes = it.second->getShapes();
2075 for (std::size_t i = 0; i < transforms.size(); ++i)
2083 aabb.resize(6, 0.0);
2084 if (!bounding_box.isEmpty())
2088 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data(), 3) = bounding_box.min();
2089 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data() + 1, 3) = bounding_box.max();
2095 const std::vector<std::string>& nm = robot_model_->getVariableNames();
2096 for (std::size_t i = 0; i < nm.size(); ++i)
2097 out << nm[i] <<
"=" << position_[i] <<
'\n';
2111 if (joint->getVariableCount() > 1)
2124 out <<
" " << std::fixed << std::setprecision(5) << bound.
min_position_ <<
"\t";
2126 double step = delta / 20.0;
2128 bool marker_shown =
false;
2132 if (!marker_shown && current_value < value)
2135 marker_shown =
true;
2144 out <<
" \t" << std::fixed << std::setprecision(5) << bound.
max_position_ <<
" \t" << joint->getName()
2145 <<
" current: " << std::fixed << std::setprecision(5) << current_value <<
'\n';
2154 out <<
" * Dirty Joint Transforms: \n";
2155 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2158 out <<
" " << joint->getName() <<
'\n';
2159 out <<
" * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->
getName() :
"NULL") <<
'\n';
2160 out <<
" * Dirty Collision Body Transforms: "
2161 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->
getName() :
"NULL\n");
2166 out <<
"Robot State @" <<
this <<
'\n';
2168 std::size_t n = robot_model_->getVariableCount();
2171 out <<
" * Position: ";
2172 for (std::size_t i = 0; i < n; ++i)
2173 out << position_[i] <<
" ";
2177 out <<
" * Position: NULL\n";
2181 out <<
" * Velocity: ";
2182 for (std::size_t i = 0; i < n; ++i)
2183 out << velocity_[i] <<
" ";
2187 out <<
" * Velocity: NULL\n";
2191 out <<
" * Acceleration: ";
2192 for (std::size_t i = 0; i < n; ++i)
2193 out << acceleration_[i] <<
" ";
2197 out <<
" * Acceleration: NULL\n";
2199 out <<
" * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->
getName() :
"NULL\n");
2200 out <<
" * Dirty Collision Body Transforms: "
2201 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->
getName() :
"NULL\n");
2208 if (checkIsometry(transform, CHECK_ISOMETRY_PRECISION,
false))
2210 Eigen::Quaterniond q(transform.linear());
2211 out <<
"T.xyz = [" << transform.translation().x() <<
", " << transform.translation().y() <<
", "
2212 << transform.translation().z() <<
"], Q.xyzw = [" << q.x() <<
", " << q.y() <<
", " << q.z() <<
", " << q.w()
2217 out <<
"[NON-ISOMETRY] "
2218 << transform.matrix().format(
2219 Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols,
", ",
"; ",
"",
"",
"[",
"]"));
2226 if (!variable_joint_transforms_)
2228 out <<
"No transforms computed\n";
2232 out <<
"Joint transforms:\n";
2233 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2236 out <<
" " << joint->getName();
2237 const int idx = joint->getJointIndex();
2238 if (dirty_joint_transforms_[idx])
2244 out <<
"Link poses:\n";
2245 const std::vector<const LinkModel*>& link_model = robot_model_->getLinkModels();
2246 for (
const LinkModel* link : link_model)
2248 out <<
" " << link->getName() <<
": ";
2249 printTransform(global_link_transforms_[link->getLinkIndex()], out);
2255 std::stringstream ss;
2256 ss <<
"ROBOT: " << robot_model_->getName() <<
'\n';
2257 getStateTreeJointString(ss, robot_model_->getRootJoint(),
" ",
true);
2263 void getPoseString(std::ostream& ss,
const Eigen::Isometry3d& pose,
const std::string& pfx)
2266 for (
int y = 0;
y < 4; ++
y)
2269 for (
int x = 0;
x < 4; ++
x)
2271 ss << std::setw(8) << pose(
y,
x) <<
" ";
2278 void RobotState::getStateTreeJointString(std::ostream& ss,
const JointModel* jm,
const std::string& pfx0,
2281 std::string pfx = pfx0 +
"+--";
2283 ss << pfx <<
"Joint: " << jm->getName() <<
'\n';
2285 pfx = pfx0 + (last ?
" " :
"| ");
2287 for (std::size_t i = 0; i < jm->getVariableCount(); ++i)
2290 ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] <<
'\n';
2293 const LinkModel* link_model = jm->getChildLinkModel();
2295 ss << pfx <<
"Link: " << link_model->getName() <<
'\n';
2296 getPoseString(ss, link_model->getJointOriginTransform(), pfx +
"joint_origin:");
2297 if (variable_joint_transforms_)
2299 getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx +
"joint_variable:");
2300 getPoseString(ss, global_link_transforms_[link_model->getLinkIndex()], pfx +
"link_global:");
2303 for (std::vector<const JointModel*>::const_iterator it = link_model->getChildJointModels().begin();
2304 it != link_model->getChildJointModels().end(); ++it)
2305 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.
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...
const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const std::string &frame, const moveit::core::JointModelGroup *jmg=nullptr) const
Get the latest link upwards the kinematic tree which is only connected via fixed joints.
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.