# High-Speed Autonomous Robotic Assembly Using In-Hand Manipulation and Re-Grasping
## 5 Motion Planning
### Grasp Planning  
Once an assembly sequence is found for an assembly solution, we can iteratively pick up each block from its initial position, rotate it to its final orientation and finally move it to its final position to assemble the final shape from blocks. To accomplish such actions using robotic manipulator and gripper, we need to find a stable grasp for the block and plan the manipulator motion sequence that moves the block from the initial pose to the final pose without a collision. As finding a stable grasp for arbitrary shaped object is a hard problem under active research [18; 19; 20], we use the following additional constraints to make the search space tractable:  
* The \(Z\)-axis of the pickup grasp pose should always be parallel to the \(Z\)-axis of the block pose (vertical pickup constraint, as shown in Figure 9a). * The \(Z\)-axis of the release grasp pose should be either parallel to the \(Z\)-axis of the build surface (vertical release case, shown in Figure 9b) or perpendicular to the \(Z\)-axis of the build surface (horizontal release case, shown in Figure 9c). * The \(Y\)-axis of the pickup and release grasp pose is either parallel to the \(X\)-axis of the block (\(X\)-axis grasp) or the \(Y\)-axis of the block (\(Y\)-axis grasp). * The tip positions of the gripper fingers should be aligned with the centers of the unit building cubes each block is composed of. Figure 8: Example of assembly sequence planning. As grasp pose relative to the block is fixed during the manipulation, we only need to determine the release grasp pose, and then the pickup grasp pose can be automatically found. Figure 10c,d shows all collision-free \(Y\)-axis and \(X\)-axis release gripper pose candidates for target block pose shown in Figure 10b. However, not all of them satisfy the vertical pickup constraint and are directly realizable without re-grasping. For example, the release grasp pose shown in Figure 10e results in the lateral pickup grasp pose in Figure 10f, which should be avoided due to the high risk of collision. On the other hand, the release grasp pose shown in Figure 10g results in the vertical pickup grasp pose shown in Figure 10h, which means we can move the block from initial pose to target pose without re-grasping. Figure 10: Grasp Planning process: (**a**) initial block pose; (**b**) target block pose; (**c**) \(Y\)-axis grasp candidates; (**d**) \(X\)-axis grasp candidates; (**e**) infeasible release grasp; (**f**) infeasible pickup grasp; (**g**) feasible release grasp; and (**h**) feasible pickup grasp. Figure 9: Pickup and release constraints: (**a**) vertical pickup; (**b**) vertical release; and (**c**) lateral release. To find feasible grasp satisfying all the constraints, we use the grasp planning algorithm shown in Algorithm 3, For each block, we enumerate all collision free release grasp pose candidates in \(X\)-axis and \(Y\)-axis first, and then calculate the pickup grasp poses for each release grasp poses using the relative transform of the block orientations. Due to symmetry, some blocks can have identical block configurations for multiple different orientations.We pre-calculate the transform results of all 24 each orientations for each block and put together orientations that result in the same configuration. Based on this information, we build the transform list, which contains all possible transforms for each block that transforms from one orientation to another orientation. Then, the transform list is used to calculate all possible pickup grasp poses. Finally, we calculate the Euler angles of pickup grasp pose and calculate the cost of each pickup-release grasp pose pair according to the pickup and release grasp pose pitch angle, relative yaw angle between pickup and release grasp poses and the grasp width. This process is iterated for each block movement in the sequence, and the grasp plan with the lowest cost is selected for each move to form the best move sequence for the solution.