1. Introduction
Mapping the path of an endeffector onto a configuration trajectory for the robot to accomplish a desired collisionfree task is a wellknown problem in robotics [
1]. The consideration of redundancy, where the actuated degrees of freedom of the manipulator exceed the endeffector variables defining its functionality in the task space, adds an interesting dimension to the planning problem. It effectively facilitates a scheme where additional objectives can also be incorporated along the way. Beyond obstacle avoidance, constraints such as minimal energy, jerkfree paths, anthropomorphism and so forth can thus be considered. A particularly attractive scenario in motion planning is the avoidance of undesirable singularities in joint space [
2], which limits the ability to move in certain task space directions. Increasing the manipulability of the robotic system at each time step is regarded as an effective means of moving away from the neigborhood of such configurations [
3], thus reducing the hazardous condition whereby small task space movements may translate to large joint velocities. Avoiding nearsingular regions is also a particularly concerning situation when the manipulator might be operating in close proximity to human operators. Moreover, operating away from singularity regions also relaxes the effect of undesirable dynamics that otherwise impose additional perturbances to the robot controllers, hence permitting superior endeffector precision whilst executing the desired task.
This brief proposes a stochastic method that exploits the particular kinematics of closedchain mechanisms with redundant actuation and a wellknown manipulability measure [
4] to track the desired endeffector taskspace motion in an efficient manner. The approach departs from global solutions with high computational costs, or optimal formulations that can only be solved numerically, without any guarantee of success except for simple obstaclefree problems. The approach has been tested through simulation on a number of redundant multibody topologies, and via experimental deployment on the Rethink Robotics 7R Sawyer arm.
The rest of the paper is organised as follows:
Section 2 provides broad coverage of techniques in relation with this motion planning for redundant manipulators in the presence of obstacles. Next,
Section 3 describes the kinematics of redundant manipulators and the exploitation of the null space. The stochastic algorithm to generate collisionfree trajectories is described in
Section 4.
Section 5 presents a set of experiments carried out both in simulation and with a real platform. Finally, the main conclusions are described in
Section 6.
2. Related Work
A robotic manipulator is considered to be redundant when it exhibits more degrees of freedom than those needed to perform the task. Typical examples of these redundant robots include serial manipulators with seven or eight degrees of freedom, mobile robots equipped with serial arms, humanoid robots and many others.
Several authors have formulated strategies to exploit the redundant degrees of freedom to improve the quality of the task being carried out. In this manner, a main task can be accomplished by the robot while the other redundant degrees of freedom are used to solve other subtasks [
3,
5] that may include: avoiding joint limits, eluding kinematic singularities and preventing the collision with obstacles in the workspace [
6].
Kinematic singularities are often avoided by trying to maximize the volume of the endeffector’s velocity ellipsoid [
4]. When the robot is placed at a kinematic singularity, the volume of this ellipsoid is zero and the robotic manipulator loses one or more degrees of freedom. Maximizing the volume of this ellipsoid is regarded as an effective means to avoid singularities and expand on the robot’s motion capabilities at a given configuration. Moreover, the optimization of the manipulability allows for faster end effector velocities (linear and angular speed), which in turn benefits the applicability of the chosen control strategies and, given the reciprocity between manipulabity and force ellipsoids, it also procures access to more precise forces and contacts. On the other hand, suboptimal reduced manipulability often means that the contact of the end effector with the surface cannot be assured.
A global optimal control of redundancy is formulated in [
7] based on Pontryagin’s maximum principle. The method employs the redundant degrees of freedom to optimize manipulability or joint smoothness while attaining the same position and orientation along a trajectory. However, though reliable and fast, the method cannot be applied if the gradient of the cost function with respect to the obstacles cannot be computed. Another offline technique along a given path is presented in [
8],where a novel combination with a smoothing interpolation based on Bezier curves is proven able to avoid sharp edges and high accelerations. The method also confers the robot with the ability to avoid collisions, by accounting for the velocity of dynamic obstacles and previewing its next position in order to plan the optimal correction of the trajectory. This method was further extended to the case where redundant manipulators operate in a spatial workspace, with the added capability of avoiding spherelike obstacles [
9]. Finally, [
10] deals with an optimization problem in the case where the robot is redundant along the end effector’s tool axis. The technique allows the finding of a sequence via points in order to minimize the time between target points whilst avoiding obstacles.
A number of authors have proposed planning algorithms based on the discretization of the workspace. This discretization usually implies that kinematic constraints cannot be exactly satisfied and often lack in the quality of the paths produced. In [
6], a deterministic approach to path planning is presented. The solution is based on the discretization of the Jacobian nullspace and a backtracking strategy to prevent the incursion into kinematic singularities.
A technique based on gradient descent is presented in [
11]. The method relies on computing a gradient for a cost function based on smoothness and obstacles. The trajectory of the robot inside the workspace is free and the planned position and orientation is based on a potentialbased cost function.
Stochastic optimization is presented in [
12]. The planner is based on a stochastic method that iteratively optimizes nonsmooth cost functions, most distinctly when they cannot be easily represented by closed functions. The algorithm defines a cost function as the sum of obstacle and torque costs, plus errors in the position and orientation of the end effector. Being a sum of factors, however, entails that the optimization of the cost function cannot ensure that the end effector achieves a given position and orientation exactly.
A significant branch of stochastic planners is based on Rapidlyexploring Random Trees (RRTs) [
13]. These efficient algorithms usually work by building two trees rooted at the beginning and end configurations of the trajectory. A simple greedy heuristic is presented in [
14,
15] to grow the trees and explore the high dimensional space while trying to connect both trees. The planner has been successfully applied to a variety of path planning problems for the computation of collisionfree grasping and manipulation tasks. The growing phase of the tree considers a random generation of new samples in order to explore new solutions in the joint space, but does not include any feature that enables the optimization of the generated trajectories that are, essentially, random. A problem that arises in some of these RRTbased planning algorithms is that, often, a continuous cyclic path in task space does not correspond to a closed path in joint space. As a result, the behavior is not predictable and constitutes a risk if, for instance, a human agent is operating in the vicinity of the robot. In [
16,
17], a variation of an RRTbased planning algorithm is proposed that satisfies the constrains of the path and, additionally, ensures the attainment of joint trajectories that are cyclic.
The method presented here presents the following characteristics:
Handles kinematic restrictions on the end effector exactly and does not rely on a discretization of the task space or configuration space;
Exploits the nullspace at each configuration along the path to maximize the manipulability of the robot while avoiding obstacles;
Produces smooth trajectories that can be directly commanded to the robot without the need for a posterior smoothing phase;
Delivers a fitting experimental performance for challenging motion problems.
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 task space $\mathrm{I}\phantom{\rule{0.166667em}{0ex}}{\mathrm{R}}^{\mathrm{m}}$: In our case we have six Degrees of Freedom (DoF) in our polishing application;
The join space $\mathrm{I}\phantom{\rule{0.166667em}{0ex}}{\mathrm{R}}^{\mathrm{n}}$, as the number of DoF of the robotic manipulator. In this case we have 7DoF.
The degrees of redundancy of the manipulator is $nm$, which means that infinite solutions $\mathbf{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 $nm=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 endeffector to be placed at a given position and orientation inside its workspace, a selfmotion 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. Wellknown 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:
where
$\mathbf{x}$ is the
$(m\times 1)$ vector of task variables defining the position and orientation of the end effector,
$\mathbf{f}$ represents a known nonlinear transformation vector and
$\mathbf{q}$ is the
$(n\times 1)$ vector of joint variables. The robot must then reach a set of goals defined as
$G=\{{\mathbf{x}}_{1},{\mathbf{x}}_{2},\dots {\mathbf{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:
where
J represents the
$(m\times n)$ manipulator Jacobian matrix (also defined as
$\partial \mathbf{f}/\partial \mathbf{q}$). The upper dot denotes time derivative. For simplicity
$J\left(\mathbf{q}\right)$ 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
$\mathbf{x}\left(t\right)$ as known in the task space, find a joint space trajectory
$\mathbf{q}\left(t\right)$ that satisfies
$\mathbf{f}\left(\mathbf{q}\right(t\left)\right)=\mathbf{x}\left(t\right)$ for any
t. In our case, we try to find a set of joint vectors
$Q=\{{\mathbf{q}}_{1},{\mathbf{q}}_{2},\dots {\mathbf{q}}_{N}\}$ such that
$\mathbf{f}\left({\mathbf{q}}_{i}\right)={\mathbf{x}}_{i}$ 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: ${\sum}_{i=1}^{N}{\omega}_{i}={\sum}_{i=1}^{N}det\left({J}_{i}{J}_{i}^{T}\right)$  2:
s.o. ${\mathbf{q}}_{i+1}={\mathbf{q}}_{i}+{\dot{\mathbf{q}}}_{i}\ast \Delta t$.  3:
s.o. ${\dot{\mathbf{q}}}_{i}={J}_{i}^{\u2020}\dot{\mathbf{x}}+{k}_{i}\ast (I{J}_{i}^{\u2020}{J}_{i}){\dot{\mathbf{q}}}_{0}$  4:
RETURN $Q=\{{\mathbf{q}}_{1},{\mathbf{q}}_{2},\dots ,{\mathbf{q}}_{N}\}$ the joint path

A wellknown solution to invert the kinematic Equation (
2) is:
where
${J}^{\u2020}$ is the Moore–Penrose pseudoinverse [
20], defined as
${J}^{\u2020}={J}^{T}{\left(J{J}^{T}\right)}^{1}$. This pseudoinverse has nice properties, since it minimizes the norm
${\dot{\mathbf{q}}}^{T}\dot{\mathbf{q}}$. Thus, given an initial joint position
${\mathbf{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=\{{\mathbf{x}}_{1},{\mathbf{x}}_{2},\dots ,{\mathbf{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
$\mathbf{x}$ closer to the desired goal
${\mathbf{x}}_{i}$. The final solution of the algorithm depends on the initial seed used
${\mathbf{q}}_{r}$, thus, infinite solutions can be obtained by initializing
${\mathbf{q}}_{r}$ 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
${\mathbf{q}}_{r}$ along the initial trajectory.
Algorithm 2 Simple Planner (G, ${\mathbf{q}}_{r}$) 
INPUT:$G=\{{\mathbf{x}}_{1},{\mathbf{x}}_{2},\dots {\mathbf{x}}_{N}\}$, set of positions and orientations 
${\mathbf{q}}_{r}$, an initial random seed for joint positions 
OUTPUT:$\{{\mathbf{q}}_{1},{\mathbf{q}}_{2},\dots ,{\mathbf{q}}_{N}\}$, the joints configuration path 
 1:
functionSimple_Planner(G)  2:
$\mathbf{q}={\mathbf{q}}_{r}$  3:
for $i=1,2,\dots ,N$ do  4:
${\mathbf{x}}_{i}$ = $G\left\{i\right\}$  5:
${\mathbf{q}}_{i}$ = Inverse_Kinematics$({\mathbf{x}}_{i},\mathbf{q})$  6:
$\mathbf{q}={\mathbf{q}}_{i}$  7:
end for return $Q=\{{\mathbf{q}}_{1},{\mathbf{q}}_{2},\dots ,{\mathbf{q}}_{N}\}$ (the joint path)  8:
end function

Algorithm 3 Inverse Kinematics(${\mathbf{x}}_{i}$, ${\mathbf{q}}_{r}$) 
INPUT: 
${\mathbf{x}}_{i}$, the position and orientation of the end effector 
${\mathbf{q}}_{r}$, the initial seed of the algorithm 
OUTPUT: 
$\mathbf{q}$, joint positions 
 1:
functionInverse_Kinematics(${\mathbf{x}}_{i}$, ${\mathbf{q}}_{r}$)  2:
${\mathbf{q}}_{\mathbf{i}}={\mathbf{q}}_{\mathbf{r}}$  3:
while $\mathbf{x}\ne {\mathbf{x}}_{i}$ do  4:
$\dot{\mathbf{x}}=Compute\_VW({\mathbf{x}}_{i},\mathbf{x})$  5:
${J}_{i}$ = Jacobian(${\mathbf{q}}_{i}$)  6:
${\dot{\mathbf{q}}}_{i}={J}_{i}^{\u2020}\dot{\mathbf{x}}$  7:
$\mathbf{q}=\mathbf{q}+{\dot{\mathbf{q}}}_{i}\ast \Delta t$  8:
$\mathbf{x}=f\left(\mathbf{q}\right)$  9:
end while  10:
return$\mathbf{q}$  11:
end function  12:
functionCompute_VW(${\mathbf{x}}_{i}$, $\mathbf{x}$)  13:
//Computes linear and angular speed  14:
//to reduce the error in $\mathbf{x}{\mathbf{x}}_{i}$  15:
$\dot{\mathbf{x}}=(\mathbf{x}{\mathbf{x}}_{i})/\Delta t$ return $\dot{\mathbf{x}}$  16:
end function

A more general solution to (
3) can be written as:
where I is an
$(n\times n)$ identity matrix and
${\dot{\mathbf{q}}}_{0}$ is an arbitrary
$(n\times 1)$ joint velocity vector. This solution includes the projector operator
$(I{J}^{\u2020}J)$, which allows us to project a vector
${\dot{\mathbf{q}}}_{0}$ on the null space of the initial solution provided by
${J}^{\u2020}\dot{\mathbf{x}}$. Gradient projection methods exploit this property and compute a joint speed vector
$\dot{\mathbf{q}}$ as:
${p}_{0}$ being an arbitrary cost function that needs to be optimized and
$\frac{\partial {p}_{0}}{\partial q}$ its gradient. Equation (
5) indicates that the redundant degrees of freedom can be used to attain additional constraints, such as obtaining greater manipulability along the trajectory or avoiding collisions with the environment. Usually, manipulability is measured with the index introduced by Yoshikawa [
4]:
Additionally, using the manipulability has a strong impact when trying to avoid kinematic singularities. In particular, that situation happens whenever the matrix J, at some joint position $\mathbf{q}$, has a rank less than m. This situation can be easily detected, since the manipulability index $\omega $ becomes null and the manipulator loses, at least, one degree of freedom, which jeopardizes its ability to complete the task.
4. Trajectory Tracking Optimisation
The manipulator task studied in this work corresponds to a tracking problem, whereby an endeffector task space trajectory needs to be mapped to a corresponding joint space trajectory. Given ${\mathbf{x}}_{i}$, representing the position and orientation of the end effector with respect to a base reference system, the proposed methodology can be regarded as a trajectory optimisation problem for the set of N waypoint goals $G=\{{\mathbf{x}}_{1},{\mathbf{x}}_{2},\dots {\mathbf{x}}_{N}\}$ defining the contact surface that the robot must follow with precision. Maximising the manipulability index along the trajectory whilst avoiding obstacles allows us to move away from singularities whilst facilitating the desired motion along the desired surface for the contact/visiting task being pursued.
The method starts by generating a set of
K hypotheses on the path:
Each hypothesis K of the path ensures that the robot’s end effector reaches the goal ${\mathbf{x}}_{i}$ for $i=1,\dots N$, and utilises an inverse kinematic method based on the Moore–Penrose pseudo inverse as described in Algorithm 2. The initial joint positions ${\mathbf{q}}_{r}$ of the manipulator are initialized randomly and, as a result, the resulting K paths ${Q}_{k}=\{{\mathbf{q}}_{1},{\mathbf{q}}_{2}\dots {\mathbf{q}}_{N}\}$ are completely random with the property of ${\dot{\mathbf{q}}}^{T}\dot{\mathbf{q}}$ being minimal for each different trajectory at each time step.
This set of
K joint paths form a set of hypotheses that start from different and arbitrary initial
${\mathbf{q}}_{1}$ joint configurations. Each of the generated trajectories is optimized at each time step
$i=1,\dots N$ in order to increase its manipulability index indicated in Equation (
6) and, at the same time, avoid obstacles. For each of the
K hypotheses over the trajectory, our method performs a sampling on the null space at each step
i of the trajectory. The sampling is achieved by generating
H samples around each joint configuration
${\mathbf{q}}_{i}$. To generate each new sample, we compute a vector belonging to the null space as:
where
${\dot{\mathbf{q}}}_{0}$ represents an arbitrary vector that generates a sample of the null space. Next,
H random movements are generated as:
Any of these new samples ${\mathbf{q}}_{h}$ around each joint configuration ${\mathbf{q}}_{i}$ does not alter the position and orientation of the end effector since ${\dot{\mathbf{q}}}_{h}$ is only a selfmotion and thus $J{\dot{\mathbf{q}}}_{h}=\mathbf{0}$. The variable $\alpha $ is chosen from a normal distribution and $\Delta t$ is an integration time. The time integration step $\Delta t$ and $\alpha $ are parameters of the algorithm.
At each time step, each of the new
${\mathbf{q}}_{h}$ (for
$h=1,\dots H$) is a self motion around the joint positions
${\mathbf{q}}_{i}$ that can potentially improve its manipulability index while avoiding obstacles. For each
${\mathbf{q}}_{h}$, we then compute a weight that accounts for the manipulability:
where
${\omega}_{max}$ is the maximum observed manipulability of the mechanism. The parameter
$\delta $ avoids the division by zero if the mechanism incurs in a singularity.
In order to consider the presence of obstacles, for each sample
${\mathbf{q}}_{h}$, we compute the closest distance
d of all the points of the robot arm to obstacles and with itself. The distance
d is negative whenever any point of the manipulator lies inside an obstacle. We then compute:
$\u03f5$ is the minimum required distance to the obstacles and
$\lambda $ is a parameter that smooths the computed weights. During the experiments we have used
$\u03f5=0.2$ m and
$\lambda =0.5$ m.
A weight is computed that accounts for the manipulability index and the distance to obstacles as:
Finally, a sample
${\mathbf{q}}_{h}$ from the nullspace is selected that maximizes the weight
${W}_{{q}_{h}}$. The final path of the robot is obtained by selecting the path with the higher weight
${W}_{{q}_{h}}$. The complete algorithm is described in Algorithm 4.
Algorithm 4 Stochastic Planner(G, O) 
INPUT: 
$G={\mathbf{x}}_{1},{\mathbf{x}}_{2},\dots {\mathbf{x}}_{N}$, set of positions and orientations. 
OUTPUT: 
$Q={\mathbf{q}}_{1},{\mathbf{q}}_{2},\dots {\mathbf{q}}_{N}$, path of the robot. 
 1:
functionStochasticPlanner(G, O)  2:
//Build K initial random paths.  3:
for $k=1,2,\dots ,K$ do  4:
${\mathbf{q}}_{r}=uniform(1,m)$  5:
for $i=1,2,\dots ,N$ do  6:
${\mathbf{q}}_{i+1}$ = InverseKinematics(${\mathbf{x}}_{i}$, ${\mathbf{q}}_{i}$)  7:
end for  8:
//Store a random path for the K particle.  9:
${Q}_{k}=\{{\mathbf{q}}_{1},{\mathbf{q}}_{2},\dots ,{\mathbf{q}}_{N}\}$  10:
end for  11:
while convergence of $\sum {W}_{k}$ do  12:
for $k=1,2,\dots ,K$ do  13:
${p}_{k}$ = ${G}_{k}$  14:
for $i=1,2,\dots ,N$ do  15:
${q}_{i}$ = SampleFromNullSpace(${G}_{k,i}$)  16:
${w}_{h}$ = ComputeWeights(${q}_{h}$)  17:
//Find $\mathbf{q}$ that maximizes $W=\{{w}_{1},\dots {w}_{H}\}$  18:
$\widehat{\mathbf{q}}$ = $argma{x}_{{q}_{h}}\left({W}_{h}\right)$  19:
//Add $\widehat{\mathbf{q}}$ to the path  20:
end for  21:
end for  22:
end while  23:
return$Q=\{{\mathbf{q}}_{1},{\mathbf{q}}_{2},\dots ,{\mathbf{q}}_{N}\}$, the joint path  24:
end function  25:
functionSampleFromNullSpace(${G}_{k,i}$) INPUT: Current joint position i at path k. OUTPUT: H samples ${q}_{h}$ from null space.  26:
for $h=1,2,\dots ,H$ do  27:
${\dot{\mathbf{q}}}_{h}=(I{J}^{\u2020}J){\dot{\mathbf{q}}}_{0}$  28:
${\mathbf{q}}_{h}={\mathbf{q}}_{i}+\u03f5\xb7{\dot{\mathbf{q}}}_{h}\Delta t$  29:
end for return ${q}_{h}=\{{q}_{1},{q}_{2},\dots ,{q}_{H}\}$, samples from null space.  30:
end function
