# Maximising Wrenches for Kinematically Redundant Systems with Experiments on
UVMS

Wilhelm J. Marais1, Stefan B. Williams1, Oscar Pizarro1 1Authors are with
Australian Centre for Field Robotics, University of Sydney, NSW Australia,
(j.marais, stefanw<EMAIL_ADDRESS>
###### Abstract

This paper presents methods for finding optimal configurations and actuator
forces/torques to maximise contact wrenches in a desired direction for
Underwater Vehicles Manipulator Systems (UVMS). The wrench maximisation
problem is formulated as a linear programming problem, and the optimal
configuration is solved as a bi-level optimisation in the parameterised
redundancy space. We additionally consider the cases of one or more
manipulators with multiple contact forces, maximising wrench capability while
tracking a trajectory, and generating large wrench impulses using dynamic
motions. We look at the specific cases of maximising force to lift a heavy
load, and maximising torque during a valve turning operation. Extensive
experimental results are presented using an underwater robotic platform
equipped with a 4DOF manipulator, and show significant increases in wrench
capability compared to existing methods for UVMS.

##  I Introduction

High Degree Of Freedom (DOF) vehicle-manipulator systems have seen increased
use in both industrial and field robotics settings due to the advantages
provided by kinematic redundancy. Since these systems have a large number of
degrees of freedom responsible for end effector motion, kinematic positioning
generally requires iterative techniques. Despite this, these kinematically
redundant systems can make use of the continuous space of configurations which
solve a particular inverse kinematics problem to optimise additional secondary
objectives [1]. Secondary objectives may include obstacle avoidance,
optimisation of dynamic manipulability and avoidance of configurations limits
[2, 3]. During interaction tasks with objects or the environment, end effector
poses as well as wrenches are of consideration [4, 5]. External contact forces
and torques between the end effector and the environment are transmitted
through each DOF of the system responsible for end effector motion, in a way
which is highly dependent on the configuration. Exploiting the continuous
redundancy offered by high DOF systems allows for configurations which
simultaneously achieve a desired end effector pose, and maximise the wrench
capabilities [6].

Interaction tasks in underwater environments for scientific exploration or
industrial purposes have been traditionally conducted by medium to large
Underwater Vehicle Manipulator Systems (UVMS), requiring specialised equipment
and multiple operators for launch and recovery as well as operation [7]. In
recent years, the emergence of smaller and lower cost commercial off-the-shelf
underwater vehicles and manipulators has seen a transition from larger to
small vehicles for some interaction tasks [8, 9]. This reduces both operations
costs and time, as well as increasing the accessibility of these systems since
they can be launched, operated, and recovered by a small team with no
specialised equipment. This work looks at maximising the capability of these
small systems, allowing them to perform a larger range of interaction tasks.
Specifically, maximising contact wrenches between the end effector and the
environment are considered. This work assumes the object to be manipulated has
already been firmly grasped by the end effector of the UVMS, and the system
can then transition to a configuration which allows for the maximum wrench to
be applied at the end effector.

Figure 1: UVMS used in experiments with a 6 DOF Bluerov vehicle and 4 DOF
Blueprint Lab Reach Alpha manipulator, showing vehicle pose $\eta$ and end
effector pose $x$ relative to the world frame $\\{W\\}$. Manipulator joints
are labelled $q_{1}$ to $q_{4}$

We compare a number of cases in which wrench capability should be maximised.
These include the static case, where the aim is to maximise the wrench at a
single configuration, which we further extended to consider multiple contact
points with the environment. The second case considers a trajectory where a
given desired end effector path should be tracked, while finding a set of
configurations along the way which are dynamically feasible and maximise the
lowest applicable wrench along the path. Finally, the case of generating large
wrench impulses for a fixed end effector pose is considered. Extensive
experimental validation of the proposed methods is provided, which shows
significant increases in wrench capability compared to previous methods for
UVMS.

The contributions of this paper are the following:

  * •

A bi-level optimisation approach for finding optimal configurations and
actuator efforts for maximising wrenches for UVMS is proposed. Experimental
results show this provides significantly larger wrenches than existing
transmission ratio optimisation methods

  * •

Consideration of relaxing constraints on orthogonal wrenches for certain
tasks, with experimental results showing a threefold increase in maximum
wrench capability

  * •

An optimisation method is proposed for maximising wrenches for UVMS with
multiple contact points, including parameterisation of a set of secondary
grasping points, and analysis of the required constraints. Experimental
results show increased wrench capability using the proposed methods

  * •

A bi-level optimisation method is proposed for generating trajectories which
maximise wrenches over a trajectory, with experimental results confirming the
validity of the proposed method

  * •

A proposed method for generating heaving motions for large wrench impulses,
with experimental results showing the validity of the approach.

The rest of this paper is structured as follows. Section II gives a recap of
the mathematical background and current methods for wrench analysis and
finding optimal configurations for wrench maximisation. Section III describes
the methods proposed in this work for maximising static wrench capability for
UVMS, including consideration of multiple contact points. Section IV extends
the methods used for static analysis to consider dynamic trajectories given an
end effector path. Section V describes the proposed method for generating
large wrench impulses using dynamic motions. Finally, Section VI presents
experimental results for each section, followed by concluding remarks and
directions for future work in Section VII.

##  II Background and Related Work

###  II-A Kinematic and Dynamic Modelling

A vehicle manipulator system has system configuration $\theta=(\eta,q)^{T}$,
with $dim(\theta)=n$, where $\eta\in SE(3)$ is the vehicle pose in the world
frame, and $q\in\mathbb{R}^{n-6}$ is the manipulator joint angles. These are
shown in Figure 1. Given some end effector pose $x\in SE(3)$, in the world
frame, the forward non-linear map $f_{k}$ gives

$x=f_{k}(\theta)$ (1)

for which analytical solutions in the reverse direction, the inverse
kinematics problem, are generally not available. Numerical solutions make use
of the linear velocity relationship

$\dot{x}=J(\theta)\dot{\theta}$ (2)

where $\dot{x}\in\mathbb{R}^{6}$ is the end effector velocity vector in the
inertial frame, $\dot{\theta}\in\mathbb{R}^{n}$ is the vector of system
velocities corresponding to each DOF, and $J(\theta)$ is the configuration
dependant Jacobian. The $\theta$ dependence for $J$ is dropped in further
notation. For kinematically redundant systems this has a least squares
solution for a desired $\dot{x}$ given by

$\dot{\theta}=J^{+}\dot{x}$ (3)

where $J^{+}$ is the pseudoinverse of $J$. Note in this work we consider fully
actuated vehicles, therefore the Jacobian for a single manipulator system
always has full row rank. The null space of $J$, defined as the set of system
velocities which cause no end effector velocity can be included as

$\dot{\theta}=J^{+}\dot{x}+\alpha(I-J^{+}J)\nabla H$ (4)

where $\alpha$ is a scaling factor, and $H$ is some secondary objective to be
optimised in the null space [1]. The null space projection $(I-J^{+}J)$ can be
thought of as a set of (possibly non linearly independent) basis vectors which
is tangent to the inverse kinematics equality constraint.

Now we have the end effector wrench vector

$h_{e}=\left(\begin{array}[]{c}{}^{0}f_{e}\\\
{}^{0}n_{e}\end{array}\right)\in\mathbb{R}^{6}$ (5)

consisting of end effector forces ${}^{0}f_{e}$ and torques ${}^{0}n_{e}$ in
the inertial frame, with force relationship

$\tau_{h}=J^{T}h_{e}$ (6)

where $\tau_{h}\in\mathbb{R}^{n}$ is the vector of forces and torques on each
DOF due to $h_{e}$. The actuator model is given by $Bu$, where
$u\in[u_{min},u_{max}]$ in $\mathbb{R}^{m}$ is the control input vector for
each actuator, and the matrix $B\in\mathbb{R}^{n\times m}$ is the mapping
between actuator control input force/torque on each DOF. Including the
dynamics of the systems gives

$M(\theta)\ddot{\theta}+C(\theta,\dot{\theta})\dot{\theta}+D(\theta,\dot{\theta})\dot{\theta}+g(\theta)+J^{T}h_{e}=Bu$
(7)

where $M(\theta)$ is the configuration dependent mass matrix,
$C(\theta,\dot{\theta})$ is the Coriolis terms, $D(\theta,\dot{\theta})$ is
damping terms modelled using a combination of linear and quadratic drags, and
$g(\theta)$ is the vector of gravity and buoyancy forces. We collect all
dynamic terms on the the left into the vector $\tau_{d}$, giving

$\tau_{d}+J^{T}h_{e}=Bu$ (8)

###  II-B Ellipsoids and Polytopes

Analysis of wrench capability typically involves ellipsoids and polytopes.
Velocity ellipsoids and their counterparts, wrench ellipsoids, were introduced
by [10]. These provide a mapping from a unit ball of joint torques to end
effector wrench, defined as

$\\{h_{e}\;\mid\;\|J^{T}h_{e}\|_{2}\leq 1\\}$ (9)

Recent work proposed maximising the volume of the velocity ellipsoid by
projecting the gradient onto the null space of the system as in Equation 4,
and further by solving the problem as a quadratic program [11] . A similar
null space projection method was used by [12] to maximise the dynamic
manipulability ellipsoid during a value turning operation for an UVMS.

The volume of the wrench ellipsoid accounts for the ability of the system to
apply forces and torques in all directions in an isotropic manner. In this
work, it is desired to maximise the wrench along a single force/torque
direction, therefore anisotropic capability measures are more appropriate. One
such measure is the transmission ratio, which is defined as the radius of the
wrench ellipsoid in a desired direction, and can been optimised by null space
projection [13]. To find the distance to the wrench ellipsoid in a given
direction, consider a unit vector $c\in\mathbb{R}^{6}$ which defines the
wrench direction, and scalar $\beta$, with the condition that it lies on the
ellipse given by

$(\beta c)^{T}JJ^{T}(\beta c)=1$ (10)

and solving for $\beta$,

$\beta=(c^{T}JJ^{T}c)^{-1/2}$ (11)

This analysis does not include the effect of gravity, buoyancy and dynamical
effects which offsets the centre of the ellipsoid, giving a dynamic wrench
ellipsoid defined as

$\\{h_{e}\;\mid\;\|J^{T}h_{e}+\tau_{d}\|_{2}\leq 1\\}$ (12)

For an ellipse with centre not at the origin, we can solve for $\beta$
assuming the origin is contained in the ellipsoid. This condition is met with
a system which can support its own weight under gravity and sustain the
dynamic loads. We solve

$(J^{T}\beta c+\tau_{d})^{T}(J^{T}\beta c+\tau_{d})=1$ (13)

for $\beta$ by taking the positive root.

Symmetric torque limits have been proposed to be incorporated into the
transmission ratio [14], yet dynamical effects were neglected. Some works have
considered minimising the sum of squared torques on each joint for a given
wrench of a redundant serial manipulator by null space projection [15], which
is functionally equivalent to optimising the transmission ratio.

The idea of wrench ellipsoids was extended by [16] by scaling the ellipsoid by
a desired force/stiffness matrix, and instead optimising the trace of the
resultant ellipsoid (the sum of the square of the radii), to get a
configuration dependant measure. Further work [17] looked at combining
velocity, acceleration and wrench capabilities in the joint torque space for
serial manipulators. This involves mapping maximal balls of velocity,
acceleration and wrench individually onto ellipsoids in the joint torque
space, and then combining to form a torque hypersurface which has to satisfy
actuator constraints. Since the method combines worst-case scenarios for
velocities, accelerations and wrenches, it provides a very conservative
measure of manipulator capability.

It has been shown that wrench ellipsoids provide only an approximate measure
of manipulator performance as it fails to capture to the true constraints of
the system, as compared to wrench polytopes which provides a better
description of the true capabilities of a system [18]. By finding the set of
effector wrenches which satisfy joint constraints, the wrench polytope is
defined as

$\\{h_{e}\;\mid\;\tau_{min}\leq(J^{T}h_{e}+\tau_{d})\leq\tau_{max}\\}$ (14)

where $\tau_{min}$ and $\tau_{max}$ are the minimum and maximum loads on each
DOF. Wrench polytope analysis has been used extensively in the design and
evaluation of manipulators and manipulator poses [19, 20], yet optimisation is
difficult since the quality of a polytope is difficult to quantify. Some
attempts to define capability measures using polytopes involve computing
actuator saturation for a given wrench direction for serial manipulators [6],
or solving Linear Programming (LP) problems for parallel manipulators [21,
22].

###  II-C Redundancy Parameterisation

The idea of redundancy parameterisation has been proposed in several works to
greatly reduce the number of dimensions over which to optimise a given
performance objective, and to remove the non-linear inverse kinematics
constraint. This methods avoids the use of the Jacobian null space projection
in Equation 4 and instead explicitly considers a minimal set of DOF which
describe the available self motions while keeping a fixed end effector pose.
Redundancy parameterisation has been used for choosing optimal stiffness
configurations in serial manipulators with one degree of redundancy [23], as
well as for exploring the force capabilities of a manipulator to apply forces
normal to a surface [24]. This work was extended in [6] to an exhaustive
search over 2 redundant DOF for a 7DOF manipulator, where the saturating
wrench was used to find the weight lifting capability for each pose, while
[25] examined a planar parallel manipulator with a 3DOF redundant space to
minimise the sum of squared joint torques for a given wrench. Given an
appropriate parameterisation of the redundant DOF written as
$\theta_{r}\in\mathbb{R}^{n-6}$, Equation 4 can be re-written as

$\dot{\theta}=J_{e}\dot{x}+A_{r}\dot{\theta_{r}}$ (15)

where $\dot{\theta_{r}}\in\mathbb{R}^{n-6}$ is the vector of velocities of the
redundant DOF, $A_{r}\in\mathbb{R}^{n\times(n-6)}$ is the null space
projection matrix which maps redundant velocities to system velocities, and
$J_{e}\in\mathbb{R}^{n\times 6}$ is the Jacobian which maps end effector
velocities to all DOF which are not redundant DOF.

###  II-D Wrench Capability Over a Trajectory

The previous works have considered wrench capabilities for a single end
effector pose. In some cases the wrench capability over an entire trajectory
has to be considered. Local redundancy resolution methods have been used for
continuous wrench maximisation over a trajectory [12], yet local methods may
lead to numerical instability and sub-optimal trajectories [26]. Global
redundancy resolution methods which consider entire paths have been proposed
for trajectory planning in kinematically redundant systems [27]. Early works
in global redundancy resolution [28, 29] looked at planning paths which avoid
actuator saturation while experiencing a large fixed load. This has been
extended to consider maximising the allowed load , and incorporation of
dynamical effects . [22] used an evolutionary algorithm combined with multiple
lower level linear programming solutions to find the maximum wrench capability
over a trajectory for a re-configurable parallel robot.

Hamiltonian control approaches which make use of Pontrayagin’s minimum
principle over an entire trajectory [30, 31] have been used for finding
maximum load carrying capacity for mobile manipulators, yet often lead to
switching control laws which are not physically realisable. Additionally,
these methods require solving two point boundary value problems and therefore
are generally restricted to systems with low degrees of redundancy. Recent
work has considered receding horizon style planning using Model Predictive
Control (MPC) to track desired wrenches [32], yet little work has been done to
apply this technique to maximising wrench capability.

##  III Maximising Static Wrench Capability

This section details how to find optimal configurations and actuator efforts
to maximise the wrench capability in a desired direction, for a single end
effector pose and system configuration. Dynamic effects due to velocity and
acceleration are therefore ignored, yet are considered in Section IV. Examples
where static wrench capability is desired is for maximising weight lifting
capability or torque during a valve turning operation during a relatively slow
operations.

Figure 2: Wrench ellipsoid and polytopes for a 2D UVMS showing force
capability in the given configuration and the desired direction $c$ in which
the wrench capability should be maximised

First, the previous definitions of wrench ellipsoids and polytopes have to be
extended to include the overactuated mapping given in equation 7. We can
invert the mapping in equation 8 to get

$u=B^{+}(\tau_{d}+J^{T}h_{e})$ (16)

giving a new definition for the wrench ellipsoid

$\\{h_{e}\;\mid\;\|W_{u}B^{+}(J^{T}h_{e}+\tau_{d})\|_{2}\leq 1\\}$ (17)

where the actuator weighting matrix [14] is given by
$W_{u}=diag[\frac{1}{u_{max_{1}}},...,\frac{1}{u_{max_{m}}}]$ where
$u_{max_{i}}$ is the positive force/torque limit for actuator $i$. In the case
of asymmetric limits as in the case of an underwater thruster, the smallest
absolute value is used. The transmission ratio is also redefined as the
positive root of

$(W_{u}B^{+}(J^{T}h_{e}+\tau_{d}))^{T}(W_{u}B^{+}(J^{T}h_{e}+\tau_{d}))=1$
(18)

for a given desired wrench along direction $c$. Using the inverted actuator
mapping to redefine the wrench polytope gives

$\\{h_{e}\;\mid\;u_{min}\leq B^{+}(J^{T}h_{e}+\tau_{d})\leq u_{max}\\}$ (19)

although $B^{+}$, the pseudoinverse of $B$, does not give a true measure of
the capabilities of the system. Thus this is referred to as the $L_{2}$
polytope, while the $L_{\infty}$ polytope is given by

$\\{h_{e}\;\mid\;(Bu)_{min}\leq J^{T}h_{e}+\tau_{d}\leq(Bu)_{max}\\}$ (20)

which gives a true measure of the actuator capabilities. A simple
2-Dimensional (2D) test case is considered to compare each capability measures
shown in Figure 2. This shows a underwater vehicle with diagonally mounted
thrusters on each corner which can apply 1N thrust. The arm has two joints
each which can apply a torque of 1Nm. The wrench ellipsoid, $L_{2}$ and
$L_{\infty}$ polytopes along the 2D force dimensions with 0 end effector
torque are shown. Clearly the $L_{\infty}$ polytope gives a better measure of
the true wrench capabilities of the system.

Now consider finding the configuration which maximises the wrench capability
of the system shown in Figure 2 in the direction $c$, while keeping the same
end effector pose. A redundancy parameterisation method for UVMS has been
proposed in previous work by the authors [33], and is shown in Figure 3. Given
a desired end effector pose, each DOF in reverse order starting from the end
effector is a redundant DOF until the pose of the base is fully defined, with
invalid poses due to self collision discarded. The redundant configuration
$\theta_{r}$, together with the end effector pose $x$ can be used to determine
the full system configuration $\theta$.

Figure 3: Sequence of diagrams showing method of redundancy parameterisation
for UVMS

For the system considered in Figure 2, there are two redundant DOF
corresponding to the joints on the manipulator, giving a two dimensional
search. Several optimisation objectives can be defined for this optimisation
problem. The first is the transmission ratio given as the positive root of
Equation 18 for the desired wrench direction $c$. This is labelled as
optimisation objective $\beta_{1}$. The second is to maximise the $L_{\infty}$
polytope in the direction of $c$. This can be found by solving a linear
programming problem given by

$\max_{u,h_{e}}\quad c^{T}h_{e}$ (21)

with inequality constraints due to the actuators

$u_{min}\leq u\leq u_{max}$ (22)

and equality constraints due to the force/torque balance

$\tau_{d}+J^{T}h_{e}=Bu$ (23)

Finally there is a constraint on the direction of the applied wrench given by

$(cc^{+}-I_{6})h_{e}=0$ (24)

where $I_{6}$ is a $6\times 6$ identity matrix, which enforces $h_{e}$ to have
no component orthogonal to $c$ in $\mathbb{R}^{6}$. This is labelled as
optimisation objective $\beta_{2}$. Finally, the orthogonal wrench equality
constraint in Equation 24 can be removed in some scenarios. One example is
during a valve turning operation, where reaction forces orthogonal to the
desired wrench maximisation direction can be applied on the valve. This
optimisation objective which relaxes the orthogonality constraint is labelled
$\beta_{3}$. The linear programming problems are solved using the dual-simplex
method in Matlab’s linprog function.

The aim is to find an optimal configuration which maximises each of the
objectives $\beta_{1},\beta_{2}$ and $\beta_{3}$. This results in a bi-level
optimisation framework, where the upper level problem is a search over the
parameterised redundancy space to find the configuration which maximises the
lower level objective. The lower level objective involves finding the optimal
actuator forces/torques to maximise the end effector wrench in the direction
of $c$, using the method of either $\beta_{1},\beta_{2}$ or $\beta_{3}$.

Figure 4: Optimal configurations for a wrench in the $c$ direction for a 2D
UVMS, resulting in different optimal configurations according to the measure
to be optimised

Figure 4 shows the results of optimising for each of these objectives the same
system as in Figure 2 via a grid search over the 2 redundant DOF, as well as
the corresponding ellipsoid or polytopes. The results show that the optimal
configuration changes depending on the objective to be optimised. There is
also a significant difference in wrench capability. Maximising $\beta_{1}$
gives a maximum force of $1.91N$, while $\beta_{2}$ gives $3.47N$ and
$\beta_{3}$ gives $3.88N$. These differences become more pronounced with
higher DOF systems, such as the UVMS used for experiments in Section VI. For
higher dimensional searches, simulated annealing is used to find a solution
with a globally large maximum.

###  III-A Multiple Contact Points

The case of multiple contact points is common when using two or more
manipulators. The reaction forces of the end effector of a second manipulator
on some nearby grasp point can help increase the wrench applied by the first
manipulator. This creates a closed kinematic chain between the UVMS system and
the environment, requiring more careful analysis.

For multiple contact points case, we define $h_{e_{2}}\in\mathbb{R}^{6}$,
which is the end effector wrench of secondary arm, and
$C_{2}\in\mathbb{R}^{6\times l}$, which is a set of $l$ unit vectors which
define the direction in which the second manipulator can apply a wrench
$h_{e_{2}}$. This is explained in more detail below. The force/torque balance
equation is given by

$\tau_{d}+J^{T}(h_{e},h_{e_{2}})^{T}=Bu$ (25)

where the terms $t_{d},J,B$ and $u$ reflect the additional actuators and DOF
of the second manipulator. In order for $(h_{e},h_{e_{2}})^{T}$ to be well
defined, $J\in\mathbb{R}^{12\times n}$ must have full row rank. This condition
is due to the requirement that the system must be able to apply a virtual
displacement for a given wrench to be achievable, and is violated at
kinematically singular configurations. During searches for optimal
configurations, the manipulability measure [10] is used to discard singular or
very near singular configurations.

The redundancy parameterisation for the dual manipulator case is similar to
the single arm case, with the primary arm again defining all degrees of
redundancy until the pose of the base is fully defined. The inverse kinematics
to reach the grasp point for the second arm can then be analytically solved.
In the case of multiple inverse kinematics solutions for the second arm, the
solution which maximises the objective is taken. In case of no solution, the
configuration is considered invalid. A lower level objective can again be
written as a linear program

$\max_{u,h_{e},h_{e_{2}}}\quad c^{T}h_{e}$ (26)

subject to

$u_{min}\leq u\leq u_{max},\;\tau_{d}+J^{T}(h_{e},h_{e_{2}})^{T}=Bu$ (27)
$(cc^{+}-I)h_{e}=0,\;(C_{2}C_{2}^{+}-I)h_{e_{2}}=0$ (28)

where as before there is an equality constraint which ensures $h_{e}$ has no
component orthogonal to $c$. The final equality constraint ensures $h_{e_{2}}$
only has components in allowed directions defined in $C_{2}$. For example,
when firmly grasping a contact point with the secondary manipulator, it might
be feasible to set $C_{2}=I_{6}$, meaning any secondary wrench is allowed. If
the secondary manipulator is pushing against a flat surface, then $C_{2}$ may
only contain the vector normal to the surface, and the optimisation would
include an additional inequality constraint to allow only pushing in one
direction.

$C_{2}h_{e_{2}}\geq 0$ (29)

The above linear program again is the lower level problem for the bi-level
optimisation, this time for finding the optimal configuration which considers
multiple contact points and manipulators.

Figure 5: Parametrised regrasp points for second manipulator adding an
additional kinematic variable $\theta_{n+1}$ Figure 6: Diagram of vehicle
manipulator system showing the starting pose, intermediate poses and ending
pose along a rotating end effector trajectory during a pipe waterjet blasting
operation. The directions in which the force resisting capability should be
maximised for each end effector pose are shown as red arrows.

###  III-B Re-grasping

The above case considered grasping a fixed point with a second arm. Generally
multiple grasping points will be available. We consider cases where these
regrasp points can be parameterised to provide extra dimensions over which to
optimise. Figure 5 shows a simple 2D example, where a one dimensional set of
parameterised re-grasping points result in an additional kinematic degree of
freedom which can be optimised over. Section VI describes another
parametrisation of re-grasping points.

##  IV Wrench Maximisation over a Trajectory

In this section, a predefined end effector trajectory is considered, with
corresponding desired wrenches along the path. Only a UVMS with a single
manipulator is considered, yet the analysis can be easily extended to multiple
manipulators. Use cases for this method may be during a valve turning
operation which requires maximising torque along the same direction
throughout, or a waterjet blasting operation around a pipe which requires
resisting large forces normal to the pipe surface along the path, shown in
Figure 6. Dynamical effects due to velocities and accelerations can not be
ignored in this case. It is assumed there is a given fully defined end
effector pose trajectory parameterised in time, and that the velocities at the
start and end are zero. To maximise the capability of the system along this
path, the objective is to maximise the minimum wrench along this trajectory.

###  IV-A Problem Formulation

In order to make the problem tractable, the trajectory of the end effector is
discretised into a set of $k$ successive poses $x_{1},...x_{k}$, written as
stacked vector $\boldsymbol{x}$, with corresponding redundant configurations
$\theta_{r,1},...\theta_{r,k}$ written as $\boldsymbol{\theta_{r}}$, giving
corresponding system configurations $\theta_{1},...\theta_{k}$, written as
$\boldsymbol{\theta}$. Each point along the trajectory of total time $T$ is
equally separated by $\Delta t=T/k$. At each end effector pose is a
corresponding end effector velocity $\dot{x}_{1},...\dot{x}_{k}$ written as
$\boldsymbol{\dot{\theta}}$, which is computed using the timestep and
differences between successive poses in $SE(3)$. Dynamic quantities are
computed using the finite difference operator

$\mathcal{D}_{i}=\frac{1}{\Delta t}\left(\begin{array}[]{ccccc}-1&1&&&\\\
&-1&1&&\\\ &&\ddots&\ddots&\\\ &&&-1&1\\\ &&&&0\end{array}\right)\otimes
I_{i\times i}$ (30)

where $\otimes$ represents the Kronecker product and $I_{i\times i}$ is the
${i\times i}$ identity matrix. Now we can rewrite Equation 15 as

$\dot{\boldsymbol{\theta}}=\boldsymbol{J_{e}}\boldsymbol{\dot{x}}+\boldsymbol{A_{r}}\dot{\boldsymbol{\theta_{r}}}=\boldsymbol{J_{e}}\boldsymbol{\dot{x}}+\boldsymbol{A_{r}}\mathcal{D}_{n-m}\boldsymbol{\theta_{r}}$
(31) $\ddot{\boldsymbol{\theta}}=D_{n}\dot{\boldsymbol{\theta}}$ (32)

where $\boldsymbol{J_{e}}$ and $\boldsymbol{A_{r}}$ are the stacked matrices
formed from $J_{e}$ and $A_{r}$ respectively at each configuration. At each
timestep, the wrench is given by $h_{e,1},...h_{e,k}$, written as
$\boldsymbol{h_{e}}$, and actuator efforts $u_{1},...u_{k}$, written as
$\boldsymbol{u}$. Each timestep also has a unit vector $c_{1},...c_{k}$ along
which the wrench should be maximised, written as $\boldsymbol{c}$. The aim is
to solve for a set of redundant configurations along the trajectory which
maximises the minimum wrench capability, a max-min optimisation.

Given a set of configurations $\boldsymbol{\theta}$, the max-min wrench
capability can be solved as a Linear Program (LP) over the entire trajectory,
with objective

$\max\;t$ (33)

subject to

$t<\boldsymbol{c}^{T}\boldsymbol{h_{e}}$ (34)

where $t$ represents the minimum wrench capability. The equality constraints
are given by the dynamics balance equations at each timestep $i$ are given by

$\tau_{d,i}+J_{i}^{T}h_{e,i}=Bu_{i}$ (35)

Since the terms in $\tau_{d}$ are fully determined by $\theta,\dot{\theta}$
and $\ddot{\theta}$, the dynamic terms can be predetermined over the entire
trajectory independently of $h_{e}$ and $u$. There are inequality constraints

$\boldsymbol{u_{min}}\leq\boldsymbol{u}\leq\boldsymbol{u_{max}}$ (36)

where $\boldsymbol{u_{min}}$ and $\boldsymbol{u_{max}}$ are the stacked
vectors of minimum and maximum actuator efforts respectively. The orthogonal
wrench constraint is again given by

$(\boldsymbol{c}\boldsymbol{c}^{+}-I_{6k})\boldsymbol{h_{e}}=0$ (37)

As before, this constraint can be relaxed under certain conditions. Finally
there is a constraint on large changes to actuator efforts throughout the
trajectory given by

$-\Delta\boldsymbol{u_{min}}\Delta t\leq
D_{mk}\boldsymbol{u}\leq\Delta\boldsymbol{u_{min}}\Delta t$ (38)

where $\Delta\boldsymbol{u_{min}}$ is the maximum allowed actuator change per
second. This accounts for the relatively slow actuator dynamics of underwater
thrusters. The solution to this LP is written as $f(\boldsymbol{\theta_{r}})$
and is a function of the redundant configurations along the trajectory
$\boldsymbol{\theta_{r}}$. Again the LP is solved using the dual-simplex
method in Matlab’s linprog function. The trajectory optimisation problem is
given by

$\boldsymbol{\theta_{r}}_{opt}=\arg\min_{\boldsymbol{\theta_{r}}}-f(\boldsymbol{\theta_{r}})$
(39)

where $\boldsymbol{\theta_{r}}_{opt}$ is the optimal set of redundant
configurations along the trajectory. There are constraints on the
configurations

$\boldsymbol{\theta_{r}}_{min}\leq\boldsymbol{\theta_{r}}\leq\boldsymbol{\theta_{r}}_{max}$
(40)

where $\boldsymbol{\theta_{r}}_{min}$ and $\boldsymbol{\theta_{r}}_{max}$ are
the minimum and maximum limits on the configurations, and constraints on the
velocities

$\boldsymbol{\dot{\theta_{r}}}_{min}\leq\boldsymbol{\dot{\theta_{r}}}\leq\boldsymbol{\dot{\theta_{r}}}_{max}$
(41)

where $\boldsymbol{\dot{\theta_{r}}}_{min}$ and
$\boldsymbol{\dot{\theta_{r}}}_{max}$ are the minimum and maximum limits on
the velocities. This is again a bi-level optimisation problem, with the linear
program in Equation 33 as the lower level optimisation. Changes to
$\boldsymbol{\theta_{r}}$ effect only the dynamics terms
$M,h,g,\dot{\theta},\ddot{\theta}$ and the Jacobian $J_{i}$ in Equation 35.
Therefore only the equality constraints are changed with changes in
$\boldsymbol{\theta_{r}}$. Using the Karush-Kuhn-Tucker (KKT) conditions, the
gradient of $f(\boldsymbol{\theta_{r}})$ with respect to
$\boldsymbol{\theta_{r}}$ can be found as

$\frac{df(\boldsymbol{\theta_{r}})}{d\boldsymbol{\theta_{r}}}=\lambda_{eq}^{T}\frac{dc_{eq}}{d\boldsymbol{\theta_{r}}}\Bigr{|}_{\begin{subarray}{c}(u^{*},h_{e}^{*})\end{subarray}}$
(42)

where $\lambda_{eq}$ is the vector of Lagrange multipliers associated with the
equality constraints, and the final term on the right is the gradient of the
equality constraints $c_{eq}$, evaluated with the arguments of the solution to
the lower level LP problem $(u^{*},h_{e}^{*})$. This is a non-convex objective
with multiple local minima. An interior-point solver using the Matlab function
fmincon is used, initiated at several different starting points in an attempt
to find a good global minimum.

Given an dynamically infeasible trajectory due to large dynamics terms, the
lower level LP can not find a feasible solution which satisfies the
constraints in Equation 35. In this case, the lower level problem is set to
return a max-min wrench of
$f(\boldsymbol{\theta_{r}})=\frac{1}{2}\tau_{d}^{T}\tau_{d}$, and gradient
$\tau_{d}$ to the upper level problem, pushing the optimiser towards
dynamically feasible trajectories.

###  IV-B Tracking Dynamic Trajectories

The above method finds a trajectory with smoothly varying actuator efforts,
which maximises the minimum wrench in a given desired direction along the
entire trajectory. The actual reaction forces and torques between the end
effector and the environment during this trajectory will not be the same as
those in the LP solution. Since the actuator constraints form a convex set,
any value for $c^{T}h_{e}$ less than the maximum found by the optimisation
will still fall within the feasible set. Additionally, assuming smooth changes
in the interaction wrenches with the environment, and given the linear mapping
between wrenches and each DOF, the smoothness properties are also conserved.
The problem is finding the actuator efforts at a given timestep to track the
desired trajectory, given that the actual efforts $\boldsymbol{u}$ may not
match the LP solution. At a given timestep $i$, the control effort $\tau_{c}$
to track the trajectory is computed using a appropriate controller [33]. Given
the overactuated system, the objective is to find the actuator efforts $u_{c}$
which are close to the optimal LP solution $u_{i}^{*}$, posed as a quadratic
cost

$\min_{u_{c}}(u_{c}-u_{i}^{*})^{T}Q(u_{c}-u_{i}^{*})$ (43)

with equality constraint

$Bu_{c}=\tau_{c}$ (44)

which is simply the actuator model. Using the method of Lagrange multipliers
this has solution

$u_{c}^{*}=u_{i}+B_{Q}^{+}(\tau_{c}-Bu_{i})$ (45)

where $B_{Q}^{+}$ is the weighted pseduoinverse given by

$B_{Q}^{+}=Q^{-1}B^{T}(BQ^{-1}B^{T})^{-1}$ (46)

This results does not incorporate the inequality constraints on the actuators.
These are

$u_{min}\leq u_{i}\leq u_{max}$ (47)

which account for actuator effort limits and

$-\Delta u_{min}\Delta t\leq D_{m}u_{i}\leq\Delta u_{min}\Delta t$ (48)

which account for actuator effort rate change limits. This is a quadratic
programming problem, which can be solved efficiently in real time for control.

##  V Maximum Wrench Impulse

This section looks at generating a maximum momentary wrench for a fixed end
effector pose, using dynamic vehicle-manipulator motions while keeping the end
effector fixed. A use case for this method may be for shifting a very heavy
weight or a stuck valve. Humans naturally perform these motions for similar
tasks, where the momentum of the person is used to momentarily generate large
forces.

###  V-A Problem Formulation

For a given fixed end effector pose, and set of redundant configurations which
define a dynamic trajectory, the maximum wrench problem is given by

$f=\max_{\boldsymbol{u},\boldsymbol{h_{e}}}\;\max(\boldsymbol{c}^{T}\boldsymbol{h_{e}})$
(49)

which is a linear $max-max$ problem. These problems can be reformulated as a
Mixed Integer Linear Program (MILP) using the big-M method, although this is
no longer a convex optimisation problem. Instead, the maximum wrench impulse
can be chosen to occur at an arbitrarily chosen specific timestep $t_{h}$

$f=\max_{\boldsymbol{u},\boldsymbol{h_{e}}}\;\max(c_{t_{h}}^{T}h_{e,t_{h}})$
(50)

All constraints from the the dynamic wrench maximisation in Equations 35\- 38
also apply. This is a linear program which again is the lower level problem
for the bi-level trajectory optimisation with upper level given by Equation
39, with the solution and gradients found in the same way as before.

##  VI Results

We present the wrench maximisation results for the 4 DOF manipulator and 6DOF
vehicle shown in Figure 1. This systems has a degree of redundancy of 4. Two
wrench objectives are compared, torque along the z direction simulating
turning a valve, and force in the z direction which tests lifting a heavy
load. The torque limit on each joint is $[\pm 9,\pm 9,\pm 9,\pm 2]Nm$ and each
thruster has an asymmetrical thrust limit of -40N to 50N. The force of each
thruster can vary by 50N/s, while the rate of change of joint torque is
effectively instantaneuous. Six load cells were attached in series shown in
Figure 1, allowing all six components of the applied wrench to be measured
simultaneously. The sensor setup was calibrated using known masses and levers.

###  VI-A Static Wrench Capability

For the case of static torque maximisation in the z direction, four
configurations are compared, shown in Figure 7. These first is the default
configuration, which is simply the manipulator in a neutral position with the
vehicle upright. The other three cases are the optimised configurations for
$\beta_{1},\beta_{2}$ and $\beta_{3}$ as described in section III, with the
optimal thruster forces and manipulator torques computed accordingly.

Figure 7: Diagrams showing optimal static configuration for torque in the z
direction, comparing the default upright case, to the solutions which optimise
$\beta_{1}$, $\beta_{2}$ and $\beta_{3}$

Figure LABEL:fig:_staticTorque shows the torque along the z direction as
measured by the sensor setup during the experiments for each of the four
configurations. Each experiment consists of a 5 seconds ramp-up and ramp-down
and a 5 second maximum wrench.

Figure 8: Torque along the z direction during static wrench maximisation,
comparing the default, $\beta_{1}$, $\beta_{2}$ and $\beta_{3}$

The default case is limited to a torque of 9Nm, since the z axis is aligned
with the base joint of the manipulator in this case. Optimising for the
transmission ratio $\beta_{1}$ results in a maximum torque of approximately
19.5Nm, while optimising using the maximum wrench polytope $\beta_{2}$ gives a
maximum of 25Nm. Optimising for the maximum wrench polytope with the
constraint on orthogonal wrench components removed leads to a maximum torque
of 76Nm. The inconsistent torque readings during the fixed maximum thrust
period are due to the effects of swirling water after several seconds in the
relatively small test tank.

The default, $\beta_{1}$ and $\beta_{2}$ cases all have constraints on
wrenches orthogonal to torque along the z direction. The $\beta_{3}$ case
relaxes this constraint and has significant orthogonal components. Figure 9
shows each of the 3 orthogonal forces and two orthogonal torques during the
the experiment. All the orthogonal components during the $\beta_{2}$
experiment are relatively small, with a peak of around 10Nm along the y
direction due to flexible strain in the sensor and UVMS setups. In contrast,
the $\beta_{3}$ experiment has significant forces of around 80N and torque of
around 50Nm. These may be acceptable in a case such as turning a valve, where
the valve can resist these orthogonal wrenches.

Figure 9: Forces in xyz and torques in xy during static wrench maximisation in
the z direction, comparing the default, $\beta_{2}$ and $\beta_{3}$

For the case of static force maximisation in the z direction, three
configurations are compared, shown in Figure 10. Again the first is the
default configuration, followed by optimal configurations for $\beta_{1}$ and
$\beta_{2}$. In this case testing for $\beta_{3}$ is not valid, as the weight
should lifting vertically up and not accelerated in orthogonal directions.
This test was performed using known weights, and the successfully lifted
masses are shown in Figure 10. The default case can lift a maximum of 4kg, the
optimisation using $\beta_{1}$ can lift 7kg, and the optimisation using
$\beta_{2}$ can lift 10kg.

Figure 10: Experiments comparing comparing the default, $\beta_{1}$ and
$\beta_{2}$ force maximisation cases, showing the lifted mass. Each test mass
is attached to a safety weight to keep the system constrained.

The results for both the torque maximisation and force maximisation
experiments shows significant increases of approximately 30% and 40%
respectively in wrench capability when comparing optimisation using the
transmission ratio $\beta_{1}$, as compared to the proposed method of
optimising the directional wrench polytope using a bi-level optimisation with
a LP lower level problem $\beta_{2}$. Additionally, in the case when the
orthogonal wrench constraints can be removed, the experimental torque
capability increased threefold.

Figure 11: Experimental setup for multiple contacts points. A fixed bar is
used as the primary manipulator, with end effector pose $x_{1}$, and a 4DOF
manipulator is used as the secondary manipulator with end effector pose
$x_{2}$. The secondary manipulator is contacting the side of the tank where a
normal force into the wall can be applied, labelled as $h_{e_{2}}$. Possible
regrasping points for the second manipulator which provide additional DOF over
which to optimise are labelled as $\theta_{n+1}$ and $\theta_{n+2}$

###  VI-B Multiple Contact Points

The experimental setup for multiple contact point is shown in Figure 11. A
fixed bar is used as the primary manipulator with end effector $x_{1}$, and is
attached to the 6 axis force-torque sensor setup. A 4DOF manipulator is used
as the secondary manipulator with end effector pose $x_{2}$, which is shown in
contact with the side of the tank where a normal force can be applied. The
first constraint in 28 is also applied, limiting the wrench applied at $x_{1}$
to a pure torque in the z direction. It is assumed all forces and torques at
$x_{2}$ are 0 except for a positive normal force into the wall. Therefore
$C_{2}$ only has one component and the second constraint in 28 applies, as
well as in inequality constraint in Equation 29 to ensure the contact force is
into the wall.

Figure 12: Default and optimised configurations and end effector poses for a
secondary contact point during torque maximisation Figure 13: Torque along the
z direction during static wrench maximisation comparing single manipulator,
dual manipulators in the default end effector pose, and dual manipulators in
the optimised end effector pose

Figure 11 shows the set of possible regrasping points which provide two
additional DOF over which to optimise. Figure 13 shows the results for torque
maximisation along the z direction, comparing the single and dual manipulator
cases. For the dual manipulator case, both the default end effector pose and
the optimised end effector pose are tested, shown in Figure 12. The single
manipulator torque reaches a maximum of 23Nm, the default dual case reaches
26Nm, and the optimised dual case reaches 30Nm, an increase of 13% and 30%
respectively. The data for this experiment contains more noise since the
vehicle is very close to the edge of the tank by necessity, causing large
effects from swirling water in the tank. These effects may be significant near
solid structures in underwater environment, requiring additional modelling
which is left as future work.

###  VI-C Wrench Maximisation over a Trajectory

The experimental setup for testing wrenches over a rotating end effector
trajectory along the global z axis is shown in Figure 14. A high torque servo
motor above the water line is used to generate the controlled rotation, and is
attached to a shaft which reaches into the tank. The end effector of the UVMS
attaches to a jaw coupling on the end of the shaft, and the whole setup is
mounted to the end of the 6 axis force-torque sensor.

Figure 14: Setup for testing dynamics wrenches over a trajectory, suing a
servo motor and attached shaft to simulate a turning valve Figure 15: Plots
during the rotating end effector trajectory showing (top) the end effector
angle, (middle) vehicle thrust forces, and (bottom) redundant joint angles

The objective which is tested is maximising the torque along the z axis with
no orthogonal wrench components, while the end effector tracks a $180^{\circ}$
rotating trajectory. The trajectory is shown in the top plot of Figure 15, and
is generated as a cubic spline with a velocity of 0 at each end point, with a
total time of 5 seconds. 10 points along the trajectory are considered,
therefore $T=10$ for the trajectory optimisation problem described in Section
IV. The values for each joint angle and thruster force were interpolated
linearly between each timestep when sending commands to the UVMS during the
experiment. The middle plot in Figure 15 shows the thruster forces throughout
the trajectory, showing the effect of the constraint in Equation 38 which
limits large changes in actuator effort. Finally the bottom plot in Figure 15
shows the joint angles which correspond to the redundant DOF, showing a
relatively smooth change in redundant configuration throughout the rotation.

Figure 16 shows a sequence of images of the UVMS during the experiment at at
$0^{\circ}$, $90^{\circ}$ and $180^{\circ}$, showing the changing joint angles
throughtout.

Figure 16: A sequence of images during the 5 second $180^{\circ}$ rotating
trajectory, showing the changing manipulator joint angles during the rotation.
The images are at $0^{\circ}$, $90^{\circ}$ and $180^{\circ}$

Figure 17 shows the results of the torque along the z axis, comparing the
dynamic case which tracks the optimised trajectory, and the static case which
maintains a fixed redundant configuration throughout. The static redundant
configuration which is chosen is the configuration which maximises the static
torque capability using $\beta_{2}$. The static case achieves a minimum torque
of 16Nm throughout the 5 second rotation period, while the dynamic trajectory
achieves a minimum torque of 19Nm, a 19% increase. The results show that
consideration of the dynamical effects during the rotating end effector
trajectory leads to a significant increase in the max-min torque.

Figure 17: Torque along the z direction during a dynamic rotating end effector
trajectory, comparison the static and dynamic redundant configurations cases

Since this rotation if about the vertical axis, the effects of changing
gravity and buoyancy vectors in the UVMS frame due to the end effector
rotation are not present. Therefore the improved performance of the dynamic
case as compared to the static case is purely due to consideration of
dynamical effects from velocity and acceleration. The difference in
performance between the dynamic trajectory optimised case and the static case
are likely to be much more significant with end effector trajectories which
include rotations about the non-vertical axis.

###  VI-D Maximum Wrench Impulse

Figure 18: Plots during the maximum wrench impulse trajectory showing (top)
the vehicle thrust forces, and (bottom) redundant joint angles

The experimental setup for maximum wrench impulse trajectories is the same as
the static wrench maximisation setup shown in Figure 1. Again the objective is
to maximise the torque along the z direction with no orthogonal wrench
components, in this case by using dynamic motions to generate a large torque
impulse momentarily, while keeping the end effector fixed. A trajectory of
2.5s with 4 intermediate configurations was chosen, therefore $T=4$ for the
trajectory optimisation problem described in Section V. The 3rd timestep is
chosen as the point of maximum torque, therefore $t_{h}=3$. As before, values
for each joint angle and thruster force are interpolated linearly between each
timestep when sending commands to the UVMS during the experiment. Figure 18
shows the results of the trajectory optimisation, with the top plot showing
the thruster forces throughout the trajectory, and the bottom plot shows the
joint angles. The resulting trajectory leads to both joint velocities and
thruster rates of change at the limits set in the optimisation problem,
leading to a highly dynamic trajectory. Figure 19 shows a sequence of images
during the trajectory, and Figure 20 shows a plot of the measured torque
results. The plot compares the static case which is identical to the
$\beta_{2}$ optimised results from Figure 8, compared to the dynamic impulse
case. The dynamic trajectory generated a torque impulse of 35Nm, which is a
40% torque improvement over the best static results with orthogonal wrench
constraints.

Figure 19: A sequence of images during the 3 second maximum impulse trajectory
Figure 20: Torque along the z direction during a heaving motion, compared to
the static $\beta_{2}$ case

##  VII Conclusion

This work looks at maximising the maximum wrench capability of UVMS. A bi-
level optimisation method is proposed for maximising static wrenches, and
experimental results show a significant improvement over optimising the
transmission ratio. Further results show that relaxing constrains on
orthogonal wrenches leads to significant increases in wrench capability in
relevant use cases. The case of multiple contact points, as well as re-
grasping of secondary points is also considered, with experimental results
again showing increased wrench capability. A similar bi-level optimisation
approach is introduced for max-min wrench optimisation over a trajectory, with
experimental results confirming the validity of the method. Finally, a method
is proposed for finding dynamic trajectories which generate large wrench
impulses, with supporting experimental results. Further work is required for
dealing with the effects of self-generated currents by the vehicle thrusters
when operating near underwater structures. Additional work would look at
automatic recognition of viable secondary contact points.

## ACKNOWLEDGMENT

This work has been enabled by use of a Reach Alpha manipulator from Blueprint
Lab (https://blueprintlab.com/)

## References

  * [1] C. A. Klein and C. Huang, “Review of pseudoinverse control for use with kinematically redundant manipulators,” _IEEE Transactions on Systems, Man, and Cybernetics_ , vol. SMC-13, no. 2, pp. 245–250, March 1983.
  * [2] P. Cieslak, P. Ridao, and M. Giergiel, “Autonomous underwater panel operation by girona500 uvms: A practical approach to autonomous underwater manipulation,” in _2015 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2015, pp. 529–536.
  * [3] G. Antonelli, _Underwater Robots: Motion and Force Control of Vehicle-Manipulator Systems_ , 2005, vol. 2.
  * [4] O. Khatib, “A unified approach for motion and force control of robot manipulators: The operational space formulation,” _IEEE Journal on Robotics and Automation_ , vol. 3, no. 1, pp. 43–53, February 1987.
  * [5] G. Antonelli, S. Chiaverini, and N. Sarkar, “External force control for underwater vehicle-manipulator systems,” _IEEE Transactions on Robotics and Automation_ , vol. 17, no. 6, pp. 931–938, 2001.
  * [6] D. Busson and R. Béarée, “A pragmatic approach to exploiting full force capacity for serial redundant manipulators,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 2, pp. 888–894, April 2018.
  * [7] S. Sivčev, J. Coleman, E. Omerdić, G. Dooly, and D. Toal, “Underwater manipulators: A review,” _Ocean Engineering_ , vol. 163, pp. 431–450, 2018\. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S0029801818310308
  * [8] L. Bezanson, S. Reed, E. M. Martin, J. Vasquez, and C. Barbalata, “Coupled control of a lightweight rov and manipulator arm for intervention tasks,” in _OCEANS 2017 - Anchorage_ , 2017, pp. 1–6.
  * [9] I. Carlucho, D. W. Stephens, and C. Barbalata, “An adaptive data-driven controller for underwater manipulators with variable payload,” _Applied Ocean Research_ , vol. 113, p. 102726, 2021. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S0141118721002030
  * [10] T. Yoshikawa, “Manipulability of robotic mechanisms,” _The International Journal of Robotics Research_ , vol. 4, no. 2, pp. 3–9, 1985.
  * [11] Y. Zhang, X. Yan, D. Chen, D. Guo, and W. Li, “Qp-based refined manipulability-maximizing scheme for coordinated motion planning and control of physically constrained wheeled mobile redundant manipulators,” _Nonlinear Dynamics_ , vol. 85, 07 2016.
  * [12] J. Bae, J. Bak, S. Jin, T. Seo, and J. Kim, “Optimal configuration and parametric design of an underwater vehicle manipulator system for a valve task,” _Mechanism and Machine Theory_ , vol. 123, pp. 76 – 88, 2018. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S0094114X17315380
  * [13] M. Faroni, M. Beschi, A. Visioli, and L. Molinari Tosatti, “A global approach to manipulability optimisation for a dual-arm manipulator,” in _2016 IEEE 21st International Conference on Emerging Technologies and Factory Automation (ETFA)_ , 2016, pp. 1–6.
  * [14] H. Xing, A. Torabi, L. Ding, H. Gao, Z. Deng, and M. Tavakoli, “Enhancement of force exertion capability of a mobile manipulator by kinematic reconfiguration,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 4, pp. 5842–5849, 2020.
  * [15] B. Nemec, “Force control of redundant robots,” _IFAC Proceedings Volumes_ , vol. 30, no. 20, pp. 209 – 214, 1997, 5th IFAC Symposium on Robot Control 1997 (SYROCO ’97), Nantes, France, 3-5 September.
  * [16] A. Ajoudani, N. G. Tsagarakis, and A. Bicchi, “Choosing poses for force and stiffness control,” _IEEE Transactions on Robotics_ , vol. 33, no. 6, pp. 1483–1490, 2017.
  * [17] A. Bowling and O. Khatib, “The dynamic capability equations: a new tool for analyzing robotic manipulator performance,” _IEEE Transactions on Robotics_ , vol. 21, no. 1, pp. 115–123, 2005.
  * [18] P. Chiacchio, Y. Bouffard-Vercelli, and F. Pierrot, “Evaluation of force capabilities for redundant manipulators,” in _Proceedings of IEEE International Conference on Robotics and Automation_ , vol. 4, 1996, pp. 3520–3525 vol.4.
  * [19] F. Firmani, A. Zibil, S. B. Nokleby, and R. P. Podhorodeski, “Wrench capabilities of planar parallel manipulators. part i: Wrench polytopes and performance indices,” _Robotica_ , vol. 26, no. 6, pp. 791–802, 11 2008, copyright - Copyright © Cambridge University Press 2008; Last updated - 2015-08-15.
  * [20] R. Boudreau, S. Nokleby, and M. Gallant, “Wrench capabilities of a kinematically redundant planar parallel manipulator,” _Robotica_ , vol. 39, no. 9, p. 1601–1616, 2021.
  * [21] P. Buttolo and B. Hannaford, “Advantages of actuation redundancy for the design of haptic displays,” 01 1995.
  * [22] C.-J. Lin and C.-T. Chen, “Reconfiguration for the maximum dynamic wrench capability of a parallel robot,” _Applied Sciences_ , vol. 6, no. 3, 2016\. [Online]. Available: https://www.mdpi.com/2076-3417/6/3/80
  * [23] D. Busson, R. Bearee, and A. Olabi, “Task-oriented rigidity optimization for 7 dof redundant manipulators,” _IFAC-PapersOnLine_ , vol. 50, no. 1, pp. 14 588 – 14 593, 2017, 20th IFAC World Congress.
  * [24] G. Nützi, X. Feng, R. Siegwart, and S. Kock, “Optimal contact force prediction of redundant single and dual-arm manipulators,” _Journal of the Chinese Society of Mechanical Engineers_ , vol. 43, pp. 251–258, 06 2013.
  * [25] R. Boudreau and S. Nokleby, “Force optimization of kinematically-redundant planar parallel manipulators following a desired trajectory,” _Mechanism and Machine Theory_ , vol. 56, pp. 138 – 155, 2012.
  * [26] K. Kazerounian and Z. Wang, “Global versus local optimization in redundancy resolution of robotic manipulators,” _The International Journal of Robotics Research_ , vol. 7, no. 5, pp. 3–12, 1988. [Online]. Available: https://doi.org/10.1177/027836498800700501
  * [27] K. Suh and J. Hollerbach, “Local versus global torque optimization of redundant manipulators,” in _Proceedings. 1987 IEEE International Conference on Robotics and Automation_ , vol. 4, 1987, pp. 619–624.
  * [28] E. Papadopoulos and Y. Gonthier, “Large force-task planning for mobile and redundant robots,” in _Proceedings 1995 IEEE/RSJ International Conference on Intelligent Robots and Systems. Human Robot Interaction and Cooperative Robots_ , vol. 2, 1995, pp. 144–149 vol.2.
  * [29] ——, “On manipulator posture planning for large force tasks,” in _Proceedings of 1995 IEEE International Conference on Robotics and Automation_ , vol. 1, 1995, pp. 126–131 vol.1.
  * [30] M. H. Korayem, A. Nikoobin, and V. Azimirad, “Maximum load carrying capacity of mobile manipulators: optimal control approach,” _Robotica_ , vol. 27, no. 1, p. 147–159, 2009.
  * [31] M. H. Korayem, V. Azimirad, H. Vatanjou, and A. H. Korayem, “Maximum load determination of nonholonomic mobile manipulator using hierarchical optimal control,” _Robotica_ , vol. 30, no. 1, p. 53–65, 2012.
  * [32] A. Wahrburg and K. Listmann, “Mpc-based admittance control for robotic manipulators,” in _2016 IEEE 55th Conference on Decision and Control (CDC)_ , 2016, pp. 7548–7554.
  * [33] W. J. Marais, S. B. Williams, and O. Pizarro, “Anisotropic disturbance rejection for kinematically redundant systems with applications on an uvms,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 4, pp. 7017–7024, 2021\.