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.

Sign up for GitHub

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

Make pregrasp pose optional in GenerateGraspPose #157

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
990ec26
Make pregrasp pose optional in GenerateGraspPose
henningkayser Apr 10, 2020
3c6dfe2
Improve 'pregrasp' parameter handling in GenerateGraspPose
henningkayser Apr 14, 2020
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
2 changes: 1 addition & 1 deletion core/include/moveit/task_constructor/stages/generate_grasp_pose.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ class GenerateGraspPose : public GeneratePose
void setAngleDelta(double delta) { setProperty("angle_delta", delta); }

void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }
void setPreGraspPose(const sensor_msgs::JointState& pregrasp) { properties().set("pregrasp", pregrasp); }
void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); }
void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); }

Expand Down
55 changes: 46 additions & 9 deletions core/src/stages/generate_grasp_pose.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,13 +78,41 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) {
const std::string& eef = props.get<std::string>("eef");
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);
else if (props.property("pregrasp").defined()) {
// if pregrasp pose is defined, check if it's valid
const moveit::core::JointModelGroup* eef_jmg = robot_model->getEndEffector(eef);
const boost::any& pregrasp_prop = props.get("pregrasp");
if (pregrasp_prop.type() == typeid(std::string)) {
// check if the specified pregrasp pose is a valid named target
const auto& pregrasp_name = boost::any_cast<std::string>(pregrasp_prop);
std::map<std::string, double> m;
if (!eef_jmg->getVariableDefaultPositions(pregrasp_name, m))
errors.push_back(*this, "pregrasp is set to unknown end effector pose: " + pregrasp_name);
Copy link
Contributor

Choose a reason for hiding this comment

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

If you lookup the named target anyway, you might as well save the result for the compute call to avoid branching on the any again there.

} else if (pregrasp_prop.type() == typeid(sensor_msgs::JointState)) {
// check if the specified pregrasp pose is a valid named target
Copy link
Contributor

Choose a reason for hiding this comment

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

The comment does not make sense.

const auto& pregrasp_state = boost::any_cast<sensor_msgs::JointState>(pregrasp_prop);
if (pregrasp_state.name.size() == pregrasp_state.position.size() &&
pregrasp_state.name.size() == pregrasp_state.velocity.size() &&
pregrasp_state.name.size() == pregrasp_state.effort.size()) {
Copy link
Contributor

Choose a reason for hiding this comment

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

Why would you require velocity/effort to be set?
Positions suffice, though dynamics might be specified to define the dynamic grasp behavior.

Copy link
Contributor

Choose a reason for hiding this comment

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

Currently, the code only uses positions. Dynamic grasp behaviour comes into play if a trajectory would be specified. That's not the case.

// We only apply the joint state for for joints of the end effector
Copy link
Contributor

Choose a reason for hiding this comment

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

Suggested change
// We only apply the joint state for for joints of the end effector
// We only apply the joint state for joints of the end effector

sensor_msgs::JointState eef_state;
eef_state.header = pregrasp_state.header;
for (size_t i = 0; i < pregrasp_state.name.size(); ++i) {
if (eef_jmg->hasJointModel(pregrasp_state.name[i])) {
eef_state.name.push_back(pregrasp_state.name[i]);
eef_state.position.push_back(pregrasp_state.position[i]);
eef_state.velocity.push_back(pregrasp_state.velocity[i]);
eef_state.effort.push_back(pregrasp_state.effort[i]);
}
}
if (eef_state.name.empty())
errors.push_back(*this, "pregrasp JointState doesn't contain joint values for end effector group");
else
properties().set("pregrasp_state", eef_state); // Override property with filtered joint state
Copy link
Contributor

Choose a reason for hiding this comment

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

I'll leave it to Robert to comment on whether changing properties dynamically is a good idea.
I would consider adding a private member variable instead.

Copy link
Contributor

Choose a reason for hiding this comment

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

Fine with me.

} else {
errors.push_back(*this, "pregrasp JointState is malformed - size mismatch between joint names and values");
}
}
}

if (errors)
Expand Down Expand Up @@ -122,8 +150,17 @@ 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 target or joint state if defined
const boost::any& pregrasp_prop = props.get("pregrasp");
if (!pregrasp_prop.empty()) {
robot_state::RobotState& current_state = scene->getCurrentStateNonConst();
if (pregrasp_prop.type() == typeid(std::string)) {
current_state.setToDefaultValues(jmg, boost::any_cast<std::string>(pregrasp_prop));
} else if (pregrasp_prop.type() == typeid(sensor_msgs::JointState)) {
const auto& pregrasp_state = boost::any_cast<sensor_msgs::JointState>(pregrasp_prop);
current_state.setVariablePositions(pregrasp_state.name, pregrasp_state.position);
}
}

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