# Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment
## 2 Materials and Methods
### Formulation of the Problem
This assumption means that the linear components of the velocities were transmitted through the contact point, while, on the other hand, the angular components were not. Introducing the function \(\mathbf{p}_{C}(\mathbf{o})\), describing the locations of the contact points, the grasp limitations can be expressed as geometric and kinematic constraints as follows:  
\[\mathbf{f}(\mathbf{q}(t)) =\mathbf{p}_{C}(\mathbf{o}(t)) \tag{9}\] \[\mathbf{J}\,\dot{\mathbf{q}}(t) =\dot{\mathbf{p}}_{C}(\mathbf{o}(t)),\]  
where \(\dot{\mathbf{p}}_{C}=d\mathbf{p}_{C}/dt\) describes the linear velocities of the contact points. The practical processes that are accomplished by the industrial robots impose some conditions at the beginning and the end of the trajectory. It is natural to assume that at the initial and final moments of the performance of the task, the velocities of the manipulator equal zero:  
\[\dot{\mathbf{q}}(\mathbf{0})=\mathbf{0},\quad\dot{\mathbf{q}}(\mathbf{T})= \mathbf{0}. \tag{10}\]  
During the completion of the task, the robot should maintain the limitations resulting from its construction and the range of movements permitted in the individual joints. Introducing the scalar function \(C_{M}\), defining the algebraic distance of a given configuration from the range permitted, such mechanical constraints can be expressed in the form of a set of \(L_{M}\) inequalities in the form:  
\[\forall t\in[0,T] C_{M}^{i}(\mathbf{q}(t))\geq 0,\quad i=1,\dots,L_{M}. \tag{11}\]  
The space in which the robot operates may contain some equipment that should be treated as obstacles and must be taken into account during the motion planning. Introducing the scalar functions \(C_{Or}\) and \(C_{Oo}\), which describe the algebraic distances from the obstacles to the robot and object, respectively, the collision avoidance conditions can be written as sets of \(L_{Or}\) for the robot and \(L_{Oo}\) for the object inequalities in the form:  
\[\forall t\in[0,T] C_{Or}^{i}(\mathbf{q}(t))\geq 0,\quad i=1,\dots,L_{Or} \tag{12}\] \[\forall t\in[0,T] C_{Oo}^{i}(\mathbf{o}(t))\geq 0,\quad i=1,\dots,L_{Oo} \tag{13}\]In practice, the configuration of the manipulator joints should be far away from singular configurations. In order to achieve this goal, the Yoshikawa manipulability measure [26], which describes the distance to singular configurations, may be maximized. The Yoshikawa manipulability index \(\mu(\mathbf{q})\) is proportional to the volume of the velocity ellipsoid. The semi-axes of this ellipsoid coincide with the eigenvectors of the matrix \(\mathbf{J}\mathbf{J}^{\mathsf{T}}\), the lengths of these semi-axes being equal to the square roots of the eigenvalues of \(\mathbf{J}\mathbf{J}^{\mathsf{T}}\). Since the product of the lengths of the semi-axes is proportional to the volume of the ellipsoid and the product of the eigenvalues of the matrix is equal to the determinant of the matrix, the manipulability measure may be calculated as: \(\mu(\mathbf{q})=\sqrt{det(\mathbf{J}\mathbf{J}^{\mathsf{T}})}\). It seems that in the case of a dual-arm humanoid manipulator, the avoidance of singular configurations of the robot arms [27] is important, so the following manipulability measure was taken into account:  
\[\mu(\mathbf{q})=\sqrt{det(\mathbf{J}_{arms}\mathbf{J}_{arms}^{\mathsf{T}})}, \tag{14}\]  
where \(\mathbf{J}_{arms}=\left[ \begin{array}{cc}\mathbf{J}_{L,harm}&0\\ 0&\mathbf{J}_{R,arm}\end{array} \right]\) consists of the 8 last columns of the matrix (4).