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));
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->setPreGraspPose("open");
grasp_generator->setGraspPose("closed");

View File

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

View File

@ -57,12 +57,12 @@ SimpleGrasp::SimpleGrasp(const std::string& name)
grasp_generator_ = gengrasp.get();
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);
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<ModifyPlanningScene>("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);
}
} } }