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>
67 : robot_model_(robot_model)
68 , has_velocity_(false)
69 , has_acceleration_(false)
71 , dirty_link_transforms_(nullptr)
72 , dirty_collision_body_transforms_(nullptr)
75 if (robot_model ==
nullptr)
77 throw std::invalid_argument(
"RobotState cannot be constructed with nullptr RobotModelConstPtr");
80 dirty_link_transforms_ = robot_model_->getRootJoint();
86 robot_model_ = other.robot_model_;
95 void RobotState::init()
97 variable_joint_transforms_.resize(robot_model_->getJointModelCount(), Eigen::Isometry3d::Identity());
98 global_link_transforms_.resize(robot_model_->getLinkModelCount(), Eigen::Isometry3d::Identity());
99 global_collision_body_transforms_.resize(robot_model_->getLinkGeometryCount(), Eigen::Isometry3d::Identity());
100 dirty_joint_transforms_.resize(robot_model_->getJointModelCount(), 1);
101 position_.resize(robot_model_->getVariableCount());
102 velocity_.resize(robot_model_->getVariableCount());
103 effort_or_acceleration_.resize(robot_model_->getVariableCount());
113 void RobotState::copyFrom(
const RobotState& other)
115 has_velocity_ = other.has_velocity_;
116 has_acceleration_ = other.has_acceleration_;
117 has_effort_ = other.has_effort_;
119 dirty_collision_body_transforms_ = other.dirty_collision_body_transforms_;
120 dirty_link_transforms_ = other.dirty_link_transforms_;
122 variable_joint_transforms_ = other.variable_joint_transforms_;
123 global_link_transforms_ = other.global_link_transforms_;
124 global_collision_body_transforms_ = other.global_collision_body_transforms_;
125 dirty_joint_transforms_ = other.dirty_joint_transforms_;
126 position_ = other.position_;
127 velocity_ = other.velocity_;
128 effort_or_acceleration_ = other.effort_or_acceleration_;
132 for (
const auto& attached_body : other.attached_body_map_)
133 attachBody(std::make_unique<AttachedBody>(*attached_body.second));
136 bool RobotState::checkJointTransforms(
const JointModel* joint)
const
140 RCLCPP_WARN(
getLogger(),
"Returning dirty joint transforms for joint '%s'", joint->
getName().c_str());
146 bool RobotState::checkLinkTransforms()
const
150 RCLCPP_WARN(
getLogger(),
"Returning dirty link transforms");
156 bool RobotState::checkCollisionTransforms()
const
160 RCLCPP_WARN(
getLogger(),
"Returning dirty collision body transforms");
166 void RobotState::markVelocity()
170 has_velocity_ =
true;
171 std::fill(velocity_.begin(), velocity_.end(), 0.0);
175 void RobotState::markAcceleration()
177 if (!has_acceleration_)
179 has_acceleration_ =
true;
181 std::fill(effort_or_acceleration_.begin(), effort_or_acceleration_.end(), 0.0);
185 void RobotState::markEffort()
189 has_acceleration_ =
false;
191 std::fill(effort_or_acceleration_.begin(), effort_or_acceleration_.end(), 0.0);
195 void RobotState::updateMimicJoint(
const JointModel* joint)
197 if (joint->getVariableCount() == 0)
201 double v = position_[joint->getFirstVariableIndex()];
202 for (
const JointModel* jm : joint->getMimicRequests())
204 position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset();
205 markDirtyJointTransforms(jm);
210 void RobotState::updateMimicJoints(
const JointModelGroup* group)
212 for (
const JointModel* jm : group->getMimicJointModels())
214 assert(jm->getVariableCount() != 0);
215 const int fvi = jm->getFirstVariableIndex();
216 position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset();
217 markDirtyJointTransforms(jm);
219 markDirtyJointTransforms(group);
224 has_velocity_ =
false;
230 has_acceleration_ =
false;
242 has_velocity_ =
false;
247 has_acceleration_ =
false;
265 robot_model_->getVariableRandomPositions(rng, position_);
266 std::fill(dirty_joint_transforms_.begin(), dirty_joint_transforms_.end(), 1);
267 dirty_link_transforms_ = robot_model_->getRootJoint();
282 joint->getVariableRandomPositions(rng, &position_[joint->getFirstVariableIndex()]);
283 updateMimicJoints(group);
287 const std::vector<double>& distances)
296 const std::vector<double>& distances,
297 random_numbers::RandomNumberGenerator& rng)
300 assert(distances.size() == joints.size());
301 for (std::size_t i = 0; i < joints.size(); ++i)
303 const int idx = joints[i]->getFirstVariableIndex();
304 joints[i]->getVariableRandomPositionsNearBy(rng, &position_.at(joints[i]->getFirstVariableIndex()),
305 &seed.position_.at(idx), distances[i]);
307 updateMimicJoints(group);
319 random_numbers::RandomNumberGenerator& rng)
324 const int idx = joint->getFirstVariableIndex();
325 joint->getVariableRandomPositionsNearBy(rng, &position_.at(joint->getFirstVariableIndex()), &seed.position_.at(idx),
328 updateMimicJoints(group);
333 std::map<std::string, double> m;
341 robot_model_->getVariableDefaultPositions(position_);
343 std::fill(velocity_.begin(), velocity_.end(), 0);
344 std::fill(effort_or_acceleration_.begin(), effort_or_acceleration_.end(), 0);
345 std::fill(dirty_joint_transforms_.begin(), dirty_joint_transforms_.end(), 1);
346 dirty_link_transforms_ = robot_model_->getRootJoint();
352 memcpy(position_.data(), position, robot_model_->getVariableCount() *
sizeof(
double));
357 std::fill(dirty_joint_transforms_.begin(), dirty_joint_transforms_.end(), 1);
358 dirty_link_transforms_ = robot_model_->getRootJoint();
363 for (
const std::pair<const std::string, double>& it : variable_map)
365 const int index = robot_model_->getVariableIndex(it.first);
366 position_[index] = it.second;
367 const JointModel* jm = robot_model_->getJointOfVariable(index);
368 markDirtyJointTransforms(jm);
369 updateMimicJoint(jm);
373 void RobotState::getMissingKeys(
const std::map<std::string, double>& variable_map,
374 std::vector<std::string>& missing_variables)
const
376 missing_variables.clear();
377 const std::vector<std::string>& nm = robot_model_->getVariableNames();
378 for (
const std::string& variable_name : nm)
380 if (variable_map.find(variable_name) == variable_map.end())
382 if (robot_model_->getJointOfVariable(variable_name)->getMimic() ==
nullptr)
383 missing_variables.push_back(variable_name);
389 std::vector<std::string>& missing_variables)
392 getMissingKeys(variable_map, missing_variables);
396 const std::vector<double>& variable_position)
398 for (std::size_t i = 0; i < variable_names.size(); ++i)
400 const int index = robot_model_->getVariableIndex(variable_names[i]);
401 position_[index] = variable_position[i];
402 const JointModel* jm = robot_model_->getJointOfVariable(index);
403 markDirtyJointTransforms(jm);
404 updateMimicJoint(jm);
411 for (
const std::pair<const std::string, double>& it : variable_map)
412 velocity_[robot_model_->getVariableIndex(it.first)] = it.second;
416 std::vector<std::string>& missing_variables)
419 getMissingKeys(variable_map, missing_variables);
423 const std::vector<double>& variable_velocity)
426 assert(variable_names.size() == variable_velocity.size());
427 for (std::size_t i = 0; i < variable_names.size(); ++i)
428 velocity_[robot_model_->getVariableIndex(variable_names[i])] = variable_velocity[i];
434 for (
const std::pair<const std::string, double>& it : variable_map)
435 effort_or_acceleration_[robot_model_->getVariableIndex(it.first)] = it.second;
439 std::vector<std::string>& missing_variables)
442 getMissingKeys(variable_map, missing_variables);
446 const std::vector<double>& variable_acceleration)
449 assert(variable_names.size() == variable_acceleration.size());
450 for (std::size_t i = 0; i < variable_names.size(); ++i)
451 effort_or_acceleration_[robot_model_->getVariableIndex(variable_names[i])] = variable_acceleration[i];
457 for (
const std::pair<const std::string, double>& it : variable_map)
458 effort_or_acceleration_[robot_model_->getVariableIndex(it.first)] = it.second;
462 std::vector<std::string>& missing_variables)
465 getMissingKeys(variable_map, missing_variables);
469 const std::vector<double>& variable_effort)
472 assert(variable_names.size() == variable_effort.size());
473 for (std::size_t i = 0; i < variable_names.size(); ++i)
474 effort_or_acceleration_[robot_model_->getVariableIndex(variable_names[i])] = variable_effort[i];
481 for (
size_t i = 0; i < robot_model_->getVariableCount(); ++i)
493 markDirtyJointTransforms(joint);
494 updateMimicJoint(joint);
504 markDirtyJointTransforms(joint);
505 updateMimicJoint(joint);
514 has_velocity_ =
true;
556 if (has_acceleration_)
558 RCLCPP_ERROR(
getLogger(),
"Unable to set joint efforts because array is being used for accelerations");
576 memcpy(&position_.at(il[0]), gstate, group->
getVariableCount() *
sizeof(
double));
580 for (std::size_t i = 0; i < il.size(); ++i)
581 position_[il[i]] = gstate[i];
583 updateMimicJoints(group);
589 for (std::size_t i = 0; i < il.size(); ++i)
590 position_[il[i]] = values(i);
591 updateMimicJoints(group);
601 i += jm->getVariableCount();
603 updateMimicJoints(group);
613 i += jm->getVariableCount();
615 updateMimicJoints(group);
623 memcpy(gstate, &position_.at(il[0]), group->
getVariableCount() *
sizeof(
double));
627 for (std::size_t i = 0; i < il.size(); ++i)
628 gstate[i] = position_[il[i]];
635 values.resize(il.size());
636 for (std::size_t i = 0; i < il.size(); ++i)
637 values(i) = position_[il[i]];
646 memcpy(&velocity_.at(il[0]), gstate, group->
getVariableCount() *
sizeof(
double));
650 for (std::size_t i = 0; i < il.size(); ++i)
651 velocity_[il[i]] = gstate[i];
659 for (std::size_t i = 0; i < il.size(); ++i)
660 velocity_[il[i]] = values(i);
668 memcpy(gstate, &velocity_.at(il[0]), group->
getVariableCount() *
sizeof(
double));
672 for (std::size_t i = 0; i < il.size(); ++i)
673 gstate[i] = velocity_[il[i]];
680 values.resize(il.size());
681 for (std::size_t i = 0; i < il.size(); ++i)
682 values(i) = velocity_[il[i]];
691 memcpy(&effort_or_acceleration_.at(il[0]), gstate, group->
getVariableCount() *
sizeof(
double));
695 for (std::size_t i = 0; i < il.size(); ++i)
696 effort_or_acceleration_[il[i]] = gstate[i];
704 for (std::size_t i = 0; i < il.size(); ++i)
705 effort_or_acceleration_[il[i]] = values(i);
713 memcpy(gstate, &effort_or_acceleration_.at(il[0]), group->
getVariableCount() *
sizeof(
double));
717 for (std::size_t i = 0; i < il.size(); ++i)
718 gstate[i] = effort_or_acceleration_[il[i]];
725 values.resize(il.size());
726 for (std::size_t i = 0; i < il.size(); ++i)
727 values(i) = effort_or_acceleration_[il[i]];
735 std::fill(dirty_joint_transforms_.begin(), dirty_joint_transforms_.end(), 1);
736 dirty_link_transforms_ = robot_model_->getRootJoint();
745 if (dirty_link_transforms_ !=
nullptr)
748 if (dirty_collision_body_transforms_ !=
nullptr)
751 dirty_collision_body_transforms_ =
nullptr;
755 const EigenSTL::vector_Isometry3d& ot = link->getCollisionOriginTransforms();
756 const std::vector<int>& ot_id = link->areCollisionOriginTransformsIdentity();
757 const int index_co = link->getFirstCollisionBodyTransformIndex();
758 const int index_l = link->getLinkIndex();
759 for (std::size_t j = 0, end = ot.size(); j != end; ++j)
763 global_collision_body_transforms_[index_co + j] = global_link_transforms_[index_l];
767 global_collision_body_transforms_[index_co + j].affine().noalias() =
768 global_link_transforms_[index_l].affine() * ot[j].matrix();
777 if (dirty_link_transforms_ !=
nullptr)
779 updateLinkTransformsInternal(dirty_link_transforms_);
780 if (dirty_collision_body_transforms_)
782 dirty_collision_body_transforms_ =
783 robot_model_->getCommonRoot(dirty_collision_body_transforms_, dirty_link_transforms_);
787 dirty_collision_body_transforms_ = dirty_link_transforms_;
789 dirty_link_transforms_ =
nullptr;
793 void RobotState::updateLinkTransformsInternal(
const JointModel* start)
797 int idx_link = link->getLinkIndex();
802 if (link->parentJointIsFixed())
804 global_link_transforms_[idx_link].affine().noalias() =
805 global_link_transforms_[idx_parent].affine() * link->getJointOriginTransform().matrix();
809 if (link->jointOriginTransformIsIdentity())
811 global_link_transforms_[idx_link].affine().noalias() =
812 global_link_transforms_[idx_parent].affine() *
getJointTransform(link->getParentJointModel()).matrix();
816 global_link_transforms_[idx_link].affine().noalias() =
817 global_link_transforms_[idx_parent].affine() * link->getJointOriginTransform().matrix() *
824 const JointModel* root_joint = link->getParentJointModel();
825 if (root_joint->getVariableCount() == 0)
828 global_link_transforms_[idx_link] = Eigen::Isometry3d::Identity();
830 else if (link->jointOriginTransformIsIdentity())
836 global_link_transforms_[idx_link].affine().noalias() =
837 link->getJointOriginTransform().affine() *
getJointTransform(root_joint).matrix();
843 for (
const auto& attached_body : attached_body_map_)
845 attached_body.second->computeTransform(
846 global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]);
855 if (dirty_collision_body_transforms_)
857 dirty_collision_body_transforms_ =
858 robot_model_->getCommonRoot(dirty_collision_body_transforms_, link->
getParentJointModel());
865 global_link_transforms_[link->
getLinkIndex()] = transform;
870 updateLinkTransformsInternal(joint);
879 child_link = parent_link;
895 updateLinkTransformsInternal(joint);
903 for (
const auto& attached_body : attached_body_map_)
906 global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]);
915 if ((idx = frame.find(
'/')) != std::string::npos)
917 std::string
object{ frame.substr(0, idx) };
921 if (!body->hasSubframeTransform(frame))
932 return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
940 return variable_joint_transforms_[idx];
943 unsigned char&
dirty = dirty_joint_transforms_[idx];
949 return variable_joint_transforms_[idx];
954 const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
976 const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
996 markDirtyJointTransforms(joint);
997 updateMimicJoint(joint);
1003 for (
const JointModel* jm : robot_model_->getActiveJointModels())
1022 updateMimicJoint(joint);
1045 std::pair<double, const JointModel*>
1048 double distance = std::numeric_limits<double>::max();
1062 std::vector<double> lower_bounds(bounds.size()), upper_bounds(bounds.size());
1063 for (std::size_t j = 0; j < bounds.size(); ++j)
1065 lower_bounds[j] = bounds[j].min_position_;
1066 upper_bounds[j] = bounds[j].max_position_;
1068 double new_distance = joint->distance(joint_values, &lower_bounds[0]);
1074 new_distance = joint->distance(joint_values, &upper_bounds[0]);
1081 return std::make_pair(
distance, index);
1089 const int idx = joint_id->getFirstVariableIndex();
1090 const std::vector<VariableBounds>& bounds = joint_id->getVariableBounds();
1093 for (std::size_t var_id = 0; var_id < joint_id->getVariableCount(); ++var_id)
1095 const double dtheta = std::abs(position_.at(idx + var_id) - *(other.
getVariablePositions() + idx + var_id));
1097 if (dtheta > dt * bounds[var_id].max_velocity_)
1110 const int idx = joint->getFirstVariableIndex();
1111 d += joint->getDistanceFactor() * joint->distance(&position_.at(idx), &other.position_.at(idx));
1123 return joint->
distance(&position_.at(idx), &other.position_.at(idx));
1128 checkInterpolationParamBounds(
getLogger(), t);
1131 std::fill(state.dirty_joint_transforms_.begin(), state.dirty_joint_transforms_.end(), 1);
1132 state.dirty_link_transforms_ = state.robot_model_->getRootJoint();
1137 checkInterpolationParamBounds(
getLogger(), t);
1141 const int idx = joint->getFirstVariableIndex();
1142 joint->interpolate(&position_.at(idx), &to.position_.at(idx), t, &state.position_.at(idx));
1144 state.updateMimicJoints(joint_group);
1155 joint->
interpolate(&position_.at(idx), &to.position_.at(idx), t, &state.position_.at(idx));
1156 state.markDirtyJointTransforms(joint);
1157 state.updateMimicJoint(joint);
1162 attached_body_update_callback_ = callback;
1167 return attached_body_map_.find(
id) != attached_body_map_.end();
1172 const auto it = attached_body_map_.find(
id);
1173 if (it == attached_body_map_.end())
1175 RCLCPP_ERROR(
getLogger(),
"Attached body '%s' not found",
id.c_str());
1179 return it->second.get();
1188 if (attached_body_update_callback_)
1189 attached_body_update_callback_(attached_body.get(),
true);
1190 attached_body_map_[attached_body->getName()] = std::move(attached_body);
1194 const std::vector<shapes::ShapeConstPtr>&
shapes,
1195 const EigenSTL::vector_Isometry3d& shape_poses,
const std::set<std::string>& touch_links,
1196 const std::string& link,
const trajectory_msgs::msg::JointTrajectory& detach_posture,
1199 attachBody(std::make_unique<AttachedBody>(robot_model_->getLinkModel(link),
id, pose,
shapes, shape_poses,
1200 touch_links, detach_posture, subframe_poses));
1205 attached_bodies.clear();
1206 attached_bodies.reserve(attached_body_map_.size());
1207 for (
const auto& it : attached_body_map_)
1208 attached_bodies.push_back(it.second.get());
1213 attached_bodies.clear();
1214 for (
const auto& it : attached_body_map_)
1216 if (group->
hasLinkModel(it.second->getAttachedLinkName()))
1217 attached_bodies.push_back(it.second.get());
1223 attached_bodies.clear();
1224 for (
const auto& it : attached_body_map_)
1226 if (it.second->getAttachedLink() == link_model)
1227 attached_bodies.push_back(it.second.get());
1233 for (
const auto& it : attached_body_map_)
1235 if (attached_body_update_callback_)
1236 attached_body_update_callback_(it.second.get(),
false);
1238 attached_body_map_.clear();
1243 for (
auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1245 if (it->second->getAttachedLink() != link)
1249 if (attached_body_update_callback_)
1250 attached_body_update_callback_(it->second.get(),
false);
1251 const auto del = it++;
1252 attached_body_map_.erase(del);
1258 for (
auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1260 if (!group->
hasLinkModel(it->second->getAttachedLinkName()))
1264 if (attached_body_update_callback_)
1265 attached_body_update_callback_(it->second.get(),
false);
1266 const auto del = it++;
1267 attached_body_map_.erase(del);
1273 const auto it = attached_body_map_.find(
id);
1274 if (it != attached_body_map_.end())
1276 if (attached_body_update_callback_)
1277 attached_body_update_callback_(it->second.get(),
false);
1278 attached_body_map_.erase(it);
1295 const auto& result =
getFrameInfo(frame_id, ignored_link, found);
1299 *frame_found = found;
1303 RCLCPP_WARN(
getLogger(),
"getFrameTransform() did not find a frame with name %s.", frame_id.c_str());
1310 bool& frame_found)
const
1312 if (!frame_id.empty() && frame_id[0] ==
'/')
1313 return getFrameInfo(frame_id.substr(1), robot_link, frame_found);
1315 static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
1316 if (frame_id == robot_model_->getModelFrame())
1318 robot_link = robot_model_->getRootLink();
1320 return IDENTITY_TRANSFORM;
1322 if ((robot_link = robot_model_->getLinkModel(frame_id, &frame_found)))
1324 assert(checkLinkTransforms());
1325 return global_link_transforms_[robot_link->
getLinkIndex()];
1327 robot_link =
nullptr;
1330 const auto jt = attached_body_map_.find(frame_id);
1331 if (jt != attached_body_map_.end())
1333 const Eigen::Isometry3d& transform = jt->second->getGlobalPose();
1334 robot_link = jt->second->getAttachedLink();
1336 assert(checkLinkTransforms());
1341 for (
const auto& body : attached_body_map_)
1343 const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found);
1346 robot_link = body.second->getAttachedLink();
1347 assert(checkLinkTransforms());
1352 robot_link =
nullptr;
1353 frame_found =
false;
1354 return IDENTITY_TRANSFORM;
1359 if (!frame_id.empty() && frame_id[0] ==
'/')
1361 if (robot_model_->hasLinkModel(frame_id))
1365 const auto it = attached_body_map_.find(frame_id);
1366 if (it != attached_body_map_.end())
1367 return !it->second->getGlobalCollisionBodyTransforms().empty();
1370 for (
const auto& body : attached_body_map_)
1372 if (body.second->hasSubframeTransform(frame_id))
1379 const std_msgs::msg::ColorRGBA& color,
const std::string& ns,
1380 const rclcpp::Duration& dur,
bool include_attached)
const
1382 std::size_t cur_num = arr.markers.size();
1384 unsigned int id = cur_num;
1385 for (std::size_t i = cur_num; i < arr.markers.size(); ++i, ++id)
1387 arr.markers[i].ns = ns;
1388 arr.markers[i].id = id;
1389 arr.markers[i].lifetime = dur;
1390 arr.markers[i].color = color;
1395 bool include_attached)
const
1397 rclcpp::Clock clock;
1398 for (
const std::string& link_name : link_names)
1400 RCLCPP_DEBUG(
getLogger(),
"Trying to get marker for link '%s'", link_name.c_str());
1401 const LinkModel* link_model = robot_model_->getLinkModel(link_name);
1404 if (include_attached)
1406 for (
const auto& it : attached_body_map_)
1408 if (it.second->getAttachedLink() == link_model)
1410 for (std::size_t j = 0; j < it.second->getShapes().size(); ++j)
1412 visualization_msgs::msg::Marker att_mark;
1413 att_mark.header.frame_id = robot_model_->getModelFrame();
1414 att_mark.header.stamp = clock.now();
1415 if (shapes::constructMarkerFromShape(it.second->getShapes()[j].get(), att_mark))
1418 if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<double>::epsilon())
1420 att_mark.pose = tf2::toMsg(it.second->getGlobalCollisionBodyTransforms()[j]);
1421 arr.markers.push_back(att_mark);
1431 for (std::size_t j = 0; j < link_model->
getShapes().size(); ++j)
1433 visualization_msgs::msg::Marker mark;
1434 mark.header.frame_id = robot_model_->getModelFrame();
1435 mark.header.stamp = clock.now();
1439 if (mesh_resource.empty() || link_model->
getShapes().size() > 1)
1441 if (!shapes::constructMarkerFromShape(link_model->
getShapes()[j].get(), mark))
1444 if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<double>::epsilon())
1451 mark.type = mark.MESH_RESOURCE;
1452 mark.mesh_use_embedded_materials =
false;
1453 mark.mesh_resource = mesh_resource;
1456 mark.scale.x = mesh_scale[0];
1457 mark.scale.y = mesh_scale[1];
1458 mark.scale.z = mesh_scale[2];
1462 arr.markers.push_back(mark);
1470 Eigen::MatrixXd result;
1472 throw Exception(
"Unable to compute Jacobian");
1477 const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
1478 bool use_quaternion_representation)
const
1480 assert(checkLinkTransforms());
1485 RCLCPP_ERROR(
getLogger(),
"The group '%s' is not a chain. Cannot compute Jacobian.", group->
getName().c_str());
1490 if (descendant_links.find(link) == descendant_links.end())
1492 RCLCPP_ERROR(
getLogger(),
"Link name '%s' does not exist in the chain '%s' or is not a child for this chain",
1498 if (joint_models.empty())
1500 RCLCPP_ERROR(
getLogger(),
"The group '%s' doesn't contain any joint models. Cannot compute Jacobian.",
1509 const Eigen::Isometry3d root_pose_world =
1511 const int rows = use_quaternion_representation ? 7 : 6;
1513 jacobian.resize(rows, columns);
1517 const Eigen::Vector3d tip_point = root_pose_tip * reference_point_position;
1523 for (std::size_t joint = 0; joint < active_joints; ++joint)
1526 const JointModel* joint_model = joint_models[joint];
1534 root_pose_link.linear() *
static_cast<const RevoluteJointModel*
>(joint_model)->getAxis();
1535 jacobian.block<3, 1>(0, i) = axis_wrt_origin.cross(tip_point - root_pose_link.translation());
1536 jacobian.block<3, 1>(3, i) = axis_wrt_origin;
1541 root_pose_link.linear() *
static_cast<const PrismaticJointModel*
>(joint_model)->getAxis();
1542 jacobian.block<3, 1>(0, i) = axis_wrt_origin;
1543 jacobian.block<3, 1>(3, i) = Eigen::Vector3d::Zero();
1547 jacobian.block<3, 1>(0, i) = root_pose_link.linear() *
Eigen::Vector3d(1.0, 0.0, 0.0);
1548 jacobian.block<3, 1>(0, i + 1) = root_pose_link.linear() *
Eigen::Vector3d(0.0, 1.0, 0.0);
1549 jacobian.block<3, 1>(0, i + 2) =
1550 (root_pose_link.linear() *
Eigen::Vector3d(0.0, 0.0, 1.0)).cross(tip_point - root_pose_link.translation());
1551 jacobian.block<3, 1>(3, i + 2) = root_pose_link.linear() *
Eigen::Vector3d(0.0, 0.0, 1.0);
1555 RCLCPP_ERROR(
getLogger(),
"Unknown type of joint in Jacobian computation");
1561 if (use_quaternion_representation)
1568 Eigen::Quaterniond q(root_pose_tip.linear());
1569 double w = q.w(), x = q.x(), y = q.y(), z = q.z();
1570 Eigen::MatrixXd quaternion_update_matrix(4, 3);
1571 quaternion_update_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w;
1572 jacobian.block(3, 0, 4, columns) = 0.5 * quaternion_update_matrix * jacobian.block(3, 0, 3, columns);
1580 Eigen::VectorXd qdot;
1588 Eigen::Matrix<double, 6, 1> t;
1589 tf2::fromMsg(twist, t);
1594 const Eigen::VectorXd& twist,
const LinkModel* tip)
const
1603 Eigen::MatrixXd e_wb = Eigen::ArrayXXd::Zero(6, 6);
1604 e_wb.block(0, 0, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1605 e_wb.block(3, 3, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1609 Eigen::JacobiSVD<Eigen::MatrixXd> svd_of_j(j, Eigen::ComputeThinU | Eigen::ComputeThinV);
1610 const Eigen::MatrixXd& u = svd_of_j.matrixU();
1611 const Eigen::MatrixXd& v = svd_of_j.matrixV();
1612 const Eigen::VectorXd& s = svd_of_j.singularValues();
1614 Eigen::VectorXd sinv = s;
1615 static const double PINVTOLER = std::numeric_limits<double>::epsilon();
1617 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1619 if (fabs(s(i)) > maxsv)
1622 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1625 if (fabs(s(i)) > maxsv * PINVTOLER)
1627 sinv(i) = 1.0 / s(i);
1634 Eigen::MatrixXd jinv = (v * sinv.asDiagonal() * u.transpose());
1637 qdot = jinv * twist;
1651 std::vector<double> values;
1653 return constraint(
this, jmg, &values[0]);
1667 RCLCPP_ERROR(
getLogger(),
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1670 return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint,
options, cost_function);
1678 Eigen::Isometry3d mat;
1679 tf2::fromMsg(pose, mat);
1680 static std::vector<double> consistency_limits;
1681 return setFromIK(jmg, mat, tip, consistency_limits, timeout, constraint,
options, cost_function);
1692 RCLCPP_ERROR(
getLogger(),
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1695 static std::vector<double> consistency_limits;
1696 return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, timeout, constraint,
options, cost_function);
1704 static std::vector<double> consistency_limits;
1705 return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint,
options, cost_function);
1712 const std::vector<double>& ik_sol, moveit_msgs::msg::MoveItErrorCodes& error_code)
1715 std::vector<double> solution(bij.size());
1716 for (std::size_t i = 0; i < bij.size(); ++i)
1717 solution[bij[i]] = ik_sol[i];
1718 if (constraint(state, group, &solution[0]))
1720 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
1724 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
1741 getLinkModel((!ik_frame.empty() && ik_frame[0] ==
'/') ? ik_frame.substr(1) : ik_frame);
1744 RCLCPP_ERROR(
getLogger(),
"The following IK frame does not exist: %s", ik_frame.c_str());
1753 const std::vector<double>& consistency_limits_in,
double timeout,
1759 EigenSTL::vector_Isometry3d poses;
1760 poses.push_back(pose_in);
1762 std::vector<std::string> tips;
1763 tips.push_back(tip_in);
1765 std::vector<std::vector<double> > consistency_limits;
1766 consistency_limits.push_back(consistency_limits_in);
1768 return setFromIK(jmg, poses, tips, consistency_limits, timeout, constraint,
options, cost_function);
1772 const std::vector<std::string>& tips_in,
double timeout,
1777 const std::vector<std::vector<double> > consistency_limits;
1778 return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint,
options, cost_function);
1782 const std::vector<std::string>& tips_in,
1783 const std::vector<std::vector<double> >& consistency_limit_sets,
double timeout,
1789 if (poses_in.size() != tips_in.size())
1791 RCLCPP_ERROR(
getLogger(),
"Number of poses must be the same as number of tips");
1799 bool valid_solver =
true;
1802 valid_solver =
false;
1805 else if (poses_in.size() > 1)
1807 std::string error_msg;
1808 if (!solver->supportsGroup(jmg, &error_msg))
1811 RCLCPP_ERROR(
getLogger(),
"Kinematics solver %s does not support joint group %s. Error: %s",
1812 typeid(solver_ref).
name(), jmg->
getName().c_str(), error_msg.c_str());
1813 valid_solver =
false;
1820 if (poses_in.size() > 1)
1827 RCLCPP_ERROR(
getLogger(),
"No kinematics solver instantiated for group '%s'", jmg->
getName().c_str());
1833 std::vector<double> consistency_limits;
1834 if (consistency_limit_sets.size() > 1)
1837 "Invalid number (%zu) of sets of consistency limits for a setFromIK request "
1838 "that is being solved by a single IK solver",
1839 consistency_limit_sets.size());
1842 else if (consistency_limit_sets.size() == 1)
1843 consistency_limits = consistency_limit_sets[0];
1845 const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
1848 std::vector<bool> tip_frames_used(solver_tip_frames.size(),
false);
1851 std::vector<geometry_msgs::msg::Pose> ik_queries(solver_tip_frames.size());
1854 for (std::size_t i = 0; i < poses_in.size(); ++i)
1857 Eigen::Isometry3d pose = poses_in[i];
1858 std::string pose_frame = tips_in[i];
1861 if (!pose_frame.empty() && pose_frame[0] ==
'/')
1862 pose_frame = pose_frame.substr(1);
1870 bool found_valid_frame =
false;
1871 std::size_t solver_tip_id;
1872 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1875 if (tip_frames_used[solver_tip_id])
1879 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1883 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1884 solver_tip_frame = solver_tip_frame.substr(1);
1886 if (pose_frame != solver_tip_frame)
1891 pose_frame = body->getAttachedLinkName();
1892 pose = pose * body->getPose().inverse();
1894 if (pose_frame != solver_tip_frame)
1896 const LinkModel* link_model =
getLinkModel(pose_frame);
1899 RCLCPP_ERROR(
getLogger(),
"The following Pose Frame does not exist: %s", pose_frame.c_str());
1902 const LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms();
1903 for (
const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
1907 pose_frame = solver_tip_frame;
1908 pose = pose * fixed_link.second;
1917 if (pose_frame == solver_tip_frame)
1919 found_valid_frame =
true;
1926 if (!found_valid_frame)
1928 RCLCPP_ERROR(
getLogger(),
"Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str());
1930 std::stringstream ss;
1931 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1932 ss << solver_tip_frames[solver_tip_id] <<
", ";
1933 RCLCPP_ERROR(
getLogger(),
"Available tip frames: [%s]", ss.str().c_str());
1938 tip_frames_used[solver_tip_id] =
true;
1941 geometry_msgs::msg::Pose ik_query;
1942 ik_query = tf2::toMsg(pose);
1945 ik_queries[solver_tip_id] = ik_query;
1949 for (std::size_t solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1952 if (tip_frames_used[solver_tip_id])
1956 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1960 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1961 solver_tip_frame = solver_tip_frame.substr(1);
1971 geometry_msgs::msg::Pose ik_query;
1972 ik_query = tf2::toMsg(current_pose);
1975 ik_queries[solver_tip_id] = ik_query;
1978 tip_frames_used[solver_tip_id] =
true;
1982 if (timeout < std::numeric_limits<double>::epsilon())
1989 ik_callback_fn = ik_callback_fn = [
this, jmg, constraint](
const geometry_msgs::msg::Pose& pose,
1990 const std::vector<double>& joints,
1991 moveit_msgs::msg::MoveItErrorCodes& error_code) {
1992 ikCallbackFnAdapter(
this, jmg, constraint, pose, joints, error_code);
1999 std::vector<double> initial_values;
2001 std::vector<double> seed(bij.size());
2002 for (std::size_t i = 0; i < bij.size(); ++i)
2003 seed[i] = initial_values[bij[i]];
2006 std::vector<double> ik_sol;
2007 moveit_msgs::msg::MoveItErrorCodes error;
2009 if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, cost_function,
2012 std::vector<double> solution(bij.size());
2013 for (std::size_t i = 0; i < bij.size(); ++i)
2014 solution[bij[i]] = ik_sol[i];
2022 const std::vector<std::string>& tips_in,
2023 const std::vector<std::vector<double> >& consistency_limits,
double timeout,
2030 std::vector<const JointModelGroup*> sub_groups;
2034 if (poses_in.size() != sub_groups.size())
2036 RCLCPP_ERROR(
getLogger(),
"Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(),
2041 if (tips_in.size() != sub_groups.size())
2043 RCLCPP_ERROR(
getLogger(),
"Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(),
2048 if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
2050 RCLCPP_ERROR(
getLogger(),
"Number of consistency limit vectors must be the same as number of sub-groups");
2054 for (std::size_t i = 0; i < consistency_limits.size(); ++i)
2058 RCLCPP_ERROR(
getLogger(),
"Number of joints in consistency_limits is %zu but it should be should be %u", i,
2065 std::vector<kinematics::KinematicsBaseConstPtr> solvers;
2066 for (std::size_t i = 0; i < poses_in.size(); ++i)
2068 kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
2071 RCLCPP_ERROR(
getLogger(),
"Could not find solver for group '%s'", sub_groups[i]->getName().c_str());
2074 solvers.push_back(solver);
2078 EigenSTL::vector_Isometry3d transformed_poses = poses_in;
2079 std::vector<std::string> pose_frames = tips_in;
2082 for (std::size_t i = 0; i < poses_in.size(); ++i)
2084 ASSERT_ISOMETRY(transformed_poses[i])
2085 Eigen::Isometry3d& pose = transformed_poses[i];
2086 std::string& pose_frame = pose_frames[i];
2093 std::string solver_tip_frame = solvers[i]->getTipFrame();
2097 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
2098 solver_tip_frame = solver_tip_frame.substr(1);
2100 if (pose_frame != solver_tip_frame)
2106 pose = pose * body->
getPose().inverse();
2108 if (pose_frame != solver_tip_frame)
2115 for (
const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
2117 if (fixed_link.first->getName() == solver_tip_frame)
2119 pose_frame = solver_tip_frame;
2120 pose = pose * fixed_link.second;
2127 if (pose_frame != solver_tip_frame)
2129 RCLCPP_ERROR(
getLogger(),
"Cannot compute IK for query pose reference frame '%s', desired: '%s'",
2130 pose_frame.c_str(), solver_tip_frame.c_str());
2136 std::vector<geometry_msgs::msg::Pose> ik_queries(poses_in.size());
2140 ik_callback_fn = [
this, jmg, constraint](
const geometry_msgs::msg::Pose pose,
const std::vector<double>& joints,
2141 moveit_msgs::msg::MoveItErrorCodes& error_code) {
2142 ikCallbackFnAdapter(
this, jmg, constraint, pose, joints, error_code);
2146 for (std::size_t i = 0; i < transformed_poses.size(); ++i)
2148 Eigen::Quaterniond quat(transformed_poses[i].linear());
2150 ik_queries[i].position.x = point.x();
2151 ik_queries[i].position.y = point.y();
2152 ik_queries[i].position.z = point.z();
2153 ik_queries[i].orientation.x = quat.x();
2154 ik_queries[i].orientation.y = quat.y();
2155 ik_queries[i].orientation.z = quat.z();
2156 ik_queries[i].orientation.w = quat.w();
2160 if (timeout < std::numeric_limits<double>::epsilon())
2163 auto start = std::chrono::system_clock::now();
2166 bool first_seed =
true;
2167 unsigned int attempts = 0;
2171 RCLCPP_DEBUG(
getLogger(),
"IK attempt: %d", attempts);
2172 bool found_solution =
true;
2173 for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
2175 const std::vector<size_t>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
2176 std::vector<double> seed(bij.size());
2180 std::vector<double> initial_values;
2182 for (std::size_t i = 0; i < bij.size(); ++i)
2183 seed[i] = initial_values[bij[i]];
2189 std::vector<double> random_values;
2190 sub_groups[sg]->getVariableRandomPositions(rng, random_values);
2191 for (std::size_t i = 0; i < bij.size(); ++i)
2192 seed[i] = random_values[bij[i]];
2196 std::vector<double> ik_sol;
2197 moveit_msgs::msg::MoveItErrorCodes error;
2198 const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
2199 if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, (timeout - elapsed) / sub_groups.size(), climits, ik_sol,
2202 std::vector<double> solution(bij.size());
2203 for (std::size_t i = 0; i < bij.size(); ++i)
2204 solution[bij[i]] = ik_sol[i];
2209 found_solution =
false;
2215 std::vector<double> full_solution;
2217 if (constraint ? constraint(
this, jmg, &full_solution[0]) :
true)
2219 RCLCPP_DEBUG(
getLogger(),
"Found IK solution");
2223 elapsed = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start).count();
2225 }
while (elapsed < timeout);
2231 assert(checkLinkTransforms());
2234 std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
2239 transform.translate(link->getCenteredBoundingBoxOffset());
2242 for (
const auto& it : attached_body_map_)
2244 const EigenSTL::vector_Isometry3d& transforms = it.second->getGlobalCollisionBodyTransforms();
2245 const std::vector<shapes::ShapeConstPtr>&
shapes = it.second->getShapes();
2246 for (std::size_t i = 0; i < transforms.size(); ++i)
2254 aabb.resize(6, 0.0);
2255 if (!bounding_box.isEmpty())
2259 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data(), 3) = bounding_box.min();
2260 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data() + 1, 3) = bounding_box.max();
2266 const std::vector<std::string>& nm = robot_model_->getVariableNames();
2267 for (std::size_t i = 0; i < nm.size(); ++i)
2268 out << nm[i] <<
'=' << position_[i] <<
'\n';
2282 if (joint->getVariableCount() > 1)
2295 out <<
" " << std::fixed << std::setprecision(5) << bound.
min_position_ <<
'\t';
2297 double step = delta / 20.0;
2299 bool marker_shown =
false;
2303 if (!marker_shown && current_value < value)
2306 marker_shown =
true;
2315 out <<
" \t" << std::fixed << std::setprecision(5) << bound.
max_position_ <<
" \t" << joint->getName()
2316 <<
" current: " << std::fixed << std::setprecision(5) << current_value <<
'\n';
2325 out <<
" * Dirty Joint Transforms: \n";
2326 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2330 out <<
" " << joint->getName() <<
'\n';
2332 out <<
" * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->
getName() :
"NULL") <<
'\n';
2333 out <<
" * Dirty Collision Body Transforms: "
2334 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->
getName() :
"NULL\n");
2339 out <<
"Robot State @" <<
this <<
'\n';
2341 std::size_t n = robot_model_->getVariableCount();
2342 if (!position_.empty())
2344 out <<
" * Position: ";
2345 for (std::size_t i = 0; i < n; ++i)
2346 out << position_[i] <<
' ';
2350 out <<
" * Position: NULL\n";
2352 if (!velocity_.empty())
2354 out <<
" * Velocity: ";
2355 for (std::size_t i = 0; i < n; ++i)
2356 out << velocity_[i] <<
' ';
2360 out <<
" * Velocity: NULL\n";
2362 if (has_acceleration_)
2364 out <<
" * Acceleration: ";
2365 for (std::size_t i = 0; i < n; ++i)
2366 out << effort_or_acceleration_[i] <<
' ';
2370 out <<
" * Acceleration: NULL\n";
2372 out <<
" * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->
getName() :
"NULL\n");
2373 out <<
" * Dirty Collision Body Transforms: "
2374 << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->
getName() :
"NULL\n");
2381 if (checkIsometry(transform, CHECK_ISOMETRY_PRECISION,
false))
2383 Eigen::Quaterniond q(transform.linear());
2384 out <<
"T.xyz = [" << transform.translation().x() <<
", " << transform.translation().y() <<
", "
2385 << transform.translation().z() <<
"], Q.xyzw = [" << q.x() <<
", " << q.y() <<
", " << q.z() <<
", " << q.w()
2390 out <<
"[NON-ISOMETRY] "
2391 << transform.matrix().format(
2392 Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols,
", ",
"; ",
"",
"",
"[",
"]"));
2399 if (variable_joint_transforms_.empty())
2401 out <<
"No transforms computed\n";
2405 out <<
"Joint transforms:\n";
2406 const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2409 out <<
" " << joint->getName();
2410 const int idx = joint->getJointIndex();
2411 if (dirty_joint_transforms_[idx])
2417 out <<
"Link poses:\n";
2418 const std::vector<const LinkModel*>& link_model = robot_model_->getLinkModels();
2419 for (
const LinkModel* link : link_model)
2421 out <<
" " << link->getName() <<
": ";
2422 printTransform(global_link_transforms_[link->getLinkIndex()], out);
2428 std::stringstream ss;
2429 ss <<
"ROBOT: " << robot_model_->getName() <<
'\n';
2430 getStateTreeJointString(ss, robot_model_->getRootJoint(),
" ",
true);
2436 void getPoseString(std::ostream& ss,
const Eigen::Isometry3d& pose,
const std::string& pfx)
2439 for (
int y = 0; y < 4; ++y)
2442 for (
int x = 0; x < 4; ++x)
2444 ss << std::setw(8) << pose(y, x) <<
' ';
2451 void RobotState::getStateTreeJointString(std::ostream& ss,
const JointModel* jm,
const std::string& pfx0,
2454 std::string pfx = pfx0 +
"+--";
2456 ss << pfx <<
"Joint: " << jm->getName() <<
'\n';
2458 pfx = pfx0 + (last ?
" " :
"| ");
2460 for (std::size_t i = 0; i < jm->getVariableCount(); ++i)
2463 ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] <<
'\n';
2466 const LinkModel* link_model = jm->getChildLinkModel();
2468 ss << pfx <<
"Link: " << link_model->getName() <<
'\n';
2469 getPoseString(ss, link_model->getJointOriginTransform(), pfx +
"joint_origin:");
2470 if (!variable_joint_transforms_.empty())
2472 getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx +
"joint_variable:");
2473 getPoseString(ss, global_link_transforms_[link_model->getLinkIndex()], pfx +
"link_global:");
2476 for (std::vector<const JointModel*>::const_iterator it = link_model->getChildJointModels().begin();
2477 it != link_model->getChildJointModels().end(); ++it)
2478 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 &, const moveit::core::JointModelGroup *, 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.
double getDefaultIKTimeout() const
Get the default IK timeout.
bool isChain() const
Check if this group is a linear chain.
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 std::set< const LinkModel * > & getUpdatedLinkModelsSet() const
Return the same data as getUpdatedLinkModels() but as a set.
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
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...
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group.
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
Get the values that correspond to a named state as read from the URDF. Return false on failure.
bool isContiguousWithinState() const
unsigned int getActiveVariableCount() const
Get the number of variables that describe the active joints in this joint group. This excludes variab...
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,...
virtual void computeVariablePositions(const Eigen::Isometry3d &transform, double *joint_values) const =0
Given the transform generated by joint, compute the corresponding joint values. Make sure the passed ...
size_t getJointIndex() const
Get the index of this joint within the robot model.
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
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.
virtual bool harmonizePosition(double *values, const Bounds &other_bounds) const
virtual void interpolate(const double *from, const double *to, const double t, double *state) const =0
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to 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 double distance(const double *value1, const double *value2) const =0
Compute the distance between two joint states of the same model (represented by the variable values)
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
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...
bool enforcePositionBounds(double *values) const
Force the specified values to be inside bounds and normalized. Quaternions are normalized,...
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 setJointVelocities(const JointModel *joint, const double *velocity)
void printTransforms(std::ostream &out=std::cout) const
void printTransform(const Eigen::Isometry3d &transform, std::ostream &out=std::cout) const
const double * getJointEffort(const std::string &joint_name) 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.
const double * getJointVelocities(const std::string &joint_name) const
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 ...
bool dirty() const
Returns true if anything in this state is dirty.
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....
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),...
const double * getJointAccelerations(const std::string &joint_name) const
void enforcePositionBounds(const JointModel *joint)
bool hasAttachedBody(const std::string &id) const
Check if an attached body named id exists in this state.
void enforceVelocityBounds(const JointModel *joint)
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.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
double distance(const urdf::Pose &transform)
A set of options for the kinematics solver.