Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Make pregrasp pose optional in GenerateGraspPose #157

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 15 additions & 9 deletions core/src/stages/generate_grasp_pose.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(nam
p.declare<std::string>("object");
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");

p.declare<boost::any>("pregrasp", "pregrasp posture");
p.declare<boost::any>("pregrasp", std::string(""), "pregrasp posture");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Keep "unset" as default here?

Copy link
Member

@JafarAbdi JafarAbdi Apr 11, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Out of curiosity, is there a reason to declare the pregrasp property as boost::any rather than std::string .? I see the only way it's being used is as std::string

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Very good point. I think, the idea was to enable some message alternatively. But you are right, currently, we only use strings.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

see comment below

p.declare<boost::any>("grasp", "grasp posture");
}

Expand All @@ -79,12 +79,15 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) {
if (!robot_model->hasEndEffector(eef))
errors.push_back(*this, "unknown end effector: " + eef);
else {
// check availability of eef pose
const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
const std::string& name = props.get<std::string>("pregrasp");
std::map<std::string, double> m;
if (!jmg->getVariableDefaultPositions(name, m))
errors.push_back(*this, "unknown end effector pose: " + name);
// if pregrasp pose is defined, check if it's valid
const std::string& pregrasp_name = props.get<std::string>("pregrasp");
if (!pregrasp_name.empty()) {
// check availability of eef pose
std::map<std::string, double> m;
const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
if (!jmg->getVariableDefaultPositions(pregrasp_name, m))
errors.push_back(*this, "unknown end effector pose: " + pregrasp_name);
}
}

if (errors)
Expand Down Expand Up @@ -122,8 +125,11 @@ void GenerateGraspPose::compute() {
const std::string& eef = props.get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);

robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues(jmg, props.get<std::string>("pregrasp"));
// Apply pregrasp pose if defined
if (!props.get<std::string>("pregrasp").empty()) {
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues(jmg, props.get<std::string>("pregrasp"));
}

geometry_msgs::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = props.get<std::string>("object");
Expand Down