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));
|
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");
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user