mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
remove tool_to_grasp_tf from SimpleGrasp
This commit is contained in:
parent
54e4d8eed7
commit
ff0ee407ff
@ -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");
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user