diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index 340b4b42..e49d0209 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -46,7 +46,7 @@ int main(int argc, char** argv){ t.add(std::move(initial)); auto grasp_generator = std::make_unique(); - grasp_generator->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); + grasp_generator->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); grasp_generator->setAngleDelta(.2); grasp_generator->setPreGraspPose("open"); grasp_generator->setGraspPose("closed"); diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index 160ff4e1..5f903f0b 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -38,7 +38,7 @@ #include #include -#include +#include #include namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } } @@ -76,14 +76,17 @@ public: properties().set("grasp", grasp); } - void setToolToGraspTF(const geometry_msgs::TransformStamped &transform) { - properties().set("tool_to_grasp_tf", transform); + void setIKFrame(const geometry_msgs::PoseStamped &transform) { + properties().set("ik_frame", transform); } - void setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link = ""); + void setIKFrame(const Eigen::Affine3d& pose, const std::string& link); template - void setToolToGraspTF(const T& t, const std::string& link = "") { + void setIKFrame(const T& t, const std::string& link) { Eigen::Affine3d transform; transform = t; - setToolToGraspTF(transform, link); + setIKFrame(transform, link); + } + void setIKFrame(const std::string& link) { + setIKFrame(Eigen::Affine3d::Identity(), link); } void setAngleDelta(double angle_delta) { diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 7de51a36..1eb919fd 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -57,12 +57,12 @@ SimpleGrasp::SimpleGrasp(const std::string& name) grasp_generator_ = gengrasp.get(); auto ik = std::make_unique("compute ik", std::move(gengrasp)); - const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta", "tool_to_grasp_tf" }; + const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta" }; ik->exposePropertiesOfChild(0, grasp_prop_names); insert(std::move(ik)); exposePropertiesOfChild(-1, grasp_prop_names); - exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout" }); + exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout", "ik_frame" }); } { auto allow_touch = std::make_unique("allow object collision"); @@ -124,12 +124,11 @@ void SimpleGrasp::setMonitoredStage(Stage* monitored) grasp_generator_->setMonitoredStage(monitored); } -void SimpleGrasp::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 SimpleGrasp::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) { + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = link; + tf::poseEigenToMsg(pose, pose_msg.pose); + setIKFrame(pose_msg); } } } }