**3. Kinematics of Redundant Manipulators**

A task generally requires a given number of degrees of freedom to be described and solved. In this sense, a manipulator is considered redundant when it possesses more degrees of freedom than those required to complete the task. For example, placing the robot's end effector at a given position and orientation inside the robot's workspace typically require six degrees of freedom. This is a main requirement, for example, during polishing applications, where the tool must keep close contact with the surface being treated [18]. As a result, two different spaces can be defined:


The degrees of redundancy of the manipulator is *n* − *m*, which means that infinite solutions **q** allow reaching the same position/orientation in space. Thus, in a polishing application, 6DoF are needed to place the end effector at a particular position and orientation, while controlling the pressure on the surface. The usage of a robot with additional DoF (in our case is *n* − *m* = 1) may be justified by the need to avoid obstacles in the workspace when performing the task or attaining specific poses.

The inverse kinematic problem in a redundant manipulator possesses, in general, infinite solutions. This means that, if we require the end-effector to be placed at a given position and orientation inside its workspace, a self-motion of the kinematic chain can

be performed. This implies that the arm joints can be reconfigured while the end effector maintains the same position and orientation in space. This fact gives these kinds of manipulators the ability to find different poses that attain dissimilar characteristics, while still complying with the task requirements. Well-known examples include, for instance, the Rethink Robotics Sawyer robot or the Willow Garage PR2 arms, with 7DoF available through a set of seven rotational joints. Additionally, combinations of robotic arms with mobile platforms can be considered within this category of redundant manipulators.

The relationship between the position and orientation of the end effector can be expressed as:

$$\mathbf{x} = \mathbf{f}(\mathbf{q}),\tag{1}$$

where **x** is the (*m* × 1) vector of task variables defining the position and orientation of the end effector, **f** represents a known nonlinear transformation vector and **q** is the (*n* × 1) vector of joint variables. The robot must then reach a set of goals defined as *G* = {**x**1, **x**2,... **x***N*} sampled from the desired surface to be tracked defined in the task space.

The above Equation (1) can be differentiated with respect to time as:

$$
\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}\_{\prime} \tag{2}
$$

where *J* represents the (*m* × *n*) manipulator Jacobian matrix (also defined as *∂***f**/*∂***q**). The upper dot denotes time derivative. For simplicity *J*(**q**) is written as *J*. As a result, Equation (2) defines the direct kinematics of the manipulator in terms of the end effector's velocity.

Our particular path planning problem can be posed as follows. Considering a given trajectory **x**(*t*) as known in the task space, find a joint space trajectory **q**(*t*) that satisfies **f**(**q**(*t*)) = **x**(*t*) for any *t*. In our case, we try to find a set of joint vectors *Q* = {**q**1, **q**2,... **q***N*} such that **f**(**q***<sup>i</sup>* ) = **x***<sup>i</sup>* for any of the points of the trajectory *G*. In this sense, finding an optimal trajectory with respect to the manipulability index can be stated as in Algorithm 1, which essentially proposes the optimization of the manipulability along the whole trajectory, considering that the manipulability can be expressed in a differential form with respect to the joint coordinates. Solutions to this problem have been proposed ([7,19]) at significant computational expense, becoming intractable in the presence of complex obstacle settings.

### **Algorithm 1** Global Manipulability Optimization.

1: Optimize: ∑*<sup>N</sup> <sup>i</sup>*=<sup>1</sup> *<sup>ω</sup><sup>i</sup>* <sup>=</sup> <sup>∑</sup>*<sup>N</sup> <sup>i</sup>*=<sup>1</sup> *det*(*JiJ<sup>T</sup> i* )

2: s.o. **q***i*+<sup>1</sup> = **q***<sup>i</sup>* + **q**˙ *<sup>i</sup>* ∗ Δ*t*.

3: s.o. **q**˙ *<sup>i</sup>* = *J*† *<sup>i</sup>* **<sup>x</sup>**˙ <sup>+</sup> *ki* <sup>∗</sup> (*<sup>I</sup>* <sup>−</sup> *<sup>J</sup>*† *<sup>i</sup> Ji*)**q**˙ <sup>0</sup>

4: RETURN *Q* = {**q**1, **q**2,..., **q***N*} the joint path

A well-known solution to invert the kinematic Equation (2) is:

$$
\dot{\mathbf{q}} = J^{\dagger} \dot{\mathbf{x}},\tag{3}
$$

where *J*† is the Moore–Penrose pseudo-inverse [20], defined as *J*† = *JT*(*J JT*)−1. This pseudo-inverse has nice properties, since it minimizes the norm |**q**˙ *<sup>T</sup>***q**˙ |. Thus, given an initial joint position **q***r*, the length of the computed joint trajectory is, by nature, minimal. This equation allows us to compute the joint positions required to reach a set of positions and orientations *G* = {**x**1, **x**2, ... , **x***N*} of the end effector. This minimum square solution is represented in Algorithm 2. The algorithm, for each time step, generates a joint speed that brings the solution **x** closer to the desired goal **x***i*. The final solution of the algorithm depends on the initial seed used **q***r*, thus, infinite solutions can be obtained by initializing **q***<sup>r</sup>* randomly. However, this solution may produce a path that includes kinematic singularities [3] or collides with obstacles in the environment. This simple planner makes use of Algorithm 3, computing at each time step the inverse kinematics solution from a given random joint position **q***<sup>r</sup>* along the initial trajectory.
