Optimal Grasping Pose Synthesis in a Constrained Environment

Abstract

In the last few decades, several approaches have been presented to accomplish tasks with robots or autonomous systems in a glovebox; nevertheless, in nuclear facilities, risky operations are still executed by humans that guarantee a high manipulation capability and dexterity. Inside the gloveboxes, robotic devices have to operate in cluttered environments, or environments with limited space for movement; therefore, it is of significant interest to identify grasping poses that are feasible within such constrained environments. In this paper, we present and experimentally evaluate a strategy to synthesise optimal grasps considering geometric primitives for a manipulation systems in a constrained environment. The novel strategy has been experimentally evaluated in a cluttered environment (as a glovebox mock-up) with realistic objects, and the efficacy of the proposed grasping algorithm is proposed.

Robotics is a key technology for the nuclear industry, which takes an important role for the safe and cost-effective maintenance of safety-critical sections in reactors, research labs and similar places. There is a constant search for new robotic solutions to improve the safety of operations and reduce operational costs [1,2].

In nuclear industry and research, the gloveboxes are extensively used to handle objects/tasks in a safe fashion. Despite the comprehensive protective equipment and safety procedures, which have been developed over the years, glovebox operations still pose a high risk for the operators. Moreover, due to the strenuous working conditions, glovebox operations for operators are slow and intensive in terms of the task load.

Improving safety and efficiency in glovebox operations have become a new research topic drawing more attention. These new glovebox solutions utilise cutting-edge robotics, AI and computer vision technology to assist the operator during glovebox operations, improve the safety of the operator and increase the overall productivity.

The glovebox environment, such as in Figure 1, is cluttered, dynamic and the tasks executed in gloveboxes are significantly complex and safety-critical; therefore, a stand-alone robotic system cannot be expected to out-perform a human operator with the current technology. Moreover, due to safety concerns, human-in-the-loop solutions are found to be more reliable.

Robotic solutions for gloveboxes mostly rely on teleoperation in order to keep the human in the decision making process. However, ideal robotics solutions will attain better productivity, reduced cost and increased safety by relying on autonomous systems. Despite the considerable amount of research, deploying an autonomous robotic system inside a glovebox is not feasible with current technology; however, certain parts of the task execution can benefit from autonomy.

A close inspection of task execution of operators can reveal that object manipulation or tool use is common in many glovebox operations. Hence, it can be concluded that grasping plays a fundamental role in a safe and successful operation. On the other hand, grasping alone is a very complex procedure and automating the grasping for a robotic system is one of the fundamental research questions that is yet to be solved.

Grasping is a very complex task for a robot to execute because of (i) considering the geometric constraints (such as obstacles in the environment, the glovebox boundaries) on the arm/gripper pose, (ii) identifying a suitable grasp pose on the manipulated object and (iii) applying a suitable contact force distribution for a safe hold. Improving autonomous grasping strategies could provide novel tools to support the operator in order to identify feasible grasping poses, and the same solutions could be used to develop a robotic glovebox with a high degree of autonomy.

This paper aims to present a strategy to compute the optimal trade-off between grasping posture and contact forces in a constrained environment by minimising a novel cost index that extends the work in [3] by including the desired contact point positions to plan grasping poses. A novel holistic approach, which takes into account the constraints of the workspace, is achieved by designing an optimisation algorithm with: (i) a suitable cost index that evaluates the grasping pose quality and contact force distribution, (ii) modelling the environmental limitations using linear and nonlinear functions, which relate to a collision between the manipulation system and the limited workspace.

Although our experimental setup uses a two-finger gripper configuration, the procedure presented can be generalised to hands with multiple fingers. The results in this paper show that the work is well suited to applications with limited workspace such as a robot working within a glovebox. The authors would highlight that this work provides an algorithm to synthesise grasping poses for objects with known geometrical and physical models; therefore, model identification/fitting aspects will be not considered in this paper.

The rest of the paper is arranged as follows. Section 1 introduces: (i) robotic solutions for handling and grasping tasks in a nuclear environment, (ii) grasping strategies with robotics manipulation systems in a constrained environment. Section 2 presents the limits of legacy gloveboxes and defines the problem. Section 3 describes in detail the implementation of the algorithm in order to identify optimal grasping poses. In Section 4, the experimental setup is described in detail. In Section 5, simulation results are provided to show the effectiveness of the proposed algorithm, then, optimal grasping poses are tested and validated on a real robot. Results are discussed in depth in Section 6. Conclusions and directions for future works are presented in Section 7.

1. Literature Review

1.1. Robots In Gloveboxes

In the last forty years, the robotics research community has investigated innovative robotic solutions to improve the safety and the efficiency of operational activities in nuclear environments. In [4], the authors highlight the importance of robotic solutions to accomplish inspections and decommissioning tasks in a hazardous environment and glovebox, in particular, this aspect was investigated more in-depth with preliminary experiments in [1], where a robotic manipulator was exploited to dismantle a JDPR reactor. Autonomous robot and tele-operation are also key factors to innovate a legacy glovebox that is going to be dismantled in multiple nuclear facilities in the world. Up to now, operators accomplish different tasks by inserting the hand (with proper equipment) in a hazardous environment where the consequence of an accident could be serious: the operator could be contaminated by accidental cuts of the rubber glove [5] or by an error in operation process [6]. Robotics and artificial intelligence can be profitably used to remove the operator from dangerous tasks while autonomous or semi-autonomous systems could accomplish the activities. To pursue this aim, it is necessary to improve the control strategies of manipulation systems in order to operate in complex environments with constraints and robot redundancy [7].

One preliminary study into the use of automated robotics within a glovebox is presented in [8], where an automation system and non-redundant robotic arms are proposed to mitigate human operator risks in handling activities. In order to reduce operational cost, robotic solutions are proposed to execute ad hoc tasks [9,10] and simulations are developed to aid in mitigating hazards that may be introduced as a result of the deployment of robotic manipulators [11]. The solutions proposed above are not multipurpose because they are designed to solve specific tasks. In this scenario, redundant collaborative robots can potentially improve the system manipulation capabilities [12] as redundancy can be exploited to adapt robot poses, for example, to avoid collision with objects in the constrained space, or to handle an object with higher quality grasping index [13], and therefore more robust handling. At the same time, novel strategies need to be designed to exploit redundancy within individual applications or tasks with the aim to reduce the control complexity.

The same strategies could support the operators in manipulation and grasping tasks that are accomplished with difficulty by tele-operation inside the glovebox, as shown in [14] or in [15].

While a training course could improve the ability in manipulation tasks [16] and reduce the fatigue, in some cases an autonomous system could provide aid to the operator [17] to control the robot at any level of autonomy.

More recent research fields explore how to reduce the operator workload with high-level instructions given to the robot by voice command [18] while the usability of a humanoid robot is explored in order to do bi-manual tasks inside a legacy glovebox [19,20]. In general, all the solutions, which are cited above, exploit methods and strategies presented in robotics literature in order to identify reliable grasping poses.

1.2. Grasping

Robotics play a fundamental role to identify feasible grasping solutions in a constrained workspace such as a glovebox, and within the robotics literature, two different strategies are commonly used in order to identify feasible grasping poses that satisfy the environmental constraints: (1) finding grasp poses without considering constraints and then filtering them to respect environment constraints [21,22,23,24], (2) modelling the constraints inside the algorithm to find grasping poses [25,26,27,28]. Taking into account the necessity to use a model of the object in order to identify grasping poses, the first group could be split into two additional subgroups: model-based and sensor-based.

Multiple approaches have been proposed to identify optimal grasping poses in environments without constraints. If the object model is available, swept volumes and continuous collision detection [29] or independent contact region algorithms [30] are proposed to identify a handling pose. Force closure [21] and form closure index [31] optimisation could be considered a valid offline method to collect high quality grasping poses. In [22] a real-time algorithm is proposed to collect stable grasping poses.

In [32,33] the authors design an optimisation algorithm in order to identify suitable grasping poses taking into account optimal contact force distribution constraints. The environment constraints and hand kinematics are not considered in this work. A different approach is presented in [34], where support functions and wrench oriented grasp quality measures are used; this solver is not tested in a real scenario where a cluttered environment restricts feasible grasping poses.

1.3. Grasping without Object Model

If a prior object model is not available, then a complementary approach can be employed, which aims to detect feasible grasping poses, exploiting sensor data in the scene.

Information from a synthetic data set, RGB-D sensor and grasp quality neural network is used to identify in real-time reliable grasping pose in [35,36]. Usually, good performance is only achieved after extensive neural network training with a very large dataset.

Uncertainty from the sensor or subsequent processing could affect these methods; however, one solution to this problem is proposed in [23], which exploits Gaussian Process Implicit Surfaces and Sequential Convex programming. Also, human grasping strategies can reduce the grasping control technique complexity by exploiting tactile sensing to modulate the contact forces in grasping and releasing actions [24] or by mapping the optimal robot grasping pose from the human handling strategy as captured in video recordings [37].

1.4. Grasping in Constrained Environments

Modelling the constraints directly in the search algorithm is a valid alternative to filtering grasping poses by constraints and has the beneficial property that infeasible but otherwise high-quality grasping poses are not being identified and discarded, and hence it is more likely that a good enough feasible solution will be found. Following the concept above, a holistic approach is proposed by the animation industry to identify reliable kinematic chain configurations in a constrained environment. The method is called cost index optimisation, is subject to linear and nonlinear constraints, and is presented and tested on humanoid characters in [38,39]. For robotics applications, stable holding poses in constrained environments are achieved by exploiting simulation and shape primitives in the open-source platform called Graspit! [25]. If a complete environmental knowledge is available, an accurate simulation tool can be developed using multi-body dynamics tools in order to avoid collision [26], for example with a soft humanoid hand. This approach can be time-consuming; however, decomposing the problem in multiple low dimensional spaces can improve performance [40]. In some applications, one important constraint that needs to be considered relates to avoiding collisions with objects in the scene. This goal could be achieved, as shown in [41], with a motion constraint graph that allows the identification of object surfaces that are reachable by the gripper. This method, however, does not provide information about the grasping quality. A more general solution is provided in [27], where a grasp scoring function is proposed in order to evaluate the handling pose quality, taking into account the distance of the gripper from the other objects in the scene. A different solution is proposed in [28] where visual feedback is used to evaluate constraints and their positions, and to update the proposed grasping pose. If the handling system could collide with other objects in the scene, a trajectory can be planned to reach and to grasp a desired object by pushing and moving the others as shown in [42].

In recent studies [43,44] environment constraints are exploited to perform grasping tasks; this approach is promising for application with a compliant hand in an environment where no risks are caused by interactions between the manipulation system and the environment; however, it may apply to a wider range of situations.

1.5. Our Approach: Novelties

Our work presents an optimisation routine to detect grasping poses in a constrained environment, and we present for the first time a holistic approach that includes the model of the complete manipulation system and extends the work presented in [3,33] where only the hand kinematics are taken into account. With our approach it is possible to identify only grasping poses that are feasible with the arm kinematics. Additionally, the algorithm gives the possibility to select limited regions on the contact surface in order to allow the grasping only on desired contact surfaces. These aspects are fundamental in industrial tasks where collisions, for safety reasons, are not allowed.

2. Problem Definition

2.1. Background: Legacy Glovebox

In the nuclear industry, for safe handling of hazardous objects/material, gloveboxes are used where an operator performs a task reaching through the ports that can be seen in Figure 1.

Despite the variety of objects and tools used in gloveboxes, most of the manipulated objects can be approximated with spherical and cylindrical geometries. In effect, in [12] the authors are focused on the problem to grasp and manipulate cans and pucks, in [45] spherical objects are manipulated to accomplish direct oxide reduction and automated bag-out operations, often it is requested to interact with plutonium pits and pipes.

For the high levels of radiation and wear, the manipulated objects are often corroded/contaminated [46]; therefore, a secure grasp can be achieved by using the non-corroded surfaces and by touching the containers on desired contact points (Figure 2); hence, limiting the number of grasp configurations. In this setting, the manual operations performed by the operators benefit from the dexterity of the human hand and achieve a secure grasp by adapting the hand posture and position in a functional way to the specific task.

2.2. Robotic Glovebox

Novel technologies in robotics and artificial intelligence can be exploited to increase the safety in the legacy glovebox or to design new robotic gloveboxes; in both cases, dexterous robotic manipulators, sensors and control algorithms can avoid the direct contact between the operator and hazardous material.

Inside the unstructured environment of gloveboxes, the robots could be controlled by the operator via teleoperation while more autonomous control strategies could be exploited in more standard tasks. Robot arms could be profitably used to accomplish operations that are performed by the operator in order to reduce the workload and the risks of accident or contamination. Usually, inside the glovebox the workspace is limited and it is mandatory to identify robotic arm configurations that avoid collision with glovebox structure while ensuring: (1) to reach the objects to grasp, (2) to touch the object on safe regions, (3) to apply correct contact force distribution.

2.3. Problem Definition

In a nuclear industrial environment, several grasping and manipulation operations could be executed in autonomy by robots. In this scenario, a challenging problem is identifying arm configurations that avoid collisions between the robot and the glovebox walls while ensuring feasible and reliable grasps of cylindrical (see Figure 1) and spherical objects. The force distribution problem has to be evaluated also with simpler manipulation systems as the gripper in order to ensure the correct compensation of the weight of the object while avoiding contact force magnitudes that are non-feasible (outside the friction cone) or dangerous for the integrity of the object.

This work proposes an algorithm to identify an optimal grasping pose and contact force distribution to ensure feasible grasp of known objects with modelled manipulation systems. Desired contact points on the grasped object are used as input to the algorithm in order to identify grasp poses that are functional to the task. In the case of repetitive tasks, it is possible that the desired contact points are not reachable because the surface is damaged, in this case, the algorithm has to identify the optimal grasping pose in feasible contact regions, as shown in Figure 3.

In order to test the algorithm outcomes in a scenario that is similar to industrial requirements, a glovebox mock-up was integrated with a manipulator arm (Kinova Gen3) provided with a gripper (Robotiq 2F-85).

3. Optimal Grasping Algorithm

3.1. Robotic Hand Model

In order to provide a more general approach in this paper, the theory about compliant manipulation systems is taken into account.

The grasping system is modelled following the definition in [21,47] with the notation in

Table 1. Notation for grasp analysis.

Notation	Definition
$n\in \mathbb{R}$	number of joints of the hand
$c_p\in \mathbb{R}$	number of contact points
$c\in \mathbb{R}$	dimension of the contact force/torque vector
$\tau \in {\mathbb{R}}^n$	joint torques
$q\in {\mathbb{R}}^n$	joint angles
$q_r\in {\mathbb{R}}^n$	joint reference angles
$f\in {\mathbb{R}}^c$	contact force/torque vector
$w_e\in {\mathbb{R}}^6$	object wrench
${\xi }_e\in {\mathbb{R}}^6$	object twist
${\xi }_f\in {\mathbb{R}}^c$	twists of the contact points on the fingers
${\xi }_o\in {\mathbb{R}}^c$	twists of the contact points on the object
$J\in {\mathbb{R}}^{c\times n}$	hand Jacobian matrix
$G\in {\mathbb{R}}^{6\times c}$	grasp matrix
$k_{stru}\in \mathbb{R}$	structural stiffness
$k_{ss}\in \mathbb{R}$	joint stiffness
$C_{stru}\in {\mathbb{R}}^{c\times c}$	structural compliance matrix
$C_{ss}\in {\mathbb{R}}^{n\times n}$	joint compliance matrix
$K\in {\mathbb{R}}^{c\times c}$	stiffness matrix
$G_K^R\in {\mathbb{R}}^{c\times 6}$	K-weighted pseudo-inverse of G
$s\in \mathbb{R}$	number of synergies
$\delta \sigma \in {\mathbb{R}}^s$	synergistic displacement
$S\in {\mathbb{R}}^{n\times s}$	synergy matrix
$\delta f_p\in {\mathbb{R}}^c$	particular solution
$\delta f_{hrs}\in {\mathbb{R}}^c$	active internal forces
$\delta f_{hos}\in {\mathbb{R}}^c$	passive (preload) internal forces
$F_s\in {\mathbb{R}}^{c\times s}$	map of $\delta \sigma$ into active internal forces $\delta f_{hrs}$
$e_s\in \mathbb{R}$	rank of $F_s$
$E_s\in {\mathbb{R}}^{c\times e_s}$	basis of the range space of $F_s$
$y\in {\mathbb{R}}^{e_s}$	parameterising vector of the active internal forces
${\mu }_s\in \mathbb{R}$	static friction coefficient
$f_{min}\in \mathbb{R}$	minimum value of forces component in f
$f_{max}\in \mathbb{R}$	maximum value of forces component in f
$com\in {\mathbb{R}}^3$	centre of mass of the object
$\overline{c_i}\in {\mathbb{R}}^3$	desired contact point
$c_i\in {\mathbb{R}}^3$	real contact point
$l\in \mathbb{R}$	number of arm joints
$q_a\in {\mathbb{R}}^l$	arm joint angles
$m\in \mathbb{R}$	number of contact parameters
$q_c\in {\mathbb{R}}^m$	contact parameter
$n_i\in {\mathbb{R}}^3$	normal vector to the contact surface ($i^{th}$ point)

. For the sake of clarity and completeness, we have included the balance and congruence equations for hand and object, these equations are fundamental to model interactions in the manipulation system. In this work, for simplicity, we model the contact between the object and the robot fingers with the hard finger model, and as a consequence, three contact force components are generated for each contact point. We define $c_p$ as the number of contact points, $c=3c_p$ as the dimension of the contact force/torque vector (due to the hard finger assumption), $\tau \in {\mathbb{R}}^n$ as the joint torque vector, $f\in {\mathbb{R}}^c$ as the contact force vector, ${\xi }_f\in {\mathbb{R}}^c$ as the twist of the contact points on the fingers and $q\in {\mathbb{R}}^n$ as the joint angles.

The congruence equation for the hand with n DoFs, which contacts an object in $c_p$ points:

$$\tau =J^{T}f,{\xi }_f=J\dot{q},$$

(1)

where $J\in {\mathbb{R}}^{c\times n}$ is the hand Jacobian matrix.

Similarly, the balance and congruence equations for the object are defined as:

$$w_e=Gf,{\xi }_o=G^T{\xi }_e$$

(2)

where $G\in {\mathbb{R}}^{6\times c}$ is the grasp matrix, $w_e\in {\mathbb{R}}^6$ is the object wrench, ${\xi }_o\in {\mathbb{R}}^c$ is the twist of the contact points on the object and ${\xi }_e\in {\mathbb{R}}^6$ is object twist.

The structural compliance $C_s=(1/k_{\mathrm{stru}})I_{c\times c}\in {\mathbb{R}}^{c\times c}$ is combined with the joint compliance $C_q=(1/k_{\mathrm{ss}})I_{n\times n}\in {\mathbb{R}}^{n\times n}$ in order to obtain the hand elasticity $K={(C_s+J{C}_q{J}^T)}^{-1}$. Contact forces are generated in the model through the effect of the stiffness matrix K and virtual displacements between contact points on the object and finger.

We consider synergies as coordinated movements of the finger due to mechanical coupling of the hand fingers. A relation between the synergy input displacements $\delta \sigma \in {\mathbb{R}}^s$ and the joint reference displacements $\delta q_r$ can be formalised as

$$\delta q_r=S\delta \sigma S\in {\mathbb{R}}^{n\times s}$$

(3)

where $1\le s\le n$. A new equilibrium configuration q, due to the effect of joint compliance $C_q$, can be reached by the hand due to the effect of the compliance:

$$\delta q=\delta q_r-C_q\delta \tau .$$

(4)

It is worth noting that Equations (1)–(3), which strictly derive from the manipulation system kinematics, allow to model the grasping system with multiple fingers and contact points; more details about these aspects are provided in [47,48].

3.2. Grasping Contact Force Optimisation

In grasping systems with compliant contact and joints, as described in [21], the particular solution $\delta f_p$, the active internal forces $\delta f_{hrs}$ and the passive (or pre-load) internal forces $\delta f_{hos}$ contribute on the contact force distribution:

$$f=\delta f_p+\delta f_{hrs}+\delta f_{hos}$$

(5)

The particular solution has direct relation to the external wrench $w_e\in {\mathbb{R}}^6$:

$$\delta f_p=G_K^R{w}_e,\mathrm{with}{G}_K^R=K{G}^T{\left(GK{G}^T\right)}^{-1}\in {\mathbb{R}}^{c\times 6}$$

(6)

The active internal forces, which can be combined to squeeze the objects, can be computed as:

$$\delta f_{hrs}=E_{s}y$$

(7)

where $E_s$ is the active internal force base matrix and $y\in {\mathbb{R}}^{e_s}$ is a vector that combines the active internal forces. In this paper the passive internal forces are considered null, with no loss of generality. As described in [3,47] the y element can be achieved by optimising a suitable cost index. In particular given a grasping configuration, it is possible to identify an optimal force distribution by minimising a convex function that can be composed from $G_K^R$, $E_s$, an object wrench $w\in {\mathbb{R}}^6$, minimum/maximum contact force value $f_{min}$ and $f_{max}$.

With more details, the grasping force optimisation problem is set up as:

$$y^{*}=\underset{y}{\arg \min }V(w,y)$$

(8)

where $V(w,y)$ is a convex function defined in [3]. The function $V(w,y)$ is defined as the sum of terms that model: (i) the friction cone constraints, (ii) maximum and minimum value of normal force at contact, (iii) constraints to ensure the contact force distribution equilibrium. For a given grasping pose, the minimum value of $V(w,y)$ is evaluated with the optimal contact force distribution; for sake of clarity, more details about the function $V(w,y)$ are provided in Appendix A.

In this work, the optimisation of the forces is a nested optimisation task of an additional optimisation routine to find feasible contact points between the object and the manipulation system.

3.3. Algorithm Implementation

As defined in the literature, the grasp planning solver identifies contact points on a desired object that ensure feasible grasping. In particular, two conditions are required for this process:

(1) “A Feasible Pose” has to be identified before applying grasping forces in order to ensure that the manipulation system satisfies the constraints of the environment, for example, the feasible pose must include no contact with the wall or with other objects in the environment.

(2) “Force Closure” has to be guaranteed when the manipulation system applies grasping forces in order to maintain stable holding, without damage to the object.

Depending on the manipulation system properties, object properties and environmental constraints, the problem described above may have no solution, one solution or multiple solutions. Two independent indices can be provided in order to evaluate the force closure quality and feasible pose quality. The force closure quality can be evaluated by the cost function $V(w,y)$ as described in [21], for the feasible pose we can define a cost index that evaluates the distance between desired (a priori) contact points, $\overline{c_i}$, on the object and the real contact points between the object and the robot fingertip, $c_i$. Our formulation is as follows:

$$\mathsf{\Phi }(c_1,...,c_{c_p})=\sum _{i=1}^{c_p}\left(\right||\overline{c_i}-c_i{\left|\right|)}^2$$

(9)

The value of $\mathsf{\Phi }$ will be zero if contact point positions are equal to the desired contact point positions. The contact point positions $c_i$ are mapped by the forward kinematics of each finger of the hand, as shown in [48].

To evaluate the quality of the pose and the grasp, we define a cost index as:

$$U=\mathsf{\Phi }(c_1,...,c_{c_p})+kV(w,y)$$

(10)

where k is a weight that modulates the effect of the force cost index on the total cost index. The index V is a dimensionless and positive definite cost index. The index $V(w,y)$ has a global minimum value because the function is convex; on the contrary, the $\mathsf{\Phi }$ function is not regular and multiple local minima could exist. For the reason above, it is important to note that the total cost index U is not convex and multiple local minima can exist. The cost index U is defined in order to model grasp with multiple fingers because the $\mathsf{\Phi }$ component and $V(w,y)$ component are defined taking into account the forces at each contact point and the kinematics of the hand. The $\mathsf{\Phi }$ component allows to penalise the hand configuration with contact points that are far from the desired contact points on the objects. This aspect is used to identify grasping poses that are functional to the task as the operator grasps the same object in different positions for different operations.

3.4. Grasp Planning Optimisation

For a grasping manipulation system, assume the following information is given:

• A robot manipulator model (kinematic, joint stiffness, contact stiffness and friction) formulated as in [47];
• Information relating to the object to be manipulated: shape, stiffness, centre of mass of the object ($com$) and grasping regions;
• Desired contact points ($\overline{c_1},...,\overline{c_{c_p}}$) on object surface.

We define the arm joint vector as $q_a={\left[\begin{array}{c}{q}_1^a,\dots ,q_l^a\end{array}\right]}^T$ where l is the number of joints in the arm. The contact point positions on the link are modelled with $q_c={\left[\begin{array}{c}{q}_1^c,\dots ,q_m^c\end{array}\right]}^T$ where m is the number of parameters used to map the contact points on the phalanx surface. It is worth noting that $m\le c$.

The variables $q_a$, $\sigma$, $q_c$, y are collected in vector Q with $v=l+s+m+e_s$ dimensions as follows:

$$Q={\left[\begin{array}{cccc}{q}_a,& \sigma ,& q_c,& y\end{array}\right]}^T$$

(11)

The cost index U, proposed as a metric to evaluate grasping pose quality, is a function of $(c_1,\dots ,c_p)$ variables mapped from the Q variable vector by the forward kinematics relation; therefore, we can write $U\left(Q\right)$. In order to ensure stable and feasible grasping poses, the Q vector can be found by minimising the $U\left(Q\right)$ cost index:

$$Q^{*}=\underset{Q}{\arg \min }U\left(Q\right)$$

(12)

The optimisation is constrained by a set of linear/non-linear equalities and inequalities:

• Range limits of vector Q

  $$\left\{\begin{array}{cc}{q}_{min}^a\hfill & <q_i^a<q_{max}^{a}i=1,\dots ,l;\hfill \\ {\sigma }_{min}\hfill & <{\sigma }_i<{\sigma }_{max}i=1,\dots ,s;\hfill \\ q_{min}^c\hfill & <q_i^c<q_{max}^{c}i=1,\dots ,m;\hfill \end{array}\right.$$

  (13)
• Finger and object tangent conditions

  $$F(x_{c_i},y_{c_i},z_{c_i})=0i=1,\dots ,c_p;$$

  (14)
• Link and object non interpenetration condition

  $$F(x_{l_i},y_{l_i},z_{l_i})\ge 0;\forall (x_{l_i},y_{l_i},z_{l_i})\in P\left(l_i\right)$$

  (15)
• Link (hand/arm) and environment non collision condition

  $$F(x_{l_i},y_{l_i},z_{l_i})\ge 0;i=1,\dots ,l+n;$$

  (16)

It is worth noting that $V(w,y)$ includes the following elements:

• Coulomb’s inequality

  $${\alpha }_i\left|\right|f_i\left|\right|-f_i^T{n}_i\le 0i=1,\dots ,c_p;$$

  (17)

  where ${\alpha }_i=\frac{1}{1+{\mu }_s^2}$ and ${\mu }_s$ is the static friction coefficient.
• Minimum and maximum values of normal forces.

  $$\left\{\begin{array}{c}{f}_{i,min}-f_i^T{n}_i\le 0i=1,\dots ,c_p;\hfill \\ \left|\right|f_i\left|\right|\le f_{i,max};\hfill \end{array}\right.$$

  (18)

In this work, in order to optimise the cost function and evaluate multiple local minima, a multi-start optimisation solver is used.

3.5. Starting Point Selection

Multiple criteria can be used to select starting point conditions in the optimisation research. In this work we propose to collect multiple configurations of the arm in the constrained work space by sampling joint space range values, only feasible configurations need to be stored in a database. Multiple starting conditions can be achieved with an SQL query that extracts only arm poses that have the wrist position in a region close to the object to grasp. Then, multiple starting conditions are used to run constrained optimisations in parallel, each optimisation could converge on local/global minimum values. Optimisation outcomes are sorted taking into account the cost index U in order to identify the best local minima that is a global minima candidate. If all the optimisations fail, no grasping pose is identified, this case could happen when the problem is highly constrained or when the object is located in a work space region that is impossible to reach by the manipulator.

The pseudocode for the optimal grasping algorithm is presented in Algorithm 1. In the next section, the strategy described above is tested to generate the grasping configurations of a manipulation system inside a constrained environment as glovebox mock-up.

Algorithm 1: Optimal grasping algorithm

Data: - Sphere/cylinder model and centre of mass position ($com$) - Desired contact points on the object - Desired grasping region on the object - Robot kinematics and model - Environment geometry Result: Optimal arm configuration, synergy, contact point and grasping force $Q^{*}$ $initialpoints$← (SQL query) SELECT robot arm configurations WHERE wrist  position is in a bounded region around the object $com$;  foreach $Q_i\in initialpoints$ do             $\underset{Q}{\min }$ U(Q)           s.t.  environment geometry,               robot kinematics               force distribution constraints; $\phantom{(}$ ; $Q^{*}=Q_i^{*}$ which leads to the minimum $U\left(Q_i^{*}\right)$ from the previous step.

4. Experimental Setup

4.1. Manipulation System

This section presents the setup that was used to test the algorithm presented above. The manipulation system is composed of a Kinova Gen3 Arm (7 DoFs) and a finger gripper (2F85 Robotiq) mounted on the wrist of the arm; the manipulator was installed in a glovebox mock-up, providing a confined space representative of realistic application environments, as shown in Figure 1.

Kinematic models of the arm and the left finger of the gripper were achieved by Denavit-Hartenberg (D.H.) convention, with D.H. parameters defined in

Table 2. D.H. parameters for Kinova Gen3 arm.

${\mathit{l}}_{\mathit{i}}$	b	$-\mathit{\beta }$	a	$\mathit{\alpha }$	d	$\mathit{\theta }$
	(mm)	(rad)	(mm)	(rad)	(mm)	(rad)
0	0	0	0	$\pi$	156.4	0
1	−5.4	0	0	$\pi /2$	−128.4	$q_1^a$
2	−210.4	0	0	$-\pi /2$	−6.4	$q_2^a$
3	−6.4	0	0	$\pi /2$	−210.4	$q_3^a$
4	−208.4	0	0	$-\pi /2$	−6.4	$q_4^a$
5	0	0	0	$\pi /2$	−105.9	$q_5^a$
6	−105.9	0	0	$-\pi /2$	0	$q_6^a$
7	0	0	0	$\pi$	−61.5	$q_7^a$

and

Table 3. D.H. parameters for Robotiq 2F85 gripper.

${\mathit{l}}_{\mathit{i}}$	a (mm)	$\mathit{\alpha }$ (rad)	d (mm)	$\mathit{\theta }$ (rad)
1	57.3	0	0	$q_1^h$
2	43.7	0	0	$q_2^h$

.

The kinematic model of the right finger uses the D.H. table (Table 3) pre-multiplied by a fixed transformation matrix with translation amount (25.2 mm) on the x-axis; for both fingers, the synergy matrix is defined as:

$$S={[0.5,-0.5,-0.5,0.5]}^T.$$

(19)

Taking into account the manipulation system kinematic, the optimisation is constrained by a set of linear/nonlinear equalities and inequalities:

• Joint range limits:
  $q_{min}^a=$$[-6.283,-2.094,-6.283,-2.443,-6.283,-1.919,-6.283]$${}^T$ rad, $q_{max}^a=$$[6.283,2.094,6.283,2.443,6.283,1.919,6.283]$${}^T$ rad, ${\sigma }_{min}=$ 1.570 rad, ${\sigma }_{max}=$ 2.119 rad, $q_{min}^c=$$[-43.7,-43.7]$${}^T$ mm, $q_{max}^c=$ [0, 0]${}^T$ mm,
• Upper and lower values to model the contact point position on the gripper phalanges:

  $$-43.7\le c_i\le 0i=1,\dots ,c_p;$$

  (20)

The gripper fingers are covered by a layer of rubber in order to increase the grasping capabilities. We assume a static friction value ${\mu }_s=0.9$ that is comparable with the rubber and aluminium static friction at contact. The angle of friction is equal to 41.98${}^{\circ }$.

In the force optimisation routine we assume minimum value of normal forces $f_{1,min}=f_{2,min}=0.001$ N, maximum value of normal forces is $f_{1,max}=f_{2,max}=100$ N and $K_{ss}={10}^5$ Nmm/rad as joint stiffness.

4.2. Objects

The algorithm is tested with spherical and cylindrical objects, and for both of them we assume:

• Sphere/cylinder radius r,
• Desired contact points as $\overline{c_1}=[co{m}_x,co{m}_y+r,co{m}_z]$ and $\overline{c_2}=[co{m}_x,co{m}_y-r,co{m}_z]$,
• The weight is the only component of the external wrench that is modelled as: $w_e={\left[\begin{array}{ccc}2,& 0,& 0\end{array}\right]}^T$ N. We used $K_{stru}=10$ N/mm as stiffness of the material.

4.2.1. Sphere

For a spherical object the following constraints have to be added in the optimisation routine.

• Link and object tangent conditions

  $$|c_i-com{\left|\right|}^2=r^{2}i=1,2$$

  (21)
• Link and object non interpenetration conditions
  we define $F(x_{l_i},y_{l_i},z_{l_i})$ as the distance between $com$ and gripper phalanx contact plane.

4.2.2. Cylinder

The parameter h models the cylinder height, in this case a different set of equalities/inequalities has to be evaluated in the optimisation:

• Link and object tangent conditions

  $$F(x_{c_1},y_{c_1})=0;F(x_{c_2},y_{c_2})=0;$$

  (22)
• Link and object non interpenetration conditions

  $$F(x_{l_i},y_{l_i})\ge 0;\forall (x_{l_i},y_{l_i})\in P\left(l_i\right)$$

  (23)

  with

  $$F(x_{l_i},y_{l_i})={(x_{l_i}-x_0)}^2+{(y_{l_i}-y_0)}^2-R^2$$

  (24)

  where $(x_0,y_0)$ is the centre position on the circular face.

4.3. Software Architecture

The algorithm was developed in C++ using NLOpt library with solver NLOPT_LN_COB-YLA, which is a local and derivative free algorithm. The arm configurations in the workspace ($21\times {10}^6$ poses) were collected in a database stored in Microsoft SQL Express 2019. All the simulations were run on a desktop PC equipped with Intel i7700 CPU 3.60 GHz and 16 Gb RAM.

5. Validation of The Algorithm

5.1. Simulations Results

In order to test the algorithm, multiple simulations were evaluated with the object in different positions and $k=1$. A high value of k is necessary to decrease the effect of contact positions on the total cost index. Each simulation terminated after 10–12 h.

5.2. Sphere Grasp

Starting from 1304 arm configurations, which are generated by the SQL query, a total of 722 reliable poses are identified by the optimisation algorithm. For each pose the cost index values are shown in Figure 4. The lowest cost index values $U=2.178$, sum of $\mathsf{\Phi }=3.5\times {10}^{-5}$ and $V=2.178$, are generated from optimisation variable values ($q_a$, $\sigma$, $q_c$, y) in

Table 4. Components of the Q vector at the minimum value of the cost index U for each simulation case.

	${\mathit{q}}_{\mathit{a}}$ (Rad)	$\mathit{\sigma }$ (Rad)	${\mathit{q}}_{\mathit{c}}$ (Rad)	y (mm)
Sim.1	[1.224, −0.638, 0.231, 1.344, 1.899, −1.698, 0.737]${}^T$	2.017	[−5.760, −5.730]${}^T$	5.055
Sim.2	[1.671, −0.785, −0.471, 1.937, 6.155, −1.168, 1.857]${}^T$	2.116	[−3.236, −1.938]${}^T$	−5.111
Sim.3	[0.989, −0.362, 2.711, −0.875, 4.343, −1.407, −3.642]${}^T$	2.116	[−2.862, −6.268]${}^T$	5.087
Sim.4	[−0.149, −0.307, 2.512, −0.254, 0.750, −1.668, 1.428]${}^T$	$2.116$	[−5.496, −2.417]${}^T$	$4.987$

(Sim. 1). The manipulation pose satisfies the environment constraints as shown in Figure 5 and the contact force distribution, generated from q, $q_r$, $f_1$, $f_2$ in

Table 5. The joint values (q), reference joint values ($q_r$) and contact force values ($f_1$, $f_2$) in the new equilibrium configuration for each simulation case.

	q (Rad)	${\mathit{q}}_{\mathit{r}}$ (Rad)	${\mathit{f}}_1$ (N)	${\mathit{f}}_2$ (N)
Sim.1	[2.017, −0.446, −0.638, 0.446]${}^T$	[2.004, −0.433, 1.137, 0.433]${}^T$	[−1.000, −3.631, 0.000]${}^T$	[−0.999, 3.631, −0.000]${}^T$
Sim.2	[2.116, −0.546, −0.785, 0.546]${}^T$	[2.104, −0.534, 1.036, 0.534]${}^T$	[−0.985, −3.630, 0.092]${}^T$	[−1.014, 3.630, −0.092]${}^T$
Sim.3	[2.116, −0.545, −0.362, 0.545]${}^T$	[2.104, −0.533, 1.036, 0.533]${}^T$	[−1.062, −0.872, 3.527]${}^T$	[−0.937, 0.872, −3.527]${}^T$
Sim.4	[2.116, −0.545, −0.307, 0.546]${}^T$	[2.105, −0.534, 1.036, 0.534]${}^T$	[−0.840, −3.630, −0.029]${}^T$	[−1.159, 3.630, 0.029]${}^T$

(Sim. 1), is shown in Figure 6.

5.3. Cylinder Grasp

In this section we present the results of cylinder grasping in three different conditions.

CASE 1: The algorithm identifies 479 reliable grasp poses (1304 starting poses), the cost index values are shown in Figure 7. The lowest cost index values $U=5.094$, sum of $\mathsf{\Phi }=2.912$ and $V=2.181$, are generated from optimisation variable values ($q_a$, $\sigma$, $q_c$, y) in Table 4 (Sim. 2). The manipulation pose satisfies the environment constraints as shown in Figure 8 and the contact force distribution, generated from q, $q_r$, $f_1$, $f_2$ in Table 5 (Sim. 2), is shown in Figure 9.

CASE 2: We test the algorithm to grasp the cylinder in a position close to one of the bounding box walls. This configuration increases the number of constraints and reduces the number of feasible poses.

A total of 101 reliable poses are found from 1304 starting configurations, the cost index values are shown in Figure 10. The manipulation pose, in Figure 11, satisfies the environment constraints with the lowest cost index values $U=3.738\times {10}^3$, sum of $\mathsf{\Phi }=3.736\times {10}^3$ and $V=2.195$. The optimisation variable values (in Table 4—Sim. 3) and the contact force distribution (in Table 5—Sim. 3) contribute to ensure feasible grasp.

CASE 3: Limited contact region on the cylinder is selected and modelled with constraints in the optimisation algorithm. Starting from 1304 arm configurations (from SQL query), 507 reliable grasping poses are identified, the cost index values are shown in Figure 12. The limited contact region on the object and the manipulation pose, in Figure 13, satisfies the environment constraints with the lowest cost index values $U=2.410\times {10}^3$, sum of $\mathsf{\Phi }=2.408\times {10}^3$ and $V=2.198$. The optimisation variable values (in Table 4—Sim. 3) and the contact force distribution (in Table 5—Sim. 3) contribute to ensure feasible grasp. It is worth noting that the contact points are close to the contact region limit in order to be close to the desired contact points.

5.4. Grasping Pose Experiments

The optimal and sub-optimal configurations collected by the algorithm are finally tested on the real robot. The object to grasp is an aluminium can that is used to define the physical properties in the simulation in Section 5.1. Four grasping poses with different U cost values were selected: (A) $U=1.317\times {10}^4$, $\mathsf{\Phi }=1.316\times {10}^4$, $V=2.224$, (B) $U=7.307\times {10}^3$, $\mathsf{\Phi }=7.305\times {10}^3$, $V=2.179$, (C) $U=5.253\times {10}^3$, $\mathsf{\Phi }=5.251\times {10}^3$, $V=2.179$, D) $U=5.094$, $\mathsf{\Phi }=2.912$, $V=2.181$.

All the grasps are successfully accomplished with the manipulation system setup without collision in the cluttered environment. For all the configurations tested, gripper postures are presented in Figure 14, it is worth to note that pose D is the optimal grasping configuration described in the section. A video is provided by the authors (supplementary material Video S1) in order to show the grasping actions for all the configurations above. In the experiments, the robot uses the internal joint position controller in order to reach the desired joint configurations, as motion control of the robot is not in the scope of this paper.

6. Discussion

For all the test cases, the simulations and the experimental results corroborate the usability of the described strategy that identifies multiple grasping poses in a constrained environment such as a glovebox mock-up.

In Section 5.3/CASE 1, the physical constraints do not prevent the grasping of the object on the desired contact points. In both the cases the algorithm optimises the grasping poses in order to ensure a small distance between the real contact points and the desired contact points.

In Section 5.3/CASE 2, two glovebox walls constrain the feasible grasping poses of the arm, and in particular, it is impossible to reach the desired contact points. Also in these conditions, the algorithm identifies several poses to grasp the object in order to minimise the distance between the real contact points and the desired contact points.

CASE 3 shows the usability of the strategy when limited contact surfaces are allowed on the object; for example, if the object is damaged or corrupted. In this case, the algorithm accepts only the solutions that have contact points on a limited region on the object surface, while the distance between the desired contact points and the real contact points is as minimised as possible.

In all of the tests, the starting points are collected with a query from the same database with a collection of configurations of the arm. This is an important aspect to highlight because the workspace could be generated only one time and associated with the glovebox environment in order to save time.

Simulation results highlight that the principal contributor of the total cost index is the distance between real contact points and desired contact points if the weight value $k=1$. This aspect pushes the optimisation process to find grasping solutions that are close as possible to the desired contact positions, but a higher value of k could be used to find high quality grasps far from desired contact point positions.

The algorithm is designed to be executed offline because the process is time-consuming, but increasing the number of cores can decrease the computation time in order to have faster feedback for the operation.

Compared with other solution [25], our strategy allows identifying feasible configurations of the robot in order to have contact points that are close to the desired contact points. Additionally, custom equality/inequality could be exploited to plan restricted contact regions on the object/robot surfaces. The correctness of the model can ensure the grasping of the object, which is not guaranteed by techniques that are not based on the model [35,36]. Compared with solutions that use optimisation strategies [32,33] this work extends and tests the optimisation algorithm in order to be used inside a constrained environment. Errors in the model could affect the capability of the algorithm, and this problem could be mitigated by using margins in the algebraic constraints. The optimal grasping poses could be exploited to train a machine-learning algorithm to identify a map between the object properties and grasping pose variables. It is worth noting that the trajectory of the manipulator is not evaluated in the optimisation process, and multiple objects are not included in the test scenario. Even if the proposed solution is designed and developed in order to exploit parallel computing, a cluster with multiple CPU could profitably reduce the high computational time, but at this stage the algorithm can not be used in online application. The solution proposed in [33] provides grasping poses in dozens of seconds, and it is worth noting that the kinematics of the arm is neglected. In [22], the authors proposed a real time algorithm to identify optimal grasp without taking into account the hand kinematic. A sensory-based method provides a fast solution, such as in [35], where the authors used the GQ method to identify grasping poses in less than one second; in this case, the gripper approached the object from a fixed direction and the arm configuration is not evaluated. This is a limitation in glovebox set up where object–arm collision should be avoided. A similar consideration could be done for [36]. The evaluation of the probability of the force closure increases the time to identify the grasping poses in [23], where the computation time spans from 72 to 284 s. Interesting results are reached by Graspit! [25] in which a constrained environment provides grasping poses after 10–15 s. A real-time computation of the grasp in a constrained arm is provided in [40], which also takes into account the kinematics of the arm but the quality of the grasp is qualitative.

In this paper, the algorithm does not take into account multiple objects in the scene, though linear and nonlinear constraints could be used to avoid the contact between the manipulation system and multiple objects. The database that is used to collect the starting positions could also be used in the case where other objects are in the scene, because the optimisation finds the reliable configurations.

Future works could explore how to avoid contact with other objects by adding other constraints in the optimisation, in this case it is reasonable to expect a reduced number of grasping poses that avoid collisions between the manipulation system and the objects in the scene. It is worth noting that adding a high number of constraints could reduce the capability of the numerical solver to converge to a solution.

In the last few years, novel soft manipulators have been presented by the research community. Even if these solutions are promising in multiple fields and in nuclear facilities, we are proposing a novel solution to a practical problem. Our strategy could be exploited in the majority of the robotic manipulation systems actively used in the nuclear industry, where rigid robots with rigid grippers are used in legacy setups or where soft robots can not be used.

This algorithm identifies feasible grasping poses with minimal human intervention, the operator has to provide the requested inputs and he has to start the algorithm. When all the grasping poses are identified the operator selects the desired one. In this paper we are not solving the problem of planning a feasible path to reach the desired configuration.

7. Conclusions

In this work, we present a strategy to minimise a suitable cost index to synthesise optimal grasping poses within constrained environments (such as a glovebox) for a robotic manipulation system. Previous studies and different approaches are argued in order to provide: (i) an overview about the importance of robotics in nuclear activities and robotic gloveboxes, (ii) a multidisciplinary perspective about the state-of-the-art on grasping synthesis in cluttered/not-cluttered environment. The limits of the legacy glovebox are presented in order to highlight the benefits and the challenges of autonomous grasp inside a robotic glovebox. In order to provide the necessary knowledge to describe the proposed grasping strategy, the robotic hand model and the contact force distribution are described briefly, and we define a novel cost index that can evaluate the grasp quality as the sum of two different components: grasping pose and contact force distribution. A strategy, with nested optimisation (of which one is constrained) routines, is presented in order to provide optimal and suboptimal grasping poses by minimising the proposed cost index; the environment constrains are modelled in the algorithm as linear/nonlinear equalities/inequalities. Simulation results identify the optimal/suboptimal grasping poses for two object primitives as a sphere and cylinder in a cluttered environment as a glovebox mock-up; afterwards, several grasping configurations, that were previously collected in simulations, are tested on a real manipulation system with success.

All the results are discussed providing some comparisons with other grasp planning algorithms to highlight the benefits and disadvantages of the algorithm proposed. In our future studies, we will compare the proposed grasping algorithm to the existing grasp synthesis methods and provide quantitative results. In order to improve the capability of the presented algorithm, the next research activities will explore possible solutions to evaluate criteria about the optimal trajectory of the manipulator and to include multiple objects inside the glovebox. Additionally, objects with more complex shapes will be investigated in order to generalise the presented work. Future research activities will explore novel technologies and algorithms to estimate/identify the model of the objects inside the robot workspace.