Goal:

To get the normals to the object point cloud so that it conduces the best grasp.

​

Associated steps:

​

​

 

 

 

 

​

 

 

 

Framework representing the steps involved
 

Step1: Assembling the files:
Following the commands, we downloaded the project_env.zip file from Canvas. After extracting it, we had 2 files in it namely: table and vmb_project_env package. We transferred the table file to .gazebo (a hidden file in the home directory) and vbm_project_env to the source directory. After building and launching the simulation.launch.py executable, the gazebo window containing the table and camera popped up. We added a coke can as our object for visualization. We started working on this object and undertook our next steps.

 

Step2: Point cloud generation:

After visualizing the files in Gazebo, we then worked out to write a program that would generate a point cloud of the object-coke can. This was made possible using the pcl library, which is an open-source library helping to generate point clouds.

 

Step3: Filtering and segmentation:

Using the point cloud directly to get a good grasp would never be a good idea. The coke can whose contact points were to be calculated needed to be segregated from the external noise and other conflicting objects. Such objects or shapes include the table plane here. In the real world, there can be many other objects and much more noise. In the simulation, we only had to eliminate the plane for the point cloud to be formed better. For the same, we ran the RANSAC algorithm. This algorithm eliminates the table by using random points for shape generation (here plane) and eliminates the one which crosses the threshold. Thus the table was eliminated and the point cloud of the object was generated. 

 

Step4: Visualizing the point cloud in Rviz2:

Only creating the point cloud doesn’t mean it is visible to the user directly. For this purpose, Rviz is used. Rviz basically subscribes to the topic on which the point cloud will be getting published. This way, the published points will be easily visualized by the user. The main thing to note here was that the generated point cloud shape would depend on the camera position. Since we are creating the 3d point cloud from a 2d frame of the image, we lose the part of the image that isn’t present in the image. We can however later try stitching the image and get a better point cloud for bonus points!

 

Step5: Estimating normals:

In order to get a good grasp, we will need to find the normal vectors that are not only canceling out each other but also are closer to the centroid. First, we need to find the points connecting in the cloud. Later on, these points are used to draw normals. Normals are drawn on each of the points in the point cloud. Basically, the segmentation helped us to define the object normals which we stitched together for each additional view obtained. In stitching these normals together, we were able to use the normals and connect them to each other, which we used to find the most stable grasp options, which was simplified by our use of a 2-DOF gripper, which reduced the number of grasp options and eliminated the need to find the optimal grasp for more than 2 contact points, which increases complexity. This allowed us to connect the lines between these contact points or normals to find the angles closest to 180°, calculated by using the dot product by inputting the i, j, and components of the normals. The angles did not need to be a perfect 180°, but instead we found the angles closest to that value if none were 180°. This ensured accuracy and strength of the grasp, which led to the ideal stability. 

 

Step6: Visualizing grasp points:

 

We visualized the best grasp points in Rviz by setting the color of the wanted grasp points to magenta in the Point Cloud.
 

Step7: Stitching:

After getting the normal vectors from one view of the camera, we moved the camera for another view. Now, in this view, we received a new set of points and a totally new grasp.

So, we stitched the two point clouds and got a 3D image which wasn't the case pre-stitching. After obtaining the 3D image, we ran the algorithms and found a better grasp of the 3D objects with the available 3D view.
 

Results

First we setup the environment, and see the objects in Gazebo:

 

 

 

 

 

 

 

 

 

 

 

 

 

​

 

 

 

 

 

 

Visualization setup in Gazebo world

 

This image shows a table, the cylindrical object (in this case a can of Coca-Cola) we were grasping, as well as the camera on the top right, which was moved around the table to obtain numerous views, in order to implement the active vision. Following this, we implemented segmentation on the view from the camera in order to create views containing only the Coke can, without the table.

 

​

​

​

​

​

​

​

​

​

 

 

 

 

 

 

 

 

 

 

 

 

Object image captured by the camera in Gazebo

 

This is the camera view, and the resulting image captured which was then turned into a point cloud. 

 

​

​

​

​

​

 

 

 

Visualized point cloud in Rviz2

 

Using Rviz, we visualized the coke can. We can see this one side collected here from a singular view as a point cloud.

Distance filtering:

 

Performed distance filtering to remove the gazebo world floor from the point cloud.

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

​

 

 

 

 

 

 

 

Image of the object after removing the gazebo world floor

 

Object segmentation:

Performed Plane model segmentation using RANSAC to estimate the plane. Next, we extracted the outliers to get the point cloud consisting of only the object.

​

 

 

 

 

 

 

 

 

 

 

 

 

 

 

​

 

 

 

 

 

 

 

Segmented image of the object point cloud

 

The coke can is composed of a large number of tiny individual points that when put together, compose one side of the can.

With this information, we were able to find the best grasp for this individual view. 

 

​

​

​

​

 

 

Output of the best grasp detected in the terminal, as well as centroid coordinates

 

 

We found them to be at these coordinate locations. This was calculated using the dot product method to find the scalar distance between these two points in the point cloud, as well as calculating the surface normals of each point. We had to restrict the visible point with the closest proximity to the centroid. Using the centroid distance, we obtained the grasp that was most optimal for this kind of symmetric object. This was achieved by looking at the angles between the surface normals and the distance vector between the two points. If we observed that the angle was either between -5 and 5 degrees, or 175 and 185 degrees, then the surface normals were pointing at each other. This would make the grasp optimal, due to needing less external forces, and us being able to grasp the object from those locations, putting force towards the surface normal, to be able to grasp the object. We calculated by making the following code:

At the beginning of the file we have

// Going from degrees to radians
#define deg2rad 3.14/180

// The angle we use around 0deg, as well as 180deg, to get the grasps we view as good.
#define smallanglecalc 30

// Calculating the angles in radians.
#define smallangle smallanglecalc*deg2rad
#define largeangle1 (180 - smallanglecalc)*deg2rad
#define largeangle2 (180 + smallanglecalc)*deg2rad

 

Then we used those defines in the following code later on, when we try to find our most optimal grasp.

for (auto i = 0; i < cloud_normals->size(); i++)
{

for (auto j = i+1; j < cloud_normals->size(); j++)
{

//RCLCPP_INFO(this->get_logger(), "normal;_x'%lf'\n",cloud_normals->points[j].normal_x);
//getting the normal vectors
      nx1=cloud_normals->points[i].normal_x;
  ny1=cloud_normals->points[i].normal_y;
        nz1=cloud_normals->points[i].normal_z;

        nx2=cloud_normals->points[j].normal_x;
        ny2=cloud_normals->points[j].normal_y;
        nz2=cloud_normals->points[j].normal_z;

        // corresponding points
        x1=(*plane_cloud)[i].x;
        y1=(*plane_cloud)[i].y;
        z1=(*plane_cloud)[i].z;

        x2=(*plane_cloud)[j].x;
        y2=(*plane_cloud)[j].y;
        z2=(*plane_cloud)[j].z;

        //*** Do angle computations here ***//
        rx=x1-x2;
        ry=y1-y2;
        rz=z1-z2;
        mag_r=sqrt(rx*rx+ry*ry+rz*rz);
 

         //Dot_products
        dot=nx1*rx+ny1*ry+nz1*rz;
        mag=sqrt(nx1*nx1+ny1*ny1+nz1*nz1);
        mag=mag*mag_r;
        theta1=acos(dot/mag);

        dot=nx2*rx+ny2*ry+nz2*rz;
        mag=sqrt(nx2*nx2+ny2*ny2+nz2*nz2);
        mag=mag*mag_r;
        theta2=acos(dot/mag);

      double dist1,dist2;
        if ((abs(theta1) < smallangle || (theta1 > largeangle1  && theta1 < largeangle2 )) && (abs(theta2) < smallangle  || (theta2 > largeangle1  && theta2 < largeangle2 )) && theta1 != theta2)
        {
      //RCLCPP_INFO(this->get_logger(), "Good Grasp !! \n");
      //RCLCPP_INFO(this->get_logger(), "Grasp Points: \n");
      //RCLCPP_INFO(this->get_logger(), "x1='%lf' y1='%lf' z1='%lf' \n",x1,y1,z1);
      //RCLCPP_INFO(this->get_logger(), "x2='%lf' y2='%lf' z2='%lf' \n",x2,y2,z2);

      //calculating distance of grasp points from the centroid 
      dist1 = sqrt((centroid_x-x1)*(centroid_x-x1) + (centroid_y-y1)*(centroid_y-y1) + (centroid_z-z1)*(centroid_z-z1));
          dist2 = sqrt((centroid_x-x2)*(centroid_x-x2) + (centroid_y-y2)*(centroid_y-y2) + (centroid_z-z2)*(centroid_z-z2));

      //choosing points with minimum distance to center as best grasp point
        if (dist1+dist2<= min)
        {
            min = dist1+dist2;
            indexi = i;
          indexj = j;
        }


        }

 

 

So, now we use these indexes to set the points in the pointcloud to magenta.

 

(*plane_cloud)[indexi].r = 255;
(*plane_cloud)[indexi].g = 0;
(*plane_cloud)[indexi].b = 255;
 

(*plane_cloud)[indexj].r = 255;
(*plane_cloud)[indexj].g = 0;
(*plane_cloud)[indexj].b = 255;

 

Which we then publish.

 

Now that we have found good normals for our analysis, we can move forwards to visualizing where exactly on the object we should grasp.

 

​

​

​

​

​

​

 

​

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

Points conducing the best grasp on the point cloud

 

We are visualizing the grasp here using the magenta color, due to it being very distinct and easy to see. With this object, we can now see where the best grasp would be located.

 

The best grasp points are found to be closer to the top of the coke can as the top surface of the can is included in the point cloud. This pulls the centroid point of the captured point cloud to the top. As a result the best grasp is located more at the top than the geometric center.