renamed GenerateGraspPose::setGraspFrame() to setToolToGraspTF()

This commit is contained in:
Robert Haschke 2018-01-06 10:07:15 +01:00
parent 0918ad6897
commit fe05894709
6 changed files with 20 additions and 23 deletions

View File

@ -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));

View File

@ -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));

View File

@ -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));

View File

@ -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);

View File

@ -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

View File

@ -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));