# High-Speed Autonomous Robotic Assembly Using In-Hand Manipulation and Re-Grasping
## 5 Motion Planning
### Arm and Gripper Motion Planning  
After we get the optimal move sequence which consists of pickup grasp pose and release grasp pose for each block, we need to plan the motions of the robotic manipulator and gripper to realize the movement. For a physical robot, the motion control is handled by the provided motion API, which takes the Cartesian and joint space waypoints and maximum speed and acceleration parameters to plan the arm motion. For simulated robot which does not have such API available, we use an additional control layer that uses analytical inverse kinematics of the robot to move the simulated robotic arm in Cartesian and joint spaces. For arm motion planning, the most important thing is to make sure that the arm trajectory does not make the arm collide with the environment or itself. Motion planning for collision avoidance is an active area of research [21, 22], especially an additional sensor can detect obstacles in the workspace [23, 24, 25, 26]. Instead of using a full-blown path planning to avoid possible collision, we separate the task space into two regions and use  
Figure 11: Orientation families and re-grasping. Unstable configuration is shown as transparent blocks: (**a**) 4 initial block orientations rotationally symmetric about \(Z\)-axis; (**b**) orientations acquired by a \(90^{\circ}\) rotation about \(X\)-axis; (**c**) orientations acquired by a \(-90^{\circ}\) rotation about \(X\)-axis; (**d**) orientations acquired by a \(90^{\circ}\) rotation about \(Y\)-axis; (**e**) orientations acquired by a \(-90^{\circ}\) rotation about \(Y\)-axis; and (**f**) target block orientations which cannot be directly reached by a \(90^{\circ}\) or \(-90^{\circ}\) rotation about \(X\)-axis or \(Y\)-axis. different control strategy for each region to achieve collision-free arm motion without lengthy optimization. * When the end effector is close to the support surface, for example during picking up and releasing a block, we only use vertical linear motions in Cartesian space, which guarantees that the gripper fingers do not touch other blocks thanks to the vertical pickup constraint we use for grasp planning. To make sure that the manipulator does not encounter singularity during the Cartesian space motion, we use the simulated environment to test all possible pickup and grasp motions at the designated areas. * When the end effector has enough clearance from the support surface, we use the joint level movement using the analytic inverse kinematics of the manipulator to quickly move the arm to the target pose. To change the end effector orientation among perception, pickup and release poses, we use a set of arm joint angles where the 5 th joint of the manipulator can be freely rotated without collision, which we call the safe joint angles. The 2 nd, 3 rd, 4 th and 6 th joints are first moved to the safe joint angles, and then the 5 th joint is moved to the reach the safe joint angles without self-collision. The joint movements are done in reverse to reach target joint angles from safe joint angles. This process is shown in Figure 12.
* When a 2 DOF gripper is used, the additional DOF can be used for the pitch control, and we can keep the manipulator end-effector pitch angle to be zero. In that case, the direct joint space motion between two poses is guaranteed to have no self-collision for the workspace we assume and we skip the motion going through the safe joint angles to save time. Once we generate motion plans in the Cartesian and joint space, we integrate multiple motions into one continuous motion with multiple waypoints, so that the manipulator executes consecutive motions without stopping in between. Figure 13 shows snapshots of such a continuous motion which includes picking up the block, moving and rotating the block to the target pose and placing the block to the final pose. Figure 12: Collision free arm motion planning using the safe joint angles.