本文整理汇总了C++中robot_state::RobotState::getAttachedBody方法的典型用法代码示例。如果您正苦于以下问题：C++ RobotState::getAttachedBody方法的具体用法？C++ RobotState::getAttachedBody怎么用？C++ RobotState::getAttachedBody使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类robot_state::RobotState的用法示例。
在下文中一共展示了RobotState::getAttachedBody方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: saveObjectToAction
void MotionPlanningFrame::saveObjectToAction(apc_msgs::PrimitiveAction& action,
const std::string& object_id,
const robot_state::RobotState& start_state,
const robot_state::RobotState& goal_state)
{
// Clear attached object information.
action.object_id = "";
action.attached_link_id = "";
action.object_trajectory.poses.clear();
if (object_id.empty())
return;
if (action.group_id.empty())
action.group_id = planning_display_->getCurrentPlanningGroup();
Eigen::Affine3d T_object_start_link;
Eigen::Affine3d T_object_goal_link;
std::string eef_link = computeEefLink(action.group_id);
// If the start state and goal state have attached objects
// matching this object ID, we extract the object poses from
// the attached objects instead of from the nearest.
if (start_state.getAttachedBody(object_id)) {
ROS_DEBUG("Saving attached bodies to action");
APC_ASSERT(goal_state.getAttachedBody(object_id),
"Failed to find corresponding attached body %s on goal state", object_id.c_str());
APC_ASSERT(eef_link == start_state.getAttachedBody(object_id)->getAttachedLink()->getName(),
"Mismatch between attached link %s and computed link %s",
start_state.getAttachedBody(object_id)->getAttachedLink()->getName().c_str(),
eef_link.c_str());
APC_ASSERT(eef_link == goal_state.getAttachedBody(object_id)->getAttachedLink()->getName(),
"Mismatch between attached link %s and computed link %s",
goal_state.getAttachedBody(object_id)->getAttachedLink()->getName().c_str(),
eef_link.c_str());
T_object_start_link = start_state.getAttachedBody(object_id)->getFixedTransforms()[0];
T_object_goal_link = goal_state.getAttachedBody(object_id)->getFixedTransforms()[0];
}
// Else, we compute the relative poses to the nearest instance of object ID.
else {
ROS_DEBUG("Saving nearest bodies to action");
KeyPoseMap world_state = computeWorldKeyPoseMap();
Eigen::Affine3d T_object = computeNearestFrameKeyPose(object_id, eef_link, start_state, world_state);
// Get object transform relative to the end-effector link.
Eigen::Affine3d T_start_inv = start_state.getGlobalLinkTransform(eef_link).inverse();
T_object_start_link = T_start_inv * T_object;
Eigen::Affine3d T_goal_inv = goal_state.getGlobalLinkTransform(eef_link).inverse();
T_object_goal_link = T_start_inv * T_object;
}
// Write to object.
action.object_id = object_id;
action.attached_link_id = eef_link;
action.object_trajectory.poses.resize(2);
tf::poseEigenToMsg(T_object_start_link, action.object_trajectory.poses[0]);
tf::poseEigenToMsg(T_object_goal_link, action.object_trajectory.poses[1]);
}