void GraspSet::evaluateHypotheses(const PointList& point_list, const LocalFrame& local_frame)
{
hands_.resize(angles_.size());
sample_ = local_frame.getSample();
is_valid_ = Eigen::Array<bool, 1, Eigen::Dynamic>::Constant(1, angles_.size(), false);
FingerHand finger_hand(hand_geometry_.finger_width_, hand_geometry_.outer_diameter_, hand_geometry_.depth_);
Eigen::Matrix3d rot_binormal;
// Set the lateral and forward axis of the robot hand frame (closing direction and grasp approach direction).
if (rotation_axis_ == ROTATION_AXIS_CURVATURE_AXIS)
{
finger_hand.setLateralAxis(1);
finger_hand.setForwardAxis(0);
// Rotation about binormal by 180 degrees (reverses direction of normal)
rot_binormal << -1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0;
}
// Local reference frame
Eigen::Matrix3d local_frame_mat;
local_frame_mat << local_frame.getNormal(), local_frame.getBinormal(), local_frame.getCurvatureAxis();
// Evaluate grasp at each hand orientation.
for (int i = 0; i < angles_.rows(); i++)
{
// Rotation about curvature axis by <angles_(i)> radians
Eigen::Matrix3d rot;
rot << cos(angles_(i)), -1.0 * sin(angles_(i)), 0.0,
sin(angles_(i)), cos(angles_(i)), 0.0,
0.0, 0.0, 1.0;
// Rotate points into this hand orientation.
Eigen::Matrix3d frame_rot;
frame_rot.noalias() = local_frame_mat * rot_binormal * rot;
PointList point_list_frame = point_list.rotatePointList(frame_rot.transpose());
// Crop points on hand height.
PointList point_list_cropped = point_list_frame.cropByHandHeight(hand_geometry_.height_);
// Evaluate finger placements for this orientation.
finger_hand.evaluateFingers(point_list_cropped.getPoints(), hand_geometry_.init_bite_);
// Check that there is at least one feasible 2-finger placement.
finger_hand.evaluateHand();
if (finger_hand.getHand().any())
{
// Try to move the hand as deep as possible onto the object.
int finger_idx = finger_hand.deepenHand(point_list_cropped.getPoints(), hand_geometry_.init_bite_, hand_geometry_.depth_);
// Calculate points in the closing region of the hand.
std::vector<int> indices_closing = finger_hand.computePointsInClosingRegion(point_list_cropped.getPoints(), finger_idx);
if (indices_closing.size() == 0)
{
continue;
}
// create the grasp hypothesis
Grasp hand = createHypothesis(local_frame.getSample(), point_list_cropped, indices_closing, frame_rot,
finger_hand);
hands_[i] = hand;
is_valid_[i] = true;
}
}
}