Skip to content

Commit

Permalink
adding more grasp candidate config parameters (moveit#4)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mlautman
mlautman authored Sep 5, 2018
1 parent 5fdcc64 commit 6273793
Show file tree
Hide file tree
Showing 3 changed files with 75 additions and 58 deletions.
37 changes: 31 additions & 6 deletions include/moveit_grasps/grasp_generator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,32 +101,55 @@ struct GraspCandidateConfig
, enable_face_grasps_(false)
, enable_variable_angle_grasps_(false)
, enable_edge_grasps_(false)
, generate_x_axis_grasps_(false)
, generate_y_axis_grasps_(false)
, generate_z_axis_grasps_(false)
{
}
void enableAll()
void enableAllGraspTypes()
{
enable_corner_grasps_ = true;
enable_face_grasps_ = true;
enable_variable_angle_grasps_ = true;
enable_edge_grasps_ = true;
}
void disableAll()
void enableAllGraspAxes()
{
generate_x_axis_grasps_ = true;
generate_y_axis_grasps_ = true;
generate_z_axis_grasps_ = true;
}
void enableAll()
{
enableAllGraspTypes();
enableAllGraspAxes();
}
void disableAllGraspTypes()
{
enable_corner_grasps_ = false;
enable_face_grasps_ = false;
enable_variable_angle_grasps_ = false;
enable_edge_grasps_ = false;
}
void onlyEdgeGrasps()
void disableAllGraspAxes()
{
disableAll();
enable_edge_grasps_ = true;
generate_x_axis_grasps_ = false;
generate_y_axis_grasps_ = false;
generate_z_axis_grasps_ = false;
}
void disableAll()
{
disableAllGraspTypes();
disableAllGraspAxes();
}

bool enable_corner_grasps_;
bool enable_face_grasps_;
bool enable_variable_angle_grasps_;
bool enable_edge_grasps_;
bool generate_x_axis_grasps_;
bool generate_y_axis_grasps_;
bool generate_z_axis_grasps_;
};

// Class
Expand Down Expand Up @@ -160,11 +183,13 @@ class GraspGenerator
* \param width length of cuboid along local y-axis
* \param height length of cuboid along local z-axis
* \param grasp_data data describing end effector
* \param grasp_candidate_config parameter for selectively enabling and disabling different grasp types
* \param grasp_candidates possible grasps generated
* \return true if successful
*/
bool generateGrasps(const Eigen::Affine3d& cuboid_pose, double depth, double width, double height,
const GraspDataPtr grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates);
const GraspDataPtr grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates,
const GraspCandidateConfig grasp_candidate_config = GraspCandidateConfig());

/**
* \brief Create grasp positions around one axis of a cuboid
Expand Down
56 changes: 33 additions & 23 deletions src/grasp_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -863,44 +863,54 @@ double GraspGenerator::scoreGrasp(const Eigen::Affine3d& grasp_pose, const Grasp

bool GraspGenerator::generateGrasps(const Eigen::Affine3d& cuboid_pose, double depth, double width, double height,
const moveit_grasps::GraspDataPtr grasp_data,
std::vector<GraspCandidatePtr>& grasp_candidates)
std::vector<GraspCandidatePtr>& grasp_candidates,
const GraspCandidateConfig grasp_candidate_config)
{
// Generate grasps over axes that aren't too wide to grip

// Most default type of grasp is X axis
GraspCandidateConfig grasp_candidate_config;
if (depth <= grasp_data->max_grasp_width_) // depth = size along x-axis
GraspCandidateConfig grasp_candidate_config_copy(grasp_candidate_config);

if (grasp_candidate_config_copy.generate_x_axis_grasps_)
{
ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around x-axis of cuboid");
grasp_candidate_config.enableAll();
if (depth > grasp_data->max_grasp_width_) // depth = size along x-axis
{
grasp_candidate_config_copy.disableAllGraspTypes();
grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_;
grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_;
}
generateCuboidAxisGrasps(cuboid_pose, depth, width, height, X_AXIS, grasp_data, grasp_candidate_config_copy,
grasp_candidates);
}
else
grasp_candidate_config.onlyEdgeGrasps();

generateCuboidAxisGrasps(cuboid_pose, depth, width, height, X_AXIS, grasp_data, grasp_candidate_config,
grasp_candidates);

if (width <= grasp_data->max_grasp_width_) // width = size along y-axis
grasp_candidate_config_copy = grasp_candidate_config;
if (grasp_candidate_config_copy.generate_y_axis_grasps_)
{
ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around y-axis of cuboid");
grasp_candidate_config.enableAll();
if (width > grasp_data->max_grasp_width_) // width = size along y-axis
{
grasp_candidate_config_copy.disableAllGraspTypes();
grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_;
grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_;
}
generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Y_AXIS, grasp_data, grasp_candidate_config_copy,
grasp_candidates);
}
else
grasp_candidate_config.onlyEdgeGrasps();

generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Y_AXIS, grasp_data, grasp_candidate_config,
grasp_candidates);

if (height <= grasp_data->max_grasp_width_) // height = size along z-axis
grasp_candidate_config_copy = grasp_candidate_config;
if (grasp_candidate_config_copy.generate_z_axis_grasps_)
{
ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around z-axis of cuboid");
grasp_candidate_config.enableAll();
if (height > grasp_data->max_grasp_width_) // height = size along z-axis
{
grasp_candidate_config_copy.disableAllGraspTypes();
grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_;
grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_;
}
generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Z_AXIS, grasp_data, grasp_candidate_config_copy,
grasp_candidates);
}
else
grasp_candidate_config.onlyEdgeGrasps();

generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Z_AXIS, grasp_data, grasp_candidate_config,
grasp_candidates);

if (!grasp_candidates.size())
ROS_WARN_STREAM_NAMED("grasp_generator", "Generated 0 grasps");
Expand Down
40 changes: 11 additions & 29 deletions src/grasp_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,11 +85,7 @@ bool GraspPlanner::planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& gr
}
else
{
//++grasp_it; // move to next grasp

// Once we have one valid path, just quit so we can use that one
ROS_INFO_STREAM_NAMED("grasp_planner", "Valid grasp plan generated");
break;
++grasp_it; // move to next grasp
}

if (verbose_cartesian_filtering)
Expand Down Expand Up @@ -261,9 +257,6 @@ bool GraspPlanner::computeCartesianWaypointPath(const moveit::core::JointModelGr
return false;
}

// Results
double last_valid_percentage;

std::size_t attempts = 0;
static const std::size_t MAX_IK_ATTEMPTS = 5;
bool valid_path_found = false;
Expand All @@ -288,37 +281,26 @@ bool GraspPlanner::computeCartesianWaypointPath(const moveit::core::JointModelGr
// Compute Cartesian Path
segmented_cartesian_traj.clear();
segmented_cartesian_traj.resize(3);
last_valid_percentage = start_state_copy.computeCartesianPath(
double valid_approach_percentage = start_state_copy.computeCartesianPath(
arm_jmg, segmented_cartesian_traj[APPROACH], ik_tip_link, waypoints[APPROACH], global_reference_frame, max_step,
jump_threshold, constraint_fn, kinematics::KinematicsQueryOptions());

last_valid_percentage *= start_state_copy.computeCartesianPath(
double valid_lift_retreat_percentage = start_state_copy.computeCartesianPath(
arm_jmg, segmented_cartesian_traj[LIFT], ik_tip_link, waypoints[LIFT], global_reference_frame, max_step,
jump_threshold, constraint_fn, kinematics::KinematicsQueryOptions());

last_valid_percentage *= start_state_copy.computeCartesianPath(
valid_lift_retreat_percentage *= start_state_copy.computeCartesianPath(
arm_jmg, segmented_cartesian_traj[RETREAT], ik_tip_link, waypoints[RETREAT], global_reference_frame, max_step,
jump_threshold, constraint_fn, kinematics::KinematicsQueryOptions());

ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints",
"Cartesian last_valid_percentage: " << last_valid_percentage
<< " number of segments in trajectory: "
<< segmented_cartesian_traj.size());
ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "valid_approach_percentage: " << valid_approach_percentage
<< " \tvalid_lift_retreat_"
"percentage: "
<< valid_lift_retreat_percentage);

double min_allowed_valid_percentage = 0.9;
if (last_valid_percentage == 0)
{
ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Failed to computer cartesian path: last_valid_percentage is "
"0");
}
else if (last_valid_percentage < min_allowed_valid_percentage)
{
ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints",
"Resulting cartesian path is less than "
<< min_allowed_valid_percentage
<< " % of the desired distance, % valid: " << last_valid_percentage);
}
else
// The retreat has to work for the most part but doesn't need to be perfect
double min_allowed_valid_lift_retreat_percentage = 0.90;
if (valid_approach_percentage == 1 && valid_lift_retreat_percentage >= min_allowed_valid_lift_retreat_percentage)
{
ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Found valid cartesian path");
valid_path_found = true;
Expand Down

0 comments on commit 6273793

Please sign in to comment.