mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
renamed GenerateGraspPose::setGraspFrame() to setToolToGraspTF()
This commit is contained in:
parent
0918ad6897
commit
fe05894709
@ -79,9 +79,9 @@ int main(int argc, char** argv){
|
||||
gengrasp->properties().configureInitFrom(Stage::PARENT);
|
||||
gengrasp->setGripperGraspPose("open");
|
||||
gengrasp->setObject("object");
|
||||
gengrasp->setGraspFrame(Eigen::Translation3d(0,0,.05)*
|
||||
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
||||
"lh_tool_frame");
|
||||
gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)*
|
||||
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
||||
"lh_tool_frame");
|
||||
gengrasp->setAngleDelta(-.2);
|
||||
|
||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||
|
||||
@ -79,7 +79,7 @@ int main(int argc, char** argv){
|
||||
gengrasp->setEndEffector("left_gripper");
|
||||
gengrasp->setGripperGraspPose("open");
|
||||
gengrasp->setObject("object");
|
||||
gengrasp->setGraspFrame(Eigen::Translation3d(), "l_gripper_tool_frame");
|
||||
gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
||||
gengrasp->setAngleDelta(.2);
|
||||
|
||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||
|
||||
@ -80,7 +80,7 @@ int main(int argc, char** argv){
|
||||
gengrasp->properties().configureInitFrom(Stage::PARENT);
|
||||
gengrasp->setGripperGraspPose("open");
|
||||
gengrasp->setObject("object");
|
||||
gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||
gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||
gengrasp->setAngleDelta(-.2);
|
||||
|
||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||
|
||||
@ -51,18 +51,16 @@ public:
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
|
||||
void setTimeout(double timeout);
|
||||
|
||||
void setEndEffector(const std::string &eef);
|
||||
void setGripperGraspPose(const std::string &pose_name);
|
||||
void setObject(const std::string &object);
|
||||
|
||||
void setGraspFrame(const geometry_msgs::TransformStamped &transform);
|
||||
void setGraspFrame(const Eigen::Affine3d& transform, const std::string& link = "");
|
||||
void setToolToGraspTF(const geometry_msgs::TransformStamped &transform);
|
||||
void setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link = "");
|
||||
template <typename T>
|
||||
void setGraspFrame(const T& t, const std::string& link = "") {
|
||||
void setToolToGraspTF(const T& t, const std::string& link = "") {
|
||||
Eigen::Affine3d transform; transform = t;
|
||||
setGraspFrame(transform, link);
|
||||
setToolToGraspTF(transform, link);
|
||||
}
|
||||
void setAngleDelta(double delta);
|
||||
|
||||
|
||||
@ -50,7 +50,7 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
|
||||
p.declare<std::string>("eef", "name of end-effector group");
|
||||
p.declare<std::string>("eef_named_pose");
|
||||
p.declare<std::string>("object");
|
||||
p.declare<geometry_msgs::TransformStamped>("grasp_frame", geometry_msgs::TransformStamped(), "robot frame to use for grasping");
|
||||
p.declare<geometry_msgs::TransformStamped>("tool_to_grasp_tf", geometry_msgs::TransformStamped(), "transform from robot tool frame to grasp frame");
|
||||
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
|
||||
}
|
||||
|
||||
@ -72,17 +72,16 @@ void GenerateGraspPose::setObject(const std::string &object){
|
||||
setProperty("object", object);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setGraspFrame(const geometry_msgs::TransformStamped &frame){
|
||||
setProperty("grasp_frame", frame);
|
||||
void GenerateGraspPose::setToolToGraspTF(const geometry_msgs::TransformStamped &transform){
|
||||
setProperty("tool_to_grasp_tf", transform);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setGraspFrame(const Eigen::Affine3d &transform, const std::string &link)
|
||||
{
|
||||
geometry_msgs::TransformStamped frame;
|
||||
frame.header.frame_id = link;
|
||||
frame.child_frame_id = "grasp_frame";
|
||||
tf::transformEigenToMsg(transform, frame.transform);
|
||||
setGraspFrame(frame);
|
||||
void GenerateGraspPose::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 GenerateGraspPose::setAngleDelta(double delta){
|
||||
@ -106,7 +105,7 @@ bool GenerateGraspPose::compute(){
|
||||
robot_state.setToDefaultValues(jmg , eef_named_pose);
|
||||
}
|
||||
|
||||
geometry_msgs::TransformStamped grasp_frame = props.get<geometry_msgs::TransformStamped>("grasp_frame");
|
||||
geometry_msgs::TransformStamped grasp_frame = props.get<geometry_msgs::TransformStamped>("tool_to_grasp_tf");
|
||||
const std::string &link_name = jmg ->getEndEffectorParentGroup().second;
|
||||
if (grasp_frame.header.frame_id.empty()) // if no frame_id is given
|
||||
grasp_frame.header.frame_id = link_name; // interpret the transform w.r.t. eef link frame
|
||||
|
||||
@ -41,7 +41,7 @@ int main(int argc, char** argv){
|
||||
st->setEndEffector("gripper");
|
||||
st->setObject("object");
|
||||
st->setAngleDelta(0.1);
|
||||
st->setGraspFrame(Eigen::Translation3d(.03,0,0));
|
||||
st->setToolToGraspTF(Eigen::Translation3d(.03,0,0));
|
||||
|
||||
t.add(std::move(st));
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user