# Towards Open-Set NLP-Based Multi-Level Planning for Robotic Tasks
## 4 Implementation
### Perception
#### 4.3.3 Object Detection and Grasp Pose Estimation  
For direct grasp inference on RGB-D data (**the 2.5d case**), the system relies on GG-CNN [44], a direct RGB-D to grasp model, as this was found to offer a good compromise between grasp quality, performance on available hardware and ease of integration. The demonstrator estimates the grasp pose once before executing a grasping attempt. After the desired object in the scene is identified through the text query, a 400-by-400 bounding box is created around the highlighted region. The corresponding region is cropped out of the depth image and passed to GG-CNN (GG-CNN requires a 300-by-300 image, so it  
Figure 4: A scene in the reference data set (see Section 5.2) (**I**) Image of the scene. (**II**) Vector octree map reconstruction at a resolution of 0.01 m, colored by similarity to the query _“scotch tape”_, with a grasp pose estimate indicated by a shifted coordinate axes markers. (**III**) Manually annotated reference grasps. Center (red cross) and direction (blue asterisk) indicators drawn as seen by the user of the data set tagging utility. (**IV**) A visualization showing a semantic map grasp pose estimate and the corresponding ground-truth annotation. is scaled down). The model returns the grasping pose estimation at the most promising point. The point's coordinates are converted from pixel-based to metric in the robot frame of reference and passed to the robot controller for execution. Because the depth map may contain \"NaN\" (not a number) values, the system can find the nearest non-NaN value to take as the grasping point instead. **With the 3D voxel map** the general approach for finding objects using a latent space embedding consists of three steps:  
1. A filtered point cloud at the desired resolution is obtained by iterating over the vectree nodes and rejecting points based on a dot product similarity threshold with the query embedding;
2. Clusters are found in the filtered pointcloud using _DBScan_[45] with manually tuned parameters;
3. Cluster parameters such as the centroid, object-aligned bounding box, point count, etc., are computed and used for scoring. Tunable parameters include the nonlinearities (currently, the dot products are normalized to the range \([0,1]\) and raised to some even power) and thresholding applied to the dot product similarity metric; the _DBScan_ parameters \(\epsilon\) and _min_points_; bounding box size, centroid placement, point count constraints for rejection of invalid clusters. While fully general 6 DoF grasp pose estimation is a complex task necessitating involved machine learning-based approaches [46], the use case selected for this demonstrator allows for signficant simplifying assumptions and therefore enables the direct use of outputs from the object finding algorithm for grasp estimation. Namely, all objects are assumed to have bounding boxes that are graspable by the robot gripper, and all grasps are assumed to be vertical. The grasp pose \(T_{grasp}=(R,\mathbf{t})\) is obtained by first setting the grasp position \(\mathbf{t}\) as equal to the bounding box center. Then, for obtaining the rotation, the longest oriented axis projection on the workspace \(XY\) plane is identified as  
\[\mathbf{p}=\max_{i\in\{x,y,\
eq\}}\left\| \begin{bmatrix}1&0&0\\ 0&1&0\end{bmatrix} *R_{bbox}*\mathbf{e}_{i}\right\|= \begin{bmatrix}p_{x}\\ p_{y}\end{bmatrix}  \tag{4}\]  
where \(\mathbf{e}_{x},\mathbf{e}_{y},\mathbf{e}_{z}\)--bounding box extent vectors along each axis and \(R_{bbox}\)--the oriented bounding box rotation matrix. By the convention used, the gripper actuation direction is the tool frame \(x\) axis, meaning the corresponding rotation matrix is given by  
\[\theta=\arctan 2\left(\frac{p_{y}}{p_{x}}\right)-\frac{\pi}{2} \tag{5}\]  
\[R_{grasp}= \begin{bmatrix}\cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\ 0&0&1\end{bmatrix}