本文整理汇总了C++中robot_state::RobotState::clearAttachedBodies方法的典型用法代码示例。如果您正苦于以下问题：C++ RobotState::clearAttachedBodies方法的具体用法？C++ RobotState::clearAttachedBodies怎么用？C++ RobotState::clearAttachedBodies使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类robot_state::RobotState的用法示例。
在下文中一共展示了RobotState::clearAttachedBodies方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: computeAttachNearestObjectToStateMatchingId
void MotionPlanningFrame::computeAttachNearestObjectToStateMatchingId(const std::string& object_id,
const std::string& group_id,
const KeyPoseMap& world_state,
robot_state::RobotState& robot_state)
{
// Remove any previously attached bodies.
robot_state.clearAttachedBodies();
// Get the end-effector link.
std::string eef_link_id = computeEefLink(group_id);
// Get an object key of object ID in the world.
std::string object_key = computeNearestObjectKey(object_id, eef_link_id, robot_state, world_state);
// Get object transform relative to the end-effector link.
Eigen::Affine3d T_object_world = world_state.find(object_key)->second;
Eigen::Affine3d T_eef_inv = robot_state.getGlobalLinkTransform(eef_link_id).inverse();
Eigen::Affine3d T_object_eef = T_eef_inv * T_object_world;
// Attach object to robot state.
planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
collision_detection::CollisionWorld::ObjectConstPtr object = ps->getWorld()->getObject(object_key);
APC_ASSERT(object,
"Failed to find object key %s in world", object_key.c_str());
std::vector<shapes::ShapeConstPtr> object_shapes = object->shapes_;
EigenSTL::vector_Affine3d object_poses = object->shape_poses_;
for (int i = 0; i < object_poses.size(); i++)
object_poses[i] = T_object_eef * object_poses[0].inverse() * object_poses[i];
moveit_msgs::AttachedCollisionObject aco; // For dummy arguments.
robot_state.attachBody(object_id, object_shapes, object_poses, aco.touch_links, eef_link_id, aco.detach_posture);
robot_state.update();
}示例2: attachObjectToState
void MotionPlanningFrame::attachObjectToState(robot_state::RobotState& state,
const std::string& object_id,
const std::string& link_id,
const geometry_msgs::Pose& pose_object_link)
{
state.clearAttachedBodies();
if (object_id.empty() || link_id.empty())
return;
Eigen::Affine3d T_object_link;
tf::poseMsgToEigen(pose_object_link, T_object_link);
attachObjectToState(state, object_id, link_id, T_object_link);
}