# Design and Motion Planning of a Biped Climbing Robot with Redundant Manipulator
## 3 Motion Planning
### Motion Planning of the Climbing Mechanism
\tag{2}\]  
Passing the parameters of Table 1 to Equation (1), \(p\) can be expressed explicitly by the following equation:  
\[p_{\mathrm{x}}=\cos(\theta_{1})(l_{2}\cos(\theta_{2})+l_{2}\cos(\theta_{2}+ \theta_{3})+l_{1}\sin(\theta_{2}+\theta_{3}+\theta_{4}); \tag{3}\]  
\[p_{\mathrm{y}}=\sin(\theta_{1})(l_{2}\cos(\theta_{2})+l_{2}\cos(\theta_{2}+ \theta_{3})+l_{1}\sin(\theta_{2}+\theta_{3}+\theta_{4}); \tag{4}\]  
\[p_{\mathrm{z}}=l_{1}+l_{2}\sin(\theta_{2})+l_{2}\sin(\theta_{2}+\theta_{3})- \cos(\theta_{2}+\theta_{3}+\theta_{4}). \tag{5}\]  
\(\varphi_{\mathrm{y}}\) and \(\varphi_{\mathrm{z}}\) can be expressed by the following equation:  
\[\varphi_{\mathrm{y}}=-\theta_{2}+\theta_{3}+\theta_{4}; \tag{6}\]  
\[\varphi_{\mathrm{z}}=\theta_{1}+\theta_{5}. \tag{7}\]  
Inverse kinematics is used to calculate the joints angles when the designed trajectory is given. To prevent collisions between the magnetic modules and the handrail, \(\varphi_{\mathrm{y}}\) should remain at \(0^{\circ}\) during the locomotion. The sum of \(\theta_{2}\), \(\theta_{3}\), and \(\theta_{4}\) should be 0 according to Equation (6). Passing this constraint to Equations (3)-(5), the values of \(\theta_{1}\), \(\theta_{3}\), \(\theta_{2}\), \(\theta_{4}\), and \(\theta_{5}\) can be calculated successively as:  
\[\left\{ \begin{array}{c}\theta_{1}=\arctan\left(\frac{p_{\mathrm{y}}}{p_{ \mathrm{x}}}\right)\\ \theta_{3}=\arccos\left(\frac{\left(p_{\mathrm{y}}\right)^{2}+\left(p_{ \mathrm{z}}\cos(\theta_{1})\right)^{2}-\left(l_{2}\cos(\theta_{1})\right)^{2} }{2\left(l_{2}\cos(\theta_{1})\right)^{2}}\right)\\ \theta_{2}=\frac{\theta_{3}}{2}-\arcsin\left(\frac{p_{\mathrm{z}}}{\sqrt{\left( p_{\mathrm{x}}\right)^{2}+\left(p_{\mathrm{y}}\right)^{2}}}\right)\\ \theta_{4}=\theta_{2}-\theta_{3}\\ \theta_{5}=\varphi_{\mathrm{y}}\end{array} \right. \tag{8}\]  
To prevent the CMBOT from falling, the manipulator rotates along the \(Z\)-axis of J6 and acts as a counterweight to balance the CMBOT. From Figure 10, the folded configuration of the manipulator rotates about J6 to reduce the torque about the \(Y\)-axis of (B). Point B is the origin of (B), the magnitude of the torque about B due to the gravity of climbing mechanism is given by:  
\[\mathrm{M}=\sum\
olimits_{i=1}^{5}\vec{BL_{i}}\times m_{i}g+\sum\
olimits_{i= 1}^{5}\vec{BL_{i}}\times m_{i}\mu_{L_{i}}, \tag{9}\]  
\begin{table}
\begin{tabular}{c c c c c} \hline \hline \multirow{2}{*}{**Joint**} & **Link Torsional Angle** & **Link Length** & **Link Offset** & **Joint Angle** \\  & \(\alpha_{i}\) **(*****)** & \(\alpha_{i}\) **(****mm**)** & \(\alpha_{i}\) **(*****)** & \(\theta_{i}\) **(*****)** \\ \hline
1 & 0 & 0 & 0 & \(\theta_{1}\) \\
2 & 90 & 0 & \(l_{1}\) & \(\theta_{2}\) \\
3 & 0 & \(l_{2}\) & 0 & \(\theta_{3}\) \\
4 & 0 & \(l_{2}\) & 0 & \(\theta_{4}\) \\
5 &where, M is the torque about \(B\), \(L_{i}\) is the center of mass of link \(i\), \(m_{i}\) is the mass of link \(i\), \(a_{L_{i}}\) is the acceleration of point \(L_{i}\), and g is the magnitude of gravity acceleration. Then, the angle of Joint 6 can be calculated as:  
\[\theta_{6}=M_{\rm y}/l_{e}, \tag{10}\]  
where, \(M_{\rm y}\) represents the torque about the \(Y\)-axis of (B), and \(l_{e}\) represents the distance between the equivalent center of mass of manipulator E and \(Y\)-axis of (B). The trajectories of the magnetic feet also play an important role in motion stability. A low-contact impact trajectory is proposed to reduce the influence of impingement against the handrail of the climbing configuration. Based on the compound cycloid, the proposed trajectory is smooth, so there is not a sudden change in the velocity and acceleration of the trajectory, and both the velocity and acceleration will be equal to 0 when the magnetic feet contact the handrail. The equations of the trajectory are given by:  
\[\left\{