## 4 Trajectory Planning and Tracking Methods  
In Section 3, the two-arm constraint in the grinding scenario is described, and the respective motion trajectories of the two arms need to be designed under this constraint. In order to reduce the difficulty of planning the respective motion trajectories of the two arms, here we developed a design to decompose the machining trajectories to the two mechanical arms in different directions according to the shape of the workpiece surface. When planning a machining trajectory on the surface of a workpiece, the surface of the workpiece is divided into planar slices along the X-direction of the world coordinate system, and curves in YZ-coordinates are created along the surface of the workpiece in each plane. The curve trajectory is decomposed along the Y-direction and Z-direction, viz:  
\[ \begin{array}{c}{}_{YZ}^{\rightarrow}={}_{Y}^{\rightarrow}+{}_{Z}^{ \rightarrow}\\ \end{array}  \tag{8}\]where \(\overrightarrow{P_{YZ}}\) is the trajectory curve, \(\overrightarrow{P_{Y}}\) is the trajectory in the Y-direction, and \(\overrightarrow{P_{Z}}\) is the trajectory in the Z-direction. Therefore, the trajectories in these two directions are input to the two robotic arms, respectively, and the relative movement of the sanding tool along the surface of the workpiece is realized by the movement of the two robotic arms to achieve sanding. After designing the respective trajectories of the manipulators, in order to be able to track the set trajectory of the manipulators in contact with each other during the grinding task, the two robots were controlled using impedance control. The implementation principle of the impedance control algorithm is as follows:  
\[M_{d}(\ddot{X}_{d}-\ddot{X})+D_{d}(\dot{X}_{d}-\dot{X})+K_{d}(X_{d}-X)=F_{ext} \tag{9}\]  
where \(M_{d}\), \(D_{d}\), and \(K_{d}\) are the inertia coefficient matrix, damping coefficient matrix, and stiffness coefficient matrix of the desired impedance model, respectively. \(X_{d}\), \(\dot{X}_{d}\), \(X_{d}\) are the reference position, the reference velocity, and the reference acceleration at the end of the manipulator in the operating space, respectively. \(F_{ext}\) is the force in contact with the environment. For each robot, the dynamics are described as:  
\[M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G+d=\tau-J^{T}(q)F \tag{10}\]  
where \(q\), \(\dot{q}\), and \(\ddot{q}\in\mathbb{R}^{m}\) are the position, velocity, and acceleration vectors of \(m\) joints, respectively. \(M(q)\in\mathbb{R}^{m\times m}\) is the inertial matrix. \(C(q,\dot{q})\dot{q}\in\mathbb{R}^{m}\) represents the coriolis force and centrifugal force. \(G\in\mathbb{R}^{m}\) is the gravity vector. \(d\in\mathbb{R}^{m}\) is the distraction, such as friction. \(\tau\in\mathbb{R}^{m}\) indicates the joint driving torque. \(F\in\mathbb{R}^{n}\) is an \(n\)-dimensional external force vector, and \(J^{T}(q)\in\mathbb{R}^{m\times n}\) is the transposition of the Jacobian matrix of the arm. The Jacobian matrix is used to perform the mapping of the robotic arm joint space velocities into Cartesian space, which is denoted as:  
\[J(q)= \begin{bmatrix}\frac{\partial f_{1}}{\partial q_{1}}&\frac{\partial f_{ 1}}{\partial q_{2}}&\dots&\frac{\partial f_{\tau}}{\partial q_{\tau}}\\ \frac{\partial f_{2}}{\partial q_{1}}&\frac{\partial f_{2}}{\partial q_{2}}& \dots&\frac{\partial f_{2}}{\partial q_{\tau}}\\ \dots&\\ \frac{\partial f_{2}}{\partial q_{1}}&\frac{\partial f_{2}}{\partial q_{2}}& \dots&\frac{\partial f_{2}}{\partial q_{\tau}}\end{bmatrix}  \tag{11}\]  
where \(f_{i}\) denotes the functional relationship between the joint position \(q\) and the spatial orientation \(x\), and \(\frac{\partial f_{i}}{\partial q_{j}}\) denotes the differentiation of the \(i\)th function with respect to the \(j\)th joint. From the dynamic description equation, it can be seen that the driving torque input to the arm can be divided into the torque \(\tau_{ext}=J^{T}(q)F\) generated by the interaction between the arm and the environment and the torque generated by the arm itself.