# Manipulation Planning for Large Objects through Pivoting, Tumbling, and Regrasping
## 5 Hierarchical Manipulation Planning
### Motion Level Planning
#### 5.2.1 Pivoting  
Pivoting motion is designed by raising the object up on a vertex, rotating it around the vertex, and placing it down. A property of pivoting motion is that two object poses must share the same rotational vertex. Figure 8 displays 2D examples in which pivoting gait is used to move an object in the direction indicated by arrows. In Figure 6, the contact point between the object and table is denoted by \(p_{0}\). Here, we assume point contact with friction at each contact point. Due to contact with table at a fixed vertex (\(p_{0}\)), the object motion is constrained as follows:  
\[SD_{B0} \begin{bmatrix}\dot{p}_{B}\\ \omega_{B}\end{bmatrix} =S \begin{bmatrix}\dot{p}_{0}\\ \omega_{0}\end{bmatrix} =o. \tag{5}\]  
Contact between the object and \(i\)-th EEF (\(i=1,2\)) is constrained as follows:  
\[SD_{Bi} \begin{bmatrix}\dot{p}_{B}\\ \omega_{B}\end{bmatrix} =SD_{Hi} \begin{bmatrix}\dot{p}_{Hi}\\ \omega_{Hi}\end{bmatrix} =\dot{p}_{i}, \tag{6}\]  
where \(S\) is a selection matrix that selects the linear velocity and \(D_{Bi}\) transforms the linear and angular velocity from \(\Sigma_{B}\) to \(\Sigma_{W}\). Details on the \(S\) and \(D\) matrices can be found in our  
Figure 11: (**a**) Portion of the graph in the rectangle depicted in Figure 10b. The nodes within the light blue circle share the same object pose and each node indicates the grasp configuration for the object pose. Nodes are connected by edges which correspond to primitive motions. (**b**) An object pose and a corresponding grasp pose. (**c**) Two grasp poses can be changed by the yellow edge connecting them. (**d**) The object pose in (**d**) can be changed to (**b**) or (**c**) by tumbling. previous work [3]. The object is accelerated by the force \((f_{1},f_{2})\) applied by the two EEFs. The dynamic of the object's rotational motion can be obtained as  
\[r_{1}\times f_{1}+r_{2}\times f_{2}+r_{com}\times m_{o}g=\mathcal{I}_{o}\dot{ \omega}_{B}+\omega_{B}\times\mathcal{I}_{o}\omega_{B}, \tag{7}\]  
where \(r_{i}=p_{i}-p_{0}\), \((i=1,2)\), and \(r_{com}=p_{com}-p_{0}\) where \(p_{com}\) denotes the position vector of the object's CoM. \(m_{o}\) and \(\mathcal{I}_{o}\) indicate the mass and moment of inertia of the object, respectively. \(\omega_{B}\) and \(\dot{\omega}_{B}\) indicate the angular velocity and angular acceleration of the object in frame \(\Sigma_{B}\), respectively. In the motion level, the robot motions to move the object between the object poses designed in the nodes are generated by MPC with respect to the kinematics (5), (6), and the dynamic (7). Figure 12b depicts an example of the generated motions for pivoting a piano. In MPC, we define the state vector as \(x_{k}=[\Psi_{B}\quad\omega_{B}]_{k}^{\mathrm{T}}\) where \(\Psi_{B}\) indicates the Euler angles of the object in \(\Sigma_{B}\) and select the input vector as \(u_{k}=[f_{1}\quad f_{2}]_{k}^{T}\). During the object's rotation on the vertex \(p_{0}\), the prediction of the state is obtained by,  
\[x_{k+1}=Ax_{k}+Bu_{k}+D, \tag{8}\]  
where \(A,B\), and \(D\) are coefficient matrices defined as  
\[A= \begin{bmatrix}I_{3}&W\\ O_{3}&I_{3}\end{bmatrix} ,\quad B= \begin{bmatrix}\mathcal{I}_{o}^{-1}WT^{2}/2[r_ {1}\times]&\mathcal{I}_{o}^{-1}WT^{2}/2[r_{2}\times]\\ \mathcal{I}_{o}^{-1}T[r_{2}\times]&\mathcal{I}_{o}^{-1}T[r_{2}\times]\end{bmatrix} , \tag{9}\]  
\[D= \begin{bmatrix}\mathcal{I}_{o}^{-1}WT^{2}/2[r_{com}\times]mg\\ \mathcal{I}_{o}^{-1}T[r_{com}\times]mg\end{bmatrix} , \tag{10}\]  
where the matrix \(W\) transforms the angular velocity to the velocity of the Euler angles. The future states and free variables appear in the prediction horizon (\(N_{p}\)) are defined as \(X_{k}=[x_{k},x_{k+1},\cdots,x_{k+n_{p}-1}]^{T}\) and \(U_{k}=[u_{k},u_{k+1},\cdots,u_{k+n_{p}-1}]^{T}\), respectively. The MPC tracks a reference by solving the following optimization problem,  
\[J_{mpc}=\frac{\alpha}{2}\Big{\|}X_{k+1}-X^{ref}\Big{\|}^{2}+\frac{\beta}{2} \|U_{k}\|^{2}, \tag{11}\]  
where \(\alpha\) and \(\beta\) are the weights.