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->properties().configureInitFrom(Stage::PARENT);
|
||||||
gengrasp->setGripperGraspPose("open");
|
gengrasp->setGripperGraspPose("open");
|
||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setGraspFrame(Eigen::Translation3d(0,0,.05)*
|
gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)*
|
||||||
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
||||||
"lh_tool_frame");
|
"lh_tool_frame");
|
||||||
gengrasp->setAngleDelta(-.2);
|
gengrasp->setAngleDelta(-.2);
|
||||||
|
|
||||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
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->setEndEffector("left_gripper");
|
||||||
gengrasp->setGripperGraspPose("open");
|
gengrasp->setGripperGraspPose("open");
|
||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setGraspFrame(Eigen::Translation3d(), "l_gripper_tool_frame");
|
gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
||||||
gengrasp->setAngleDelta(.2);
|
gengrasp->setAngleDelta(.2);
|
||||||
|
|
||||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
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->properties().configureInitFrom(Stage::PARENT);
|
||||||
gengrasp->setGripperGraspPose("open");
|
gengrasp->setGripperGraspPose("open");
|
||||||
gengrasp->setObject("object");
|
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);
|
gengrasp->setAngleDelta(-.2);
|
||||||
|
|
||||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||||
|
|||||||
@ -51,18 +51,16 @@ public:
|
|||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
|
|
||||||
void setTimeout(double timeout);
|
|
||||||
|
|
||||||
void setEndEffector(const std::string &eef);
|
void setEndEffector(const std::string &eef);
|
||||||
void setGripperGraspPose(const std::string &pose_name);
|
void setGripperGraspPose(const std::string &pose_name);
|
||||||
void setObject(const std::string &object);
|
void setObject(const std::string &object);
|
||||||
|
|
||||||
void setGraspFrame(const geometry_msgs::TransformStamped &transform);
|
void setToolToGraspTF(const geometry_msgs::TransformStamped &transform);
|
||||||
void setGraspFrame(const Eigen::Affine3d& transform, const std::string& link = "");
|
void setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link = "");
|
||||||
template <typename T>
|
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;
|
Eigen::Affine3d transform; transform = t;
|
||||||
setGraspFrame(transform, link);
|
setToolToGraspTF(transform, link);
|
||||||
}
|
}
|
||||||
void setAngleDelta(double delta);
|
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", "name of end-effector group");
|
||||||
p.declare<std::string>("eef_named_pose");
|
p.declare<std::string>("eef_named_pose");
|
||||||
p.declare<std::string>("object");
|
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)");
|
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -72,17 +72,16 @@ void GenerateGraspPose::setObject(const std::string &object){
|
|||||||
setProperty("object", object);
|
setProperty("object", object);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setGraspFrame(const geometry_msgs::TransformStamped &frame){
|
void GenerateGraspPose::setToolToGraspTF(const geometry_msgs::TransformStamped &transform){
|
||||||
setProperty("grasp_frame", frame);
|
setProperty("tool_to_grasp_tf", transform);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setGraspFrame(const Eigen::Affine3d &transform, const std::string &link)
|
void GenerateGraspPose::setToolToGraspTF(const Eigen::Affine3d &transform, const std::string &link){
|
||||||
{
|
geometry_msgs::TransformStamped stamped;
|
||||||
geometry_msgs::TransformStamped frame;
|
stamped.header.frame_id = link;
|
||||||
frame.header.frame_id = link;
|
stamped.child_frame_id = "grasp_frame";
|
||||||
frame.child_frame_id = "grasp_frame";
|
tf::transformEigenToMsg(transform, stamped.transform);
|
||||||
tf::transformEigenToMsg(transform, frame.transform);
|
setToolToGraspTF(stamped);
|
||||||
setGraspFrame(frame);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setAngleDelta(double delta){
|
void GenerateGraspPose::setAngleDelta(double delta){
|
||||||
@ -106,7 +105,7 @@ bool GenerateGraspPose::compute(){
|
|||||||
robot_state.setToDefaultValues(jmg , eef_named_pose);
|
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;
|
const std::string &link_name = jmg ->getEndEffectorParentGroup().second;
|
||||||
if (grasp_frame.header.frame_id.empty()) // if no frame_id is given
|
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
|
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->setEndEffector("gripper");
|
||||||
st->setObject("object");
|
st->setObject("object");
|
||||||
st->setAngleDelta(0.1);
|
st->setAngleDelta(0.1);
|
||||||
st->setGraspFrame(Eigen::Translation3d(.03,0,0));
|
st->setToolToGraspTF(Eigen::Translation3d(.03,0,0));
|
||||||
|
|
||||||
t.add(std::move(st));
|
t.add(std::move(st));
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user