Maximising the wrench capability of mobile manipulators with experiments on a UVMS Authors: Wilhelm J. Marais, Oscar Pizarro, Stefan B. Williams Published: 2025 ABSTRACT This paper presents methods for finding optimal configurations and actuator forces/torques to maximise contact wrenches in a desired direction for underwater vehicle manipulator systems (UVMS). The wrench maximisation problem is formulated as a bi-level optimisation problem, with upper-level variables in a low-dimensional parameterised redundancy space, and a linear lower-level problem. We additionally consider the cases of one or more manipulators with multiple contact forces, maximising the wrench capability while tracking a trajectory and generating large wrench impulses using dynamic motions. The specific cases of maximising force to lift a heavy load and maximising torque during a valve-turning operation are considered. Extensive experimental results are presented using a 6 degree of freedom (DOF) underwater robotic platform equipped with a 4DOF manipulator and show significant increases in the wrench capability compared to existing methods for mobile manipulators. 1 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 DOFs 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 (Klein and Huang, 1983). Secondary objectives may include obstacle avoidance, optimisation of dynamic manipulability, and avoidance of configurations limits (Cieslak et al., 2015; Antonelli, 2005). During interaction tasks with objects or the environment, end effector poses and contact wrenches are of importance (Khatib, 1987; Antonelli et al., 2001). 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 (Busson and Béarée, 2018). 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 (Sivčev et al., 2018). In recent years, the emergence of smaller and lower cost commercial off-the-shelf underwater vehicles and manipulators has seen a transition to small vehicles for some interaction tasks (Bezanson et al., 2017; Carlucho et al., 2021). This reduces both operation costs and time and increases the accessibility of these systems since they can be launched, operated, and recovered by a small team without specialised equipment. This work improves the capability of these small systems, allowing them to perform a larger range of interaction tasks. Specifically, we consider maximising contact wrenches between the end effector and the environment. 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. We compare a number of cases whose wrench capabilities should be maximised. The first is 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 the wrench capability compared to previous methods for UVMS and other mobile manipulators. This paper presents the following contributions: • A bi-level optimisation approach for finding optimal configurations and actuator efforts for maximising wrenches for UVMS. 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 the maximum wrench capability. • An optimisation method 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 for generating whole-body trajectories which maximise wrenches over a pre-defined end effector trajectory, with experimental results confirming the validity of the proposed method. • A proposed method for generating dynamic motions with a fixed end effector pose for generating large wrench impulses, with experimental results showing the validity of the approach. • A bi-level optimisation approach for finding optimal configurations and actuator efforts for maximising wrenches for UVMS. Experimental results show this provides significantly larger wrenches than existing transmission ratio optimisation methods. • A bi-level optimisation approach for finding optimal configurations and actuator efforts for maximising wrenches for UVMS. 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 the maximum wrench capability. • Consideration of relaxing constraints on orthogonal wrenches for certain tasks, with experimental results showing a threefold increase in the maximum wrench capability. • An optimisation method 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. • An optimisation method 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 for generating whole-body trajectories which maximise wrenches over a pre-defined end effector trajectory, with experimental results confirming the validity of the proposed method. • A bi-level optimisation method for generating whole-body trajectories which maximise wrenches over a pre-defined end effector trajectory, with experimental results confirming the validity of the proposed method. • A proposed method for generating dynamic motions with a fixed end effector pose for generating large wrench impulses, with experimental results showing the validity of the approach. • A proposed method for generating dynamic motions with a fixed end effector pose for generating large wrench impulses, with experimental results showing the validity of the approach. The rest of this paper is structured as follows: Section 2 provides a recap of the mathematical background and current methods for wrench analysis and finding optimal configurations for wrench maximisation. Section 3 describes the methods proposed in this work for maximising the static wrench capability for UVMS, including consideration of multiple contact points. Section 4 extends the methods used for static analysis to consider dynamic trajectories, given an end effector path. Section 5 describes the proposed method for generating large wrench impulses using dynamic motions. Finally, Section 6 presents experimental results for each section, followed by concluding remarks and directions for future work in Section 7. 2 Background and related work 2.1 Kinematic and dynamic modelling A vehicle manipulator system has system configuration θ=(η,q)T , with dim(θ)=n , where η∈SE(3) is the vehicle pose in the world frame and q∈Rn−6 represents the manipulator joint angles. These are shown in Figure 1. Given some end effector pose x∈SE(3) , in the world frame, the forward non-linear map fk gives Equation 1 x=fkθ, (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 Equation 2 x˙=Jθθ˙, (2) where x˙∈R6 is the end effector velocity vector in the inertial frame, θ˙∈Rn is the vector of system velocities corresponding to each DOF, and J(θ) is the configuration-dependent Jacobian. The θ dependence for J is dropped in further notation. For kinematically redundant systems, this has a least squares solution for a desired x˙ given by Equation 3 θ˙=J†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 a 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 θ˙=J†x˙+αI−J†J∇h, (4) where α is a scaling factor and h represents some secondary objective to be optimised in the null space (Klein and Huang, 1983). The null space projection (I−J†J) can be assumed as a set of (possibly non-linearly independent) basis vectors, which is tangent to the inverse kinematics equality constraint. UVMS used in experiments with a 6DOF BlueROV vehicle and 4DOF Reach Robotics Alpha manipulator, showing vehicle pose η and end effector pose x relative to the world frame {W} . Manipulator joints are labelled q1 to q4 . Now, we have the end effector wrench vector in Equation 5 he=fe0ne0∈R6, (5) consisting of end effector forces fe0 and torques ne0 in the inertial frame, with the force relationship τh=JThe, (6) where τh∈Rn represents the vector of forces and torques on each DOF due to he . The actuator model is given by Bu (Antonelli, 2005), where u∈[umin,umax]∈Rm is the control input vector for each actuator, and the matrix B∈Rn×m is the mapping between the actuator control input and the resultant force/torque on each DOF of the system. Including the dynamics of the system gives (Antonelli, 2005) Mθθ¨+Cθ,θ˙θ˙+Dθ,θ˙θ˙+gθ+JThe=Bu, (7) where M(θ) is the configuration-dependent mass matrix, C(θ,θ˙) is the Coriolis term, D(θ,θ˙) is the damping term modelled using a combination of linear and quadratic drags, and g(θ) is the vector of gravity and buoyancy forces. We compile all dynamic terms on the left into the vector τd , yielding τd+JThe=Bu. (8) 2.2 Wrench ellipsoids Analysis of wrench capability typically involves ellipsoids and polytopes. Velocity ellipsoids and their counterparts, wrench ellipsoids, were introduced by Yoshikawa (1985). These provide a mapping from a unit ball of joint torques to the end effector wrench, according to Equation 6. Care must be taken to avoid physical inconsistency due to the use of norms and inner products on wrench and axis force/torque spaces (Duffy, 1990). The use of appropriate scaling metrics (Doty et al., 1993; Doty et al., 1995) gives a modified version of Equation 6 expressed as Equation 9 Wτ1/2τh=Wτ1/2JTWh−1/2Wh1/2he, (9) where Wτ and Wh are the scaling metrics for τh and he , respectively (Doty et al., 1993). The result is that the vector spaces Wτ1/2τh and Wh1/2he have physically consistent inner products and norms. In this work, we arbitrarily consider identity scaling metrics, i.e., Wτ=In and Wh=I6 , where In and I6 are the n×n and 6×6 identity matrices, respectively, where the elements of each matrix have the appropriate units to provide physical consistency (see Doty et al. (1993) for an in-depth discussion). Dropping the scaling metrics for brevity, the wrench ellipsoid is defined as Equation 10 he∣‖JThe‖2≤1. (10) Recent work proposed maximising the volume of the velocity ellipsoid by projecting the gradient onto the null space of the system, as shown in Equation 4, and by further solving the problem as a quadratic program (Zhang et al., 2016). A similar null space projection method was used by Bae et al. (2018) to maximise the dynamic manipulability ellipsoid during a value-turning operation for a UVMS. 2.3 Transmission ratios 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 distance to the wrench ellipsoid from the origin in a desired direction—which can been optimised by null space projection (Faroni et al., 2016). To determine the distance to the wrench ellipsoid in a given direction, consider a unit vector c^ , which defines the desired wrench, according to the metric Wh−1/2c^ , as defined above. Again, in this work, we consider identity scaling metrics, so Wh and Wτ are dropped in further notation, yet they are required to avoid physical inconsistency and allow the use of norms and unit vectors. Now, considering a scalar β , which scales the wrench in the direction of c^ , with the condition that the result lies on the ellipse given by Faroni et al. (2016) in Equation 11, βc^TJJTβc^=1 (11) and solving for β in Equation 12, β=c^TJJTc^−1/2. (12) This analysis does not include the effect of gravity, buoyancy, and dynamical effects, which offsets the centre of the ellipsoid, yielding a dynamic wrench ellipsoid defined as dynamic wrench ellipsoid defined in Equation 13 he∣‖JThe+τd‖2≤1. (13) For an ellipse with its centre not at the origin, we can solve for β , 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. Solving Equation 14 JTβc^+τdTJTβc^+τd=1, (14) for β simply involves taking the positive root. This definition does not account for the actuator mapping B or actuator limits. Symmetric torque limits have been proposed to be incorporated into the transmission ratio (Xing et al., 2020), 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 (Nemec, 1997), which is functionally equivalent to optimising the transmission ratio. The idea of wrench ellipsoids was extended by Ajoudani et al. (2017) 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 obtain a configuration-dependent measure. Bowling and Khatib (2005) explored 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 the manipulator capability. 2.4 Wrench polytopes It has been shown that wrench ellipsoids provide only an approximate measure of manipulator performance as it fails to capture the true constraints of the system, as compared to wrench polytopes, which provides a better description of the true capabilities of a system (Chiacchio et al., 1996). By finding the set of effector wrenches which satisfy joint constraints, the wrench polytope is define in Equation 15 he∣τmin≤JThe+τd≤τmax, (15) where τmin and τmax are the minimum and maximum loads on each DOF, respectively. Wrench polytope analysis has been used extensively in the design and evaluation of manipulators and manipulator poses (Firmani et al., 2008; Boudreau et al., 2021), 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 (Busson and Béarée, 2018), or solving linear programming (LP) problems for parallel manipulators (Buttolo and Hannaford, 1995; Lin and Chen, 2016), although these methods are not directly applicable to mobile manipulator systems. 2.5 Redundancy parameterisation The idea of redundancy parameterisation has been proposed in several works to greatly reduce the number of dimensions over which a given performance objective needs to be optimised, as well as to remove the non-linear inverse kinematics constraint. These methods avoid the use of the Jacobian null space projection in Equation 4 and instead explicitly consider 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 (Busson et al., 2017), as well as for exploring the force capabilities of a manipulator to apply forces normal to a surface (Nützi et al., 2013). This work was extended in Busson and Béarée (2018) to an exhaustive search over two redundant DOFs for a 7DOF manipulator, where the saturating wrench was used to find the weightlifting capability for each pose, while Boudreau and Nokleby (2012) 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 θr∈Rn−6 , Equation 4 can be rewritten as θ˙=J†x˙+Zrθr˙, (16) where θr˙∈Rn−6 is the vector of velocities of the redundant DOF and Zr∈Rn×(n−6) is the null space projection matrix, which maps redundant velocities to system velocities, according to the parameterisation. A redundancy parameterisation method for UVMS was introduced by Marais et al. (2021), which is illustrated in Figure 2. 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 θr , together with the end effector pose x , can be used to determine the full system configuration θ . Sequence of images showing the proposed redundancy parameterisation method for UVMS, with redundancy parameters θr=[q1,q2]T . 2.6 Wrench capability over a trajectory The previous analysis has 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 (Bae et al., 2018), yet local methods may lead to numerical instability and sub-optimal trajectories (Kazerounian and Wang, 1988). Global redundancy resolution methods which consider entire paths have been proposed for trajectory planning in kinematically redundant systems (Suh and Hollerbach, 1987). Early works in global redundancy resolution (Papadopoulos and Gonthier, 1995a; Papadopoulos and Gonthier, 1995b) focused on planning paths, which avoid actuator saturation while experiencing a large fixed load. Further work used the intersection of the wrench polytope with an expected cone of disturbances for generating trajectories for kinematic manipulators which are robust to disturbances (Ferrolho et al., 2021; Lin and Chen, 2016) and 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. An iterative linear programming method was proposed for trajectory optimisation to maximise the allowable load for mobile manipulators between two end effector set points (Korayem et al., 2004), yet the computational complexity restricted its application to a 2DOF manipulator. Hamiltonian control approaches, which make use of Pontryagin’s minimum principle over an entire trajectory (Korayem et al., 2009; Korayem et al., 2012), have been used for finding the maximum load carrying capacity for mobile manipulators yet often result in control laws which require instantaneous switching of actuator forces or torques, which are not physically realisable for the systems considered in this work. 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 (Wahrburg and Listmann, 2016), or for trajectory planning for a mobile manipulator actuating a large load (Sleiman et al., 2021). Other works Li and Nguyen (2023) have considered kinodynamic pose optimisation for whole body control of a humanoid robot manipulating an object, with real-time trajectory tracking formulated as a constrained MPC problem, yet few of these works have considered explicitly maximising the wrench capability using these methods. To the best of the authors' knowledge, no effective methods exist for explicitly maximising the wrench capability of high DOF vehicle manipulator systems, which appropriately consider and exploit the full capabilities and constraints which are unique to these systems. 3 Maximising the 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 4. Cases in which the maximising static wrench capabilities are relevant include lifting a heavy load or maximising instantaneous torque when turning a valve. 3.1 Problem formulation The static wrench maximisation problem can be expressed as Equation 17 maxθ,u,hec^TWh1/2he, (17) with constraints xd=fkθ, (18) θmin≤θ≤θmax, (19) dminθ>0, (20) umin≤u≤umax, (21) τd+JThe=Bu, (22) c^c^†−I6Wh1/2he=0, (23) where as before, θ , u , he , τd, and J are the system configuration, actuator effort, end effector wrench, vector of dynamic terms, and system Jacobian, respectively, described in Section 2. Both τd and J have a dependence on θ . The desired end effector pose is given by xd∈SE(3) ; the terms umin and umax∈Rm are minimum and maximum actuator efforts, respectively; Wh is the wrench scaling metric as before, and c^∈R6 is the unit vector, as shown in Section 2, which describes the direction of force/torque, in which the wrench should be maximised. The first constraint in Equation 18 is the inverse kinematics constraint using the forward non-linear map fk , Equation 19 considers configuration limits, and the constraint in Equation 20 is to prevent self-collisions and collisions with the environment using the stand-in function dmin(θ) , which is simply the minimum distance from self-collision or between each link of the UVMS system and obstacles in the environment. Equations 21, 22 are the actuator limits and static balance constraints, respectively. Finally, Equation 23, where I6 is a 6×6 identity matrix, enforces Wh1/2he to have no force or torque components orthogonal to c^ . As in Section 2, care has to be taken when considering orthogonality of wrench vectors (Duffy, 1990), hence the use of the scaling metric. This formulation has a linear objective with combined linear and non-linear constraints and is intractable to solve in a reasonable timescale using available non-linear solvers for the system considered in this work. In particular, satisfying the inverse kinematics equality constraint in Equation 18 is difficult due to the absence of analytical solutions for kinematically redundant systems. This work proposes two methods to make solving the above problem tractable: redundancy parameterisation and separation into a bi-level optimisation problem. Bi-level optimisation has been used in several works on kinematically redundant manipulators to make optimisation problems tractable (Nusbaum et al., 2020; Menasri et al., 2013; Menasri et al., 2015). Typically, the problem is partitioned between two sets of decision variables and solved in a nested framework (Dempe, 2002). This work uses the proposed redundancy parameterisation method in Marais et al. (2021), allowing the inverse kinematics constraint in Equation 18 to be explicitly satisfied and, therefore, removed from the list of constraints and replacing θ with θr , the terms in redundancy parameterisation. Next, we recognise the following: the dynamic balance constraint in Equation 22 is linear in u and he for a given θ . Therefore, Equations 21–23 are all linear in u and he . Coupled with the objective which is similarly linear, the problem naturally separates into a linear lower-level problem with decision variables u and he , and a non-linear upper-level problem with the decision variable θr . Rewriting the newly formulated bi-level optimisation problem as an upper-level problem Equation 24 maxθrβ2*, (24) with constraints in Equations 25, 26 θrmin≤θr≤θrmax, (25) dminθr>0, (26) where β2* is the solution to the lower-level problem β2*=maxu,hec^TWh1/2he, (27) with constraints given in Equations 21–23. It must be noted that for certain sets of upper-level variables, the lower-level problem may not have a feasible solution. Physically, this corresponds to configurations where the system is unable to hold a static configuration due to external forces such as gravity and buoyancy acting on the system. Although not an issue for the system considered in this work, programmatically, these configurations are penalised with a large negative cost. This is further discussed for the case of dynamic trajectories in Section 4. 3.2 Analysis using ellipsoids and polytopes 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 obtain Equation 28 u=B†τd+JThe, (28) giving a new definition for the wrench ellipsoid Equation 29 he∣‖TuB†JThe+τd‖2≤1, (29) where the actuator weighting matrix (Xing et al., 2020) is given by Tu=diag1/min{|umin|,|umax|} . The transmission ratio is also redefined as the positive root of Equation 30 TuB†JTβc^+τdTTuB†JTβc^+τd=1, (30) for a given desired wrench along the direction c^ . Using the inverted actuator mapping to redefine the wrench polytope gives Equation 31 he∣umin≤B†JThe+τd≤umax, (31) 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 L2 polytope, while the L∞ polytope is given by Equation 32 he∣Bu=JThe+τd,umin≤u≤umax, (32) 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 3. This shows an underwater vehicle with diagonally mounted thrusters on each corner which can apply 1 N thrust. The arm has two joints, of which each can apply a torque of 1 Nm. The wrench ellipsoid L2 and L∞ polytopes along the 2D force dimensions with 0 end effector torque are shown. The L∞ polytope extends as far or further than the L2 polytope in all directions and, therefore, gives a better measure of the true wrench capabilities of the system. Wrench ellipsoid and polytopes measured in Newton (N) for a 2D UVMS with dimensions in metres (m), showing the force capability in the given configuration and the desired direction c , in which the wrench capability should be maximised. 3.3 Optimising for the maximum wrench capability Now, consider finding the configuration and combination of thruster forces and joint torques which maximises the wrench capability of the system shown in Figure 3 in the direction c^ while keeping the same end effector pose. For the system considered in Figure 3, there are three vehicle DOFs, two manipulator DOFs, four vehicle thrusters, two joint torques, and three wrench DOFs for a total of 14 variables to solve while simultaneously needing to satisfy the inverse kinematics constraint. Now, consider solving the optimisation problem using the bi-level approach proposed above, separating the problem into a linear programming problem with nine variables (four vehicle thrusters, two joint torques, and three wrench DOFs), and an upper-level problem with two variables (two manipulator DOFs). In this case, redundancy parameterisation has resulted in a bi-level optimisation problem with three fewer total variables to solve. Due to the low dimensionality of the upper-level problem, a simple grid search is used to search the configuration space for the globally optimal configuration. The lower linear programming problem is solved using the dual-simplex method in MATLAB’s linprog function. The optimal solution is shown in Figure 4 labelled β2 , with the corresponding L∞ polytope. The linear program in Equation 27 can be assumed as finding the value of the L∞ polytope in the direction of c^ . Therefore, the proposed bi-level programming approach can be considered an optimisation over the redundant DOF to optimally configure the system to maximise the L∞ polytope in the direction of c^ . Optimal configurations for a wrench in the c direction for a 2D UVMS, resulting in different optimal configurations according to the measure of wrench capability which is optimised. 3.4 Removing the orthogonal wrench constraint The orthogonal wrench equality constraint in Equation 23 can be removed in some scenarios. One example is during a valve-turning operation, where reaction forces orthogonal to c^ , the desired wrench maximisation direction, can be applied on the valve. This enables larger wrenches to be applied in the direction of c^ and results in the same bi-level optimisation problem as before, except with the constraint in Equation 23 removed. The solution to this optimisation problem is shown in Figure 4 labelled β3 , with the corresponding L∞ polytope. In this case, the bi-level programming approach can be considered an optimisation over the redundant DOF to optimally configure the system such that the L∞ polytope has a maximum extent in the direction of c^ . 3.5 Comparison with the transmission ratio Typical methods for maximising the wrench capability seek to maximise the transmission ratio (Faroni et al., 2016; Nemec, 1997). Considering that the proposed bi-level programming approach determines how to optimally configure the L∞ wrench polytope, consider instead finding the configuration which maximises the transmission ratio. The same bi-level programming approach can be used as before, with a modified lower-level problem which instead uses the transmission ratio. This is given by the following optimisation problem in Equation 33: maxθrβ1*, (33) with constraints in Equations 34, 35 θrmin≤θr≤θrmax, (34) dminθr>0, (35) where β1* is the solution to the lower-level problem in Equation 36 β1*=max β1∣‖TuB†JTβ1c^+τd‖2≤1. (36) In this case, β1* is the maximum wrench in the direction of c^ using the transmission ratio and is found by solving for the positive root of the above quadratic equation. Figure 4 shows the results of bi-level optimisation using each of these objectives β1 (maximising the transmission ratio), β2 (maximising the L∞ wrench polytope purely in the direction of c^ ), and β3 (maximising the L∞ wrench polytope with orthogonal wrench constraints removed) for the same system as in Figure 3 via a grid search over the two redundant DOFs, as well as the corresponding ellipsoid or polytopes. Surprisingly, the optimal configuration changes depending on the lower-level objective used in the bi-level optimisation formulation, despite only two degrees of redundancy. There is also a significant difference in the wrench capability. Maximising β1 gives a maximum force of 1.91N , while β2 gives 3.47N and β3 gives 3.88N . These differences become more pronounced with higher DOF systems, such as the UVMS used for experiments in Section 6. For higher dimensional redundant spaces such as the UVMS considered in this work, simulated annealing followed by a local interior-point method is used to search for an approximately globally optimal solution to the upper-level problem of bi-level optimisation. This is because the upper-level objective is non-smooth and highly non-linear with multiple local maxima, preventing the effective use of gradient-based methods yet with a relatively small number of decision variables (4 for the system in this work). This problem is solvable in several seconds on standard laptop hardware, making it amenable to real-time implementation (assuming the desired end effector pose and wrench maximization direction are defined). 3.6 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 he2∈R6 , which is the end effector wrench of the secondary arm, and C2∈R6×l , which is a set of l≤6 unit vectors, which define the direction in which the second manipulator can apply a wrench he2 . This is explained in more detail below. The force/torque balance equation is given by Equation 37 τd+JThe,he2T=Bu, (37) where the terms τd,J,B and u reflect the additional actuators and DOF of the second manipulator. In order for (he,he2)T to be well-defined, J∈R12×n must have a 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 (Yoshikawa, 1985) 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 solution 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 lower-level objective is taken. In case of no solution, the configuration is considered invalid. The resultant bi-level optimisation problem is written as Equation 38 maxθrβ2* (38) with constraints in Equations 39–42 θrmin≤θr≤θrmax, (39) dminθr>0, (40) μθr>μmin, (41) fk−1xd,θr≠∅, (42) where β2* is the solution to the lower-level problem in Equation 43 β2*=maxu,he,he2c^TWh1/2he, (43) subject to constraints in Equations 44, 45 umin≤u≤umax,τd+JThe,he2T=Bu, (44) c^c^†−IWh1/2he=0,C2C2†−IWh1/2he2=0. (45) Again, the decision variables for the upper-level problem are the redundancy parameters θr . The term μ(θr) is the configuration-dependent manipulability measure (Yoshikawa, 1985), which must be greater than some empirically chosen minimum μmin to ensure a kinematically deterministic wrench balance configuration. The final upper-level constraint requires the existence of a solution to the inverse kinematics problem fk−1(xd,θr) for desired end effector poses xd∈R12 . As before, there is an equality constraint which ensures he has no component orthogonal to c . The final equality constraint ensures Wh1/2he2 only has components in allowed directions defined in C2 . For example, when firmly grasping a contact point with the secondary manipulator, it might be feasible to set C2=I6 , meaning any secondary wrench is allowed. If the secondary manipulator is pushing against a flat surface, then C2 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, given by C2he2≥0. (46) The above bi-level optimisation problem again separates the wrench maximisation problem into a linear lower-level optimisation and a low-dimensional higher-level problem. Again, the lower-level problem is solved using the dual-simplex method in MATLAB’s linprog function, and the upper-level problem uses simulated annealing, followed by the interior-point method to effectively search the highly non-linear space for an approximate global optimum. 3.7 Regrasping 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 DOF, which can be optimised over. This leads to an additional DOF in the parameterised redundancy space θr in the above optimisation problem. Section 6 further describes the possible parametrisation of re-grasping points. Parametrised regrasp points for second manipulator adding an additional kinematic variable θn+1 . 4 Wrench maximisation over a trajectory In this section, a predefined end effector trajectory is considered, with corresponding desired wrench directions which should be maximised 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. Dynamical effects due to velocities and accelerations cannot be ignored in this case. It is assumed that 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 capability along this trajectory. 4.1 Problem formulation In order to make the problem tractable, the trajectory of the end effector is discretised into a set of k successive poses x1,…xk , written as the stacked vector x , with corresponding redundant configurations θr,1,…θr,k written as θr , giving corresponding system configurations θ1,…θk , written as θ . Each point along the trajectory of total time T is equally separated by Δt=T/k . At each end effector pose is a corresponding end effector velocity x˙1,…x˙k written as x˙ , which is computed using the timestep and differences between successive poses in SE(3) . Dynamic quantities are computed using the finite difference operator Di=1Δt−11−11⋱⋱−110⊗Ii×i, (47) where ⊗ represents the Kronecker product and Ii×i is the i×i identity matrix. For this work, Equation 16 is slightly modified to give Equation 48 θ˙=JEx˙+Zrθr˙, (48) with Equation 49 JE=Jx−10n−6×6, (49) where Jx−1 is the inverse of Jx∈R6×6 , which is the Jacobian for all DOFs which do not correspond to those is the parameterised redundancy space. As before, Zr∈Rn×(n−6) is the null space projection matrix, which maps redundant velocities to system velocities, according to parameterisation. This formulation allows velocities for tracking end effector trajectories to be completely separated from redundant space velocities and vice versa. Now, we can rewrite Equation 48 as Equations 50, 51 θ˙=JEx˙+Zrθr˙=JEx˙+ZrDn−mθr, (50) θ¨=Dnθ˙, (51) where JE and Zr are the stacked matrices formed from JE and Zr , respectively, at each configuration. Here, Dn−m and Dn are the finite difference operators acting on vectors of size (n−m) and n respectively, as defined in Equation 47. At each timestep, the wrench is given by he,1,…he,k , written as he , and actuator efforts u1,…uk , written as u . Each timestep also has a unit vector c^1,…c^k along which the wrench should be maximised, written as 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. Again, the problem can be greatly simplified by separation into a bi-level optimisation problem and through the use of redundancy parametrisation. The trajectory optimisation problem can be written as the following bi-level optimisation problem θropt=argminθr−fθr, (52) where θropt is the optimal set of redundant configurations along the trajectory and f(θr) is the max–min wrench capability. There are constraints on the configurations in Equation 53 θrmin≤θr≤θrmax, (53) where θrmin and θrmax are the minimum and maximum limits on the configurations along the trajectory, respectively, and constraints on the velocities in Equation 54 θr˙min≤θr˙≤θr˙max, (54) where θr˙min and θr˙max are the minimum and maximum limits on the velocities, respectively. The collision constraint in Equation 20 also applies at each timestep. Given a set of redundant configurations θr , the max–min wrench capability can be solved as a linear program (LP) over the entire trajectory, giving the lower level of the above bi-level optimisation in Equation 55 fθr=maxp,u,hep (55) subject to Equation 56 p