GenerateGraspPose: correctly check for existence of link frames

This commit is contained in:
Robert Haschke 2018-02-13 10:29:00 +01:00
parent 7e66c24822
commit a8cf2dc844

View File

@ -117,22 +117,25 @@ bool GenerateGraspPose::compute(){
tf::transformMsgToEigen(tool2grasp_msg.transform, to_grasp); tf::transformMsgToEigen(tool2grasp_msg.transform, to_grasp);
grasp2tool = to_grasp.inverse(); grasp2tool = to_grasp.inverse();
const robot_model::LinkModel* link = robot_state.getLinkModel(link_name);
if (!link) throw std::runtime_error("requested link '" + link_name + "' does not exist");
if (tool2grasp_msg.header.frame_id != link_name) { if (tool2grasp_msg.header.frame_id != link_name) {
// convert to_grasp into transform w.r.t. link (instead of tool frame_id) // convert to_grasp into transform w.r.t. link (instead of tool frame_id)
const Eigen::Affine3d link_pose = scene_->getFrameTransform(link_name); const robot_model::LinkModel* tool_link = robot_state.getLinkModel(tool2grasp_msg.header.frame_id);
if(link_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) if (!tool_link) throw std::runtime_error("requested frame '" + tool2grasp_msg.header.frame_id + "' is not a robot link");
throw std::runtime_error("requested link does not exist or could not be retrieved");
const Eigen::Affine3d tool_pose = scene_->getFrameTransform(tool2grasp_msg.header.frame_id); const Eigen::Affine3d link_pose = robot_state.getGlobalLinkTransform(link);
if(tool_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) const Eigen::Affine3d tool_pose = robot_state.getGlobalLinkTransform(tool_link);
throw std::runtime_error("requested frame does not exist or could not be retrieved");
to_grasp = link_pose.inverse() * tool_pose * to_grasp; to_grasp = link_pose.inverse() * tool_pose * to_grasp;
grasp2link = to_grasp.inverse(); grasp2link = to_grasp.inverse();
} else } else
grasp2link = grasp2tool; grasp2link = grasp2tool;
const Eigen::Affine3d object_pose = scene_->getFrameTransform(props.get<std::string>("object")); const std::string& object_name = props.get<std::string>("object");
if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) if (!scene_->knowsFrameTransform(object_name))
throw std::runtime_error("requested object does not exist or could not be retrieved"); throw std::runtime_error("requested object does not exist or could not be retrieved");
const Eigen::Affine3d object_pose = scene_->getFrameTransform(object_name);
while( canCompute() ) { while( canCompute() ) {
// rotate object pose about z-axis // rotate object pose about z-axis