Branch forward: GraspDetector::filterSideGraspsCloseToTable(...)

[hauff]

There might be a bug in branch forward.

• I think you want to use j instead of i in the inner loop.
• Maybe it should be angle < angle_thresh_ instead of angle > angle_thresh_.
• Isn't it easier to compute the dist in the following way? double dist = fabs(hands[j].getGraspTop().z() - table_height_);

std::vector<GraspSet> GraspDetector::filterSideGraspsCloseToTable(const std::vector<GraspSet>& hand_set_list)
{
    ...

    for (int j = 0; j < hands.size(); j++)
    {
        double angle = fabs(vert_axis_vec.transpose() * hands[i].getApproach());
        double dist = fabs((hands[i].getGraspBottom() - APPROACH_LENGTH*hands[i].getApproach())(2)) - table_height_;

        // This is a side grasps that is too close to the table.
        if (angle > angle_thresh_ && dist < table_thresh_)
        {
            is_valid(j) = false;
        }
        else
        {
            is_valid(j) = true;
            remaining++;
        }
    }

    ...
}

Maybe to check if one side of the gripper is under the table helps with detecting side grasps.

double half_width = 0.5 * outer_diameter_;
Eigen::Vector3d left_top = hands[j].getGraspTop() + half_width * hands[j].getBinormal();
Eigen::Vector3d right_top = hands[j].getGraspTop() - half_width * hands[j].getBinormal();

if (left_top.z() < table_height_ || right_top.z() < table_height_)
{
    is_valid(j) = false;
}

[atenpas]

Thanks for your suggestion. That filter was written for a specific case and reference frame. I'm happy to change it if desired ...

[saikishor]

@hauff @atenpas do you mean this kind of issue, could be resolved with that fix?

[hauff]

@saikishor

If you mean the grasp starting below the table surface ... my answer would be:

Yes, I think the fix should resolve it.

[saikishor]

@hauff I think we should make a PR with this fix. I don't know if @atenpas has the time to take a look ober the other PR's

[saikishor]

@hauff thanks for the confirmation :+1:

[hauff]

@saikishor, @atenpas as you suggested, I created a (hopefully correct) PR.

Maybe it will help. :-)

[saikishor]

In this case, aren't you using the angle parameter? Why? @hauff

[hauff]

@saikishor I didn't use the angle because I did not want to disallow a specific approach direction. (like parallel to table surface)

But if I look at your examples, then I see that I missed something. Maybe it is necessary to check weather the GraspBottom is also below the table.

There is a chance that I might have missed something else ?

[saikishor]

I think you have missed something. Because still I have the similar issue.

[hauff]

In the second image it is hard to see. Are there GraspBottom's (or fingertip's) below the table?

[saikishor]

@hauff They are grasp bottoms. Sorry for the unclear image.

[saikishor]

I guess just replacing i by j does the trick. Below are the results.

[saikishor]

moreover, i guess the angle should be greater than anglethreshold, else you will have all the grasp directions below it, thereby more collisions.

[hauff]

Do you filter the cloud before using the GPD package? It looks to me as if the table is not present in the cloud you give to GPD.

[saikishor]

Yes the table is not present in the cloud I give to the GPD. I only pass the table top cloud.

[hauff]

Ah now I understand. I am pretty sure that is the reason why you get grasps starting below the table with an approach vector pointing upwards.

I did not preprocess the cloud myself, instead I used this method from GPD.

void GraspDetector::preprocessPointCloud(CloudCamera& cloud_cam)
{
  candidates_generator_->preprocessPointCloud(cloud_cam);
}

Example:

Eigen::Matrix3Xd view_points(3, 1);
view_points.col(0) = view_point;
CloudCamera cloud_camera(cloud_ptr, 0, view_points);

grasp_detector.preprocessPointCloud(cloud_camera);
grasps = grasp_detector.detectGrasps(cloud_camera);

Should also work, if you launch a tutorial.

[saikishor]

@hauff but how does it change a lot. He is also processing the point cloud and removing the layers until the table.