diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index ddc5d4db..972aa86e 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -79,9 +79,9 @@ int main(int argc, char** argv){ gengrasp->properties().configureInitFrom(Stage::PARENT); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); - gengrasp->setGraspFrame(Eigen::Translation3d(0,0,.05)* - Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), - "lh_tool_frame"); + gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)* + Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), + "lh_tool_frame"); gengrasp->setAngleDelta(-.2); auto ik = std::make_unique("compute ik", std::move(gengrasp)); diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index e5eee7ef..82905796 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -79,7 +79,7 @@ int main(int argc, char** argv){ gengrasp->setEndEffector("left_gripper"); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); - gengrasp->setGraspFrame(Eigen::Translation3d(), "l_gripper_tool_frame"); + gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); gengrasp->setAngleDelta(.2); auto ik = std::make_unique("compute ik", std::move(gengrasp)); diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 9a9489ad..3b3380d1 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -80,7 +80,7 @@ int main(int argc, char** argv){ gengrasp->properties().configureInitFrom(Stage::PARENT); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); - gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); + gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0"); gengrasp->setAngleDelta(-.2); auto ik = std::make_unique("compute ik", std::move(gengrasp)); 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 cf65f509..c072d38a 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -51,18 +51,16 @@ public: bool canCompute() const override; bool compute() override; - void setTimeout(double timeout); - void setEndEffector(const std::string &eef); void setGripperGraspPose(const std::string &pose_name); void setObject(const std::string &object); - void setGraspFrame(const geometry_msgs::TransformStamped &transform); - void setGraspFrame(const Eigen::Affine3d& transform, const std::string& link = ""); + void setToolToGraspTF(const geometry_msgs::TransformStamped &transform); + void setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link = ""); template - void setGraspFrame(const T& t, const std::string& link = "") { + void setToolToGraspTF(const T& t, const std::string& link = "") { Eigen::Affine3d transform; transform = t; - setGraspFrame(transform, link); + setToolToGraspTF(transform, link); } void setAngleDelta(double delta); diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 31f0d560..2d37b433 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -50,7 +50,7 @@ GenerateGraspPose::GenerateGraspPose(std::string name) p.declare("eef", "name of end-effector group"); p.declare("eef_named_pose"); p.declare("object"); - p.declare("grasp_frame", geometry_msgs::TransformStamped(), "robot frame to use for grasping"); + 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)"); } @@ -72,17 +72,16 @@ void GenerateGraspPose::setObject(const std::string &object){ setProperty("object", object); } -void GenerateGraspPose::setGraspFrame(const geometry_msgs::TransformStamped &frame){ - setProperty("grasp_frame", frame); +void GenerateGraspPose::setToolToGraspTF(const geometry_msgs::TransformStamped &transform){ + setProperty("tool_to_grasp_tf", transform); } -void GenerateGraspPose::setGraspFrame(const Eigen::Affine3d &transform, const std::string &link) -{ - geometry_msgs::TransformStamped frame; - frame.header.frame_id = link; - frame.child_frame_id = "grasp_frame"; - tf::transformEigenToMsg(transform, frame.transform); - setGraspFrame(frame); +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){ @@ -106,7 +105,7 @@ bool GenerateGraspPose::compute(){ robot_state.setToDefaultValues(jmg , eef_named_pose); } - geometry_msgs::TransformStamped grasp_frame = props.get("grasp_frame"); + geometry_msgs::TransformStamped grasp_frame = props.get("tool_to_grasp_tf"); const std::string &link_name = jmg ->getEndEffectorParentGroup().second; if (grasp_frame.header.frame_id.empty()) // if no frame_id is given grasp_frame.header.frame_id = link_name; // interpret the transform w.r.t. eef link frame diff --git a/core/test/test_plan_generate_grasp_pose.cpp b/core/test/test_plan_generate_grasp_pose.cpp index 236729d9..48511b17 100644 --- a/core/test/test_plan_generate_grasp_pose.cpp +++ b/core/test/test_plan_generate_grasp_pose.cpp @@ -41,7 +41,7 @@ int main(int argc, char** argv){ st->setEndEffector("gripper"); st->setObject("object"); st->setAngleDelta(0.1); - st->setGraspFrame(Eigen::Translation3d(.03,0,0)); + st->setToolToGraspTF(Eigen::Translation3d(.03,0,0)); t.add(std::move(st));