remove tool_to_grasp_tf from SimpleGrasp

This commit is contained in:
Robert Haschke 2018-03-24 22:09:41 +01:00
parent 54e4d8eed7
commit ff0ee407ff
3 changed files with 17 additions and 15 deletions

View File

@ -46,7 +46,7 @@ int main(int argc, char** argv){
t.add(std::move(initial)); t.add(std::move(initial));
auto grasp_generator = std::make_unique<stages::SimpleGrasp>(); auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
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->setAngleDelta(.2);
grasp_generator->setPreGraspPose("open"); grasp_generator->setPreGraspPose("open");
grasp_generator->setGraspPose("closed"); grasp_generator->setGraspPose("closed");

View File

@ -38,7 +38,7 @@
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <Eigen/Geometry> #include <Eigen/Geometry>
namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } } namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } }
@ -76,14 +76,17 @@ public:
properties().set<std::string>("grasp", grasp); properties().set<std::string>("grasp", grasp);
} }
void setToolToGraspTF(const geometry_msgs::TransformStamped &transform) { void setIKFrame(const geometry_msgs::PoseStamped &transform) {
properties().set("tool_to_grasp_tf", 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 <typename T> template <typename T>
void setToolToGraspTF(const T& t, const std::string& link = "") { void setIKFrame(const T& t, const std::string& link) {
Eigen::Affine3d transform; transform = t; 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) { void setAngleDelta(double angle_delta) {

View File

@ -57,12 +57,12 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
grasp_generator_ = gengrasp.get(); grasp_generator_ = gengrasp.get();
auto ik = std::make_unique<ComputeIK>("compute ik", std::move(gengrasp)); auto ik = std::make_unique<ComputeIK>("compute ik", std::move(gengrasp));
const std::initializer_list<std::string>& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta", "tool_to_grasp_tf" }; const std::initializer_list<std::string>& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta" };
ik->exposePropertiesOfChild(0, grasp_prop_names); ik->exposePropertiesOfChild(0, grasp_prop_names);
insert(std::move(ik)); insert(std::move(ik));
exposePropertiesOfChild(-1, grasp_prop_names); 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<ModifyPlanningScene>("allow object collision"); auto allow_touch = std::make_unique<ModifyPlanningScene>("allow object collision");
@ -124,12 +124,11 @@ void SimpleGrasp::setMonitoredStage(Stage* monitored)
grasp_generator_->setMonitoredStage(monitored); grasp_generator_->setMonitoredStage(monitored);
} }
void SimpleGrasp::setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link) { void SimpleGrasp::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
geometry_msgs::TransformStamped stamped; geometry_msgs::PoseStamped pose_msg;
stamped.header.frame_id = link; pose_msg.header.frame_id = link;
stamped.child_frame_id = "grasp_frame"; tf::poseEigenToMsg(pose, pose_msg.pose);
tf::transformEigenToMsg(transform, stamped.transform); setIKFrame(pose_msg);
setToolToGraspTF(stamped);
} }
} } } } } }