From 54e4d8eed7a6100bf8afd5d20c8900128b110b38 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 24 Mar 2018 21:47:52 +0100 Subject: [PATCH] remove tool_to_grasp_tf from GenerateGraspPose --- core/demo/plan_pick_pa10.cpp | 6 +- core/demo/plan_pick_ur5.cpp | 2 +- .../stages/generate_grasp_pose.h | 10 --- core/src/stages/generate_grasp_pose.cpp | 70 +++---------------- 4 files changed, 12 insertions(+), 76 deletions(-) diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index 00948ca2..3fa104ae 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -107,14 +107,14 @@ int main(int argc, char** argv){ gengrasp->properties().configureInitFrom(Stage::PARENT); gengrasp->setNamedPose("open"); gengrasp->setObject("object"); - gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)* - Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), - "lh_tool_frame"); gengrasp->setAngleDelta(M_PI / 10.); gengrasp->setMonitoredStage(initial_stage); auto ik = std::make_unique("compute ik", std::move(gengrasp)); ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"}); + ik->setIKFrame(Eigen::Translation3d(0,0,.05)* + Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), + "lh_tool_frame"); ik->setMaxIKSolutions(1); t.add(std::move(ik)); } diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index c18f35a9..61ddb236 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -86,13 +86,13 @@ int main(int argc, char** argv){ gengrasp->properties().configureInitFrom(Stage::PARENT); gengrasp->setNamedPose("open"); gengrasp->setObject("object"); - gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0"); gengrasp->setAngleDelta(-.2); gengrasp->setMonitoredStage(initial_stage); auto ik = std::make_unique("compute ik", std::move(gengrasp)); ik->properties().configureInitFrom(Stage::PARENT, {"eef"}); ik->setMaxIKSolutions(8); + ik->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); t.add(std::move(ik)); } diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index d6559312..b1974860 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -39,8 +39,6 @@ #pragma once #include -#include -#include namespace moveit { namespace task_constructor { namespace stages { @@ -55,14 +53,6 @@ public: void setEndEffector(const std::string &eef); void setNamedPose(const std::string &pose_name); void setObject(const std::string &object); - - void setToolToGraspTF(const geometry_msgs::TransformStamped &transform); - void setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link = ""); - template - void setToolToGraspTF(const T& t, const std::string& link = "") { - Eigen::Affine3d transform; transform = t; - setToolToGraspTF(transform, link); - } void setAngleDelta(double delta); protected: diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index ddf17d38..20526f57 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -53,7 +53,6 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) p.declare("eef", "name of end-effector"); p.declare("pregrasp", "name of end-effector's pregrasp pose"); p.declare("object"); - p.declare("tool_to_grasp_tf", geometry_msgs::TransformStamped(), "transform from robot tool frame to grasp frame"); p.declare("angle_delta", 0.1, "angular steps (rad)"); } @@ -69,18 +68,6 @@ void GenerateGraspPose::setObject(const std::string &object){ setProperty("object", object); } -void GenerateGraspPose::setToolToGraspTF(const geometry_msgs::TransformStamped &transform){ - setProperty("tool_to_grasp_tf", transform); -} - -void GenerateGraspPose::setToolToGraspTF(const Eigen::Affine3d &transform, const std::string &link){ - geometry_msgs::TransformStamped stamped; - stamped.header.frame_id = link; - stamped.child_frame_id = "grasp_frame"; - tf::transformEigenToMsg(transform, stamped.transform); - setToolToGraspTF(stamped); -} - void GenerateGraspPose::setAngleDelta(double delta){ setProperty("angle_delta", delta); } @@ -116,69 +103,28 @@ bool GenerateGraspPose::compute(){ robot_state.setToDefaultValues(jmg , joint_pose); } - geometry_msgs::TransformStamped tool2grasp_msg = props.get("tool_to_grasp_tf"); - const std::string &link_name = jmg->getEndEffectorParentGroup().second; - if (tool2grasp_msg.header.frame_id.empty()) // if no frame_id is given - tool2grasp_msg.header.frame_id = link_name; // interpret the transform w.r.t. eef link frame - Eigen::Affine3d to_grasp; - Eigen::Affine3d grasp2tool, grasp2link; - tf::transformMsgToEigen(tool2grasp_msg.transform, to_grasp); - 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) { - // convert to_grasp into transform w.r.t. link (instead of tool frame_id) - const robot_model::LinkModel* tool_link = robot_state.getLinkModel(tool2grasp_msg.header.frame_id); - if (!tool_link) throw std::runtime_error("requested frame '" + tool2grasp_msg.header.frame_id + "' is not a robot link"); - - const Eigen::Affine3d link_pose = robot_state.getGlobalLinkTransform(link); - const Eigen::Affine3d tool_pose = robot_state.getGlobalLinkTransform(tool_link); - to_grasp = link_pose.inverse() * tool_pose * to_grasp; - grasp2link = to_grasp.inverse(); - } else - grasp2link = grasp2tool; - const std::string& object_name = props.get("object"); if (!scene_->knowsFrameTransform(object_name)) throw std::runtime_error("requested object does not exist or could not be retrieved"); - const Eigen::Affine3d object_pose = scene_->getFrameTransform(object_name); + + geometry_msgs::PoseStamped target_pose_msg; + target_pose_msg.header.frame_id = object_name; while( canCompute() ) { // rotate object pose about z-axis - Eigen::Affine3d grasp_pose = object_pose * Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()); - Eigen::Affine3d link_pose = grasp_pose * grasp2link; + Eigen::Affine3d target_pose(Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ())); current_angle_ += props.get("angle_delta"); InterfaceState state(scene_); - geometry_msgs::PoseStamped goal_pose_msg; - goal_pose_msg.header.frame_id = link_name; - tf::poseEigenToMsg(link_pose, goal_pose_msg.pose); - state.properties().set("target_pose", goal_pose_msg); + tf::poseEigenToMsg(target_pose, target_pose_msg.pose); + state.properties().set("target_pose", target_pose_msg); SubTrajectory trajectory; trajectory.setCost(0.0); trajectory.setName(std::to_string(current_angle_)); - // add frame at grasp pose - goal_pose_msg.header.frame_id = scene_->getPlanningFrame(); - tf::poseEigenToMsg(grasp_pose, goal_pose_msg.pose); - rviz_marker_tools::appendFrame(trajectory.markers(), goal_pose_msg, 0.1, "grasp frame"); - - // add end-effector marker visualizing the pose of the end-effector, including all rigidly connected parent links - const robot_model::LinkModel* link = robot_state.getLinkModel(link_name); - const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link); - if (parent != link) // transform pose into pose suitable to place parent - link_pose = link_pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent); - - robot_state.updateStateWithLinkAt(parent, link_pose); - auto appender = [&trajectory](visualization_msgs::Marker& marker, const std::string& name) { - marker.ns = "grasp eef"; - marker.color.a *= 0.5; - trajectory.markers().push_back(marker); - }; - generateVisualMarkers(robot_state, appender, parent->getParentJointModel()->getDescendantLinkModels()); + // add frame at target pose + rviz_marker_tools::appendFrame(trajectory.markers(), target_pose_msg, 0.1, "grasp frame"); spawn(std::move(state), std::move(trajectory)); return true;