A Learn by Demonstration Approach for Closed-Loop, Robust, Anthropomorphic Grasp Planning

Abstract

This chapter presents a learn by demonstration approach, for closed-loop, robust, anthropomorphic grasp planning. In this respect, human demonstrations are used to perform skill transfer between the human and the robot artifacts, mapping human to robot motion with functional anthropomorphism [1]. In this work we extend the synergistic description adopted in Chaps. 2–6 for human grasping, in Chap. 8 for robotic hand design and, finally, in Chap. 15 for hand pose reconstruction systems, to define a low-dimensional manifold where the extracted anthropomorphic robot arm hand system kinematics are projected and appropriate Navigation Function (NF) models are trained. The training of the NF models is performed in a task-specific manner, for various: (1) subspaces, (2) objects and (3) tasks to be executed with the corresponding object. A vision system based on RGB-D cameras (Kinect, Microsoft) provides online feedback, performing object detection, object pose estimation and triggering the appropriate NF models. The NF models formulate a closed-loop velocity control scheme, that ensures humanlikeness of robot motion and guarantees convergence to the desired goals. The aforementioned scheme is also supplemented with a grasping control methodology, that derives task-specific, force closure grasps, utilizing tactile sensing. This methodology takes into consideration the mechanical and geometric limitations imposed by the robot hand design and enables stable grasps of a plethora of everyday life objects, under a wide range of uncertainties. The efficiency of the proposed methods is verified through extensive experimental paradigms, with the Mitsubishi PA10 – DLR/HIT II 22 DoF robot arm hand system.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

1 Introduction

As stated by its title, the main challenge of this book is to bridge the gap between neuroscience and robotics with the twofold goal of (i) advancing the design of artificial systems and (ii) increase comprehension of biological systems. Considering point (i), one of the most challenging topics is the analysis and exploitation of anthropomorphism of robot motion, to improve the effectiveness of Human-Robot Interaction (HRI) applications. In Chap. 8, the synergistic description of the kinematics of human hand grasp was exploited to devise design guidelines for the anthropomorphic robotic Pisa/IIT SoftHand. In this chapter, such an analysis is extended to the arm-hand system and considerations on the concept of anthropomorphism are reported.

Over the last decades, we experienced an increasing demand for Human Robot Interaction (HRI) applications that require anthropomorphism of robot motion, as also discussed in Chap. 8. Anthropomorphism is derived from the greek word anthropos that means human and the greek word morphe that means form. More than 140 years ago Charles Darwin suggested anthropomorphism as a necessary tool for understanding efficiently nonhuman agents [2]. The essence of anthropomorphism as described in [3], is to imbue the imagined or real behavior of nonhuman agents with humanlike characteristics, motivations, intentions and emotions.

Anthropomorphism is usefull for two main reasons: (1) it guarantees safety in HRI applications (see also Chap. 10) and (2) it increases robot likeability, helping robots establish social “connections” with humans. Regarding safety in HRI, when humans and robots cooperate advantageously for the execution of certain tasks, anthropomorphic robot motion can easily be predicted by humans, to comply their activity/motion and avoid injuries. Regarding robot likeability, the more human-like a robot is in terms of appearance, motion, expressions and perceived intelligence, the more likely it is to establish a social connection with humans. More details regarding the social implications of anthropomorphism, can be found in [4–6].

In [7], the authors discriminated functional and structural anthropomorphism, for the development of technical devices that assist disabled people. A functional way of developing such a device is to provide a human function independently of the structural form, while the structural way is to accurately imitate some part of the human body. Recently, we proposed a distinction between the different notions of anthropomorphism [1] and introduced functional anthropomorphism, for mapping human to robot motion. Functional Anthropomorphism has as priority to guarantee the execution of a specific functionality in task-space and then—having accomplished such a prerequisite—to optimize anthropomorphism of robot motion. Functional anthropomorphism can be used to transform human trajectories to humanlike robot trajectories, that have similar profiles in task space but different trajectories in joint space, executing with accuracy the same tasks.

The field of Learn by Demonstration (LbD) or Robot Programming by Demonstration (PbD) has also received increased attention over the last 30 years. Learn by demonstration moves from purely preprogrammed robots to very flexible user-based interfaces according to Billard et al. [8] and it is a well known approach, that has been used for various HRI applications. The concept of LbD is based on a very simple idea: that an appropriate robot controller can be derived from human observations. Thus, the ultimate scope of LbD is to formulate robot control methodologies that can be easily adapted to new environments, generalize for new tasks and perform efficiently without extra programming by the users. Some characteristic studies are those proposed by Dillman et al. in [9–12], as well as those proposed by Schaal et al. in [13–15]. All of these studies focus on learning and generalization of motor skills by robotic artifacts, utilizing human demonstrations.

Nowadays, the majority of HRI applications involve some kind of interaction with the environment. Grasping and manipulation of everyday life objects are the most common interactions and some of the most challenging areas in robotics. Thus, roboticists seek always inspiration for facilitating these interactions, at the nature’s most versatile and dexterous end-effector, the human hand. When humans grasp objects through the concept of synergies (see e.g., Chaps. 2–4 and 8), they tend to adapt their hand posture according to: (1) the object to be grasped and (2) the task to be executed with the grasped object. In [16], it was shown that humans adopt postures that maximize the force and velocity transmission ratios, along the directions imposed by the desired task. In [17], authors searched for optimal grasps using the branch-and-bound method, on a required external set. In [18] Teichmann et al. minimized the number of contact points required, to balance any external force and moment. Chiu [19] proposed an index, that measures the compatibility of a manipulator to perform a given task, Li and Sastry [20] introduced the concept of the task ellipsoid, while Mavrogiannis et al. [21] proposed a task specific grasp selection scheme for underactuated robotic hands.

Although the aforementioned studies have advanced the field of robot grasping, most of the analytical approaches, make two unrealistic assumptions: (1) that the robot hand fingers are able to reach accurately the desired contact points, (2) that the object geometry and physical parameters are known. However, these assumptions are often not verified, due to control errors, low encoder resolution and backlash that lead to contact points deviation. Furthermore, when robots interact with a dynamic environment, object properties (e.g., material, roughness, shape of the object) may be roughly estimated by the state of the art of sensing systems. In [22], authors showed that the existence of such uncertainties can “violate” the force closure property of the grasp. Thus, such uncertainties should be taken into consideration during the contact points and contact forces selection procedures. An alternative approach is to use adaptive synergies in order to achieve successful grasps, for a wide variety of objects (see Chaps. 8, 12 and 13 for technical and theoretical details).

In [23–25] the concept of Independent Contact Regions (ICR) was introduced to compensate for such uncertainties. The ICR methodology guarantees that if each contact point is located inside the corresponding regions, then the force closure property is preserved. Recently [26, 27], we synthesized a complete human-inspired optimization framework, for deriving stable, robust grasps under different task specifications and under a wide range of uncertainties. In these works, we utilized the concept of Q distance—originally proposed by Zhu et al. [28]—in a novel fashion, to incorporate the task specificity in the grasp selection algorithm.

In this chapter we present a learn by demonstration approach for closed-loop, robust, anthropomoprhic grasp planning. For doing so, we use human data in a “Learn by Demonstration” manner to perform skill transfer, between the human and the robot arm hand system. A human to robot motion mapping scheme transforms human trajectories to anthropomorphic robot trajectories, using a criterion of functional anthropomorphism [1]. The generated anthropomorphic robot trajectories are projected in low dimensional manifolds exploiting a synergistic reduction of human arm-hand kinematics, to train appropriate Navigation Function models [29], leveraging on a synergistic organization of the human upper limb as observed in [30]. These models formulate a closed-loop control scheme that embeds anthropomorphism and guarantees convergence to the desired goals. Regarding generalization, the NF models are trained in a task-specific fashion, using three different task features as described in [31, 32]: (1) the subspace to move towards, (2) the object to be grasped and (3) the task to be executed with the grasped object. The final scheme is able to produce adaptive behavior similar to humans by switching to different grasping primitives based on online feedback from a vision system. The vision system used, employs a RGB-D (Kinect, Microsoft) camera in order to perform object recognition and object pose estimation. Finally, a set of human-inspired optimization principles are incorporated in the proposed scheme, in order to facilitate the execution of robust, task-specific grasps under a wide range of uncertainties, utilizing tactile sensing.

The effectiveness of the proposed methods is experimentally verified using the 15 DoF DLR/HIT II five fingered robot hand attached at the 7 DoF Mitsubishi PA10 robot manipulator. The 4256e Grip System (Tekscan) tactile sensor setup, was used in order to: (1) measure the forces exerted by the robot fingertips, (2) minimize the level of uncertainty on the contact points, (3) facilitate the computation of sufficient contact forces. The proposed approach can be used by a robot arm hand system like the 22 DoF Mitsubishi PA 10 DLR/HIT II, to reach and grasp anthropomorphically a wide range of everyday life objects.

The rest of the chapter is organized as follows: Sect. 9.1 describes the apparatus and the kinematic models, Sect. 9.2 presents a “Learn by Demonstration” approach for closed loop, anthropomorphic grasp planning based on Navigation Function models, Sect. 9.3 presents the optimization schemes formulated to achieve task-specific, robust grasps under a wide range of uncertainties, Sect. 9.4 validates the efficiency of the proposed methods through extensive simulated and experimental paradigms, while Sect. 9.5 concludes the chapter and discusses the results.

2 Apparatus and Kinematic Models

2.1 Mitsubishi PA 10 DLR/HIT II Robot Arm Hand System

The robot arm hand system used in this work, consists of a Mitsubishi PA10 7 DoF robot manipulator and a DLR/HIT II five fingered 15 DoF robot hand (Fig. 9.1).

The Mitsubishi PA 10 is a redundant robotic manipulator, which has seven rotational DoF arranged in an anthropomorphic manner: two DoF at the shoulder, two DoF at the elbow, and three DoF at the wrist. The robot servo controller communicates via the ARCNET protocol with a dedicated PC running soft real-time linux (Gentoo). The Planner PC establishes a TCP-based communication with the robot controller PC, allowing for position, velocity and torque control modes. More details regarding the Mitsubishi PA10, can be found in [33].

Fig. 9.1

The Mitsubishi PA10 DLR/HIT II robot arm hand system

The DLR/HIT II is a five fingered dexterous robotic hand, with 15 DoF which was jointly developed by DLR (German Aerospace Center) and HIT (Harbin Institute of Technology). DLR/HIT II has five kinematically identical fingers with three DoF per finger, two for flexion/extension and one for abduction/adduction. The last joint of each finger (Distal Inter-Phalangeal – DIP joint analogous), is coupled with the middle one (Proximal Inter-Phalangeal – PIP joint analogous), using a mechanical coupling based on a steel wire with transmition ratio 1:1. The dimensions of the robot hand are considered to be human-like and the total weight is 1.6 kg. More details regarding the kinematics and the control of the DLR/HIT II can be found in [34].

2.2 Tactile Sensors

In order to capture the forces exerted by the robot fingertips we use the 4256e Grip System^® (Tekscan), which is depicted in Fig. 9.2. The Grip System is an ultra thin (0.15 mm) tactile sensor that consists of 320 sensing elements (sensels) and which is able to measure the pressure magnitude of each sensel, using piezo-resistive technology. The Grip System^® tactile arrays are mounted on the robot fingertips using appropriate rubber tape. The Planner PC establishes a TCP communication with a PC (Windows OS) that collects the forces from the tekscan system, at the frequency of 100 Hz.

Fig. 9.2

The Grip System^® tactile sensors (Tekscan)

Fig. 9.3

Motion capture systems used to capture human kinematics. a Cyberglove (Cyberglove Systems). b Liberty (Polhemus)

2.3 Motion Capture Systems

In order to record the motion of the human arm hand system, we use a magnetic position tracking system and a dataglove. The magnetic position tracking system is the Liberty^® (Polhemus Inc.) which is equipped with four position tracking sensors and a reference system. In order to capture human arm kinematics, three sensors are placed on: (1) the shoulder, (2) the elbow, (3) the wrist. More details regarding the computation of the kinematics, are presented in [35]. In order to measure the rest 22 DoF of the human hand and the wrist, we use the Cyberglove II^® (Cyberglove Systems). The Cyberglove II has 22 flex sensors, capturing all twenty DoF of the human hand and the two DoF of the human wrist. More specifically, the flexion/extension of all three joints of each finger, the abduction between the fingers, as well as the abduction/adduction and flexion/extension of the wrist, can be measured. The motion capture systems are depicted in Fig. 9.3.

The position measurements are provided by the Liberty system at the frequency of 240 Hz. The Liberty system provides high accuracy in both position and orientation, with 0.03 in. and 0.15${^\circ }$ respectively. The acquisition frequency of the Cyberglove II dataglove is 90 Hz and the nominal accuracy is less than 1${^\circ }$.

2.4 Kinematic Model of the Human Arm Hand System

In order to describe the motion of the human upper limb in 3D space, we use three rotational DoF to model the shoulder joint, one rotational DoF for the elbow joint, one rotational DoF for pronation/supination, two rotational DoF for the wrist and finally twenty rotational DoF for the human fingers. For index, middle, ring and pinky fingers, we use three DoF for flexion/extension and one DoF for abduction/adduction, while for the thumb we use two DoF for flexion/extension, one DoF for abduction/adduction and one DoF to model the opposition to other fingers. The proposed methodology can be used with a more sophisticated human hand model, like the one proposed [36], in case there is a motion capture system available, that can measure all DoF variations of such a complex model.

3 Learn by Demonstration for Closed Loop, Anthropomorphic Grasp Planning

In this section we present a learn by demonstration approach for closed-loop, anthropomorphic grasp planning.

3.1 Learn by Demonstration Experiments

Experiments were performed by five (4 male, 1 female) healthy subjects 22, 25, 28, 29 and 41 years old. Subjects gave informed consent of the experimental procedure and the experiments were approved by the Institutional Review Board of the National Technical University of Athens. All subjects, were instructed to perform multiple reach to grasp movements towards different positions and objects in 3D space. Each subject performed all trials, with the dominant upper limb (the right arm hand system for all subjects). The experiments were performed for 22 positions in 3D space, marked on 5 different shelves. Four different objects were used for the experiments: a marker, a rectangular box, a small ball and a bottle. Different grasps were executed per object (e.g., front, side and top grasps) as described in [31]. For each object and object position combination 10 reach and grasp movements were executed. Adequate resting periods were used between the trials in order for the subjects to avoid fatigue. An image presenting the bookcase used, as well as the positions marked on different shelves of a bookcase, appears in Fig. 9.4.

Fig. 9.4

Image depicting the object positions marked on different shelves of a bookcase

3.2 Mapping Human to Robot Motion with Functional Anthropomorphism

A human to robot motion mapping procedure, is used to map human kinematics to anthropomorphic robot kinematics. Various human to robot motion mapping procedures have been proposed in the past, to guarantee anthropomorphism using specific metrics of functional anthropomorphism [37], as it is also discussed in Chap. 12. In this chapter we formulate the mapping as a non-linear constrained optimization problem for the whole arm hand system, considering as end-effectors the robot fingertips.

More specifically, let $\mathbf {x}_{RAH}=f_{RAH}(\mathbf q_{RAH})$ denote the forward kinematics mapping from joint to task space for each robot arm hand system’s finger, let m be the number of the fingers, $\mathbf {x}_{RAH}, \mathbf {x}_{RAHgoal}$ $\in R^3$ the current and desired fingertip positions and $\mathbf {h}_c=(a_c,b_c,c_c,d_c)$, $\mathbf {h}_g=(a_d,b_d,c_d,d_d)$ $\in R^4$ the current and desired fingertip orientations (expressed using quaternions, to avoid singularities). Then the distance in $\mathbb {S}^3$, between human and robot fingertip orientations is defined as:

$$\begin{aligned} \bar{d}_{RAHo}(\mathbf {h}_{c},\mathbf {h}_{d}) = cos^{-1}(a_ca_d + b_cb_d + c_cc_d + d_cd_d) \end{aligned}$$

(9.1)

Taking into account the antipodal points [38], we formulate the following distance metric:

$$\begin{aligned} d_{RAHo}(\mathbf {h}_{c},\mathbf {h}_{d}) = min\{\bar{d}_{RAHo}(\mathbf {h}_{c},\mathbf {h}_{d}),\bar{d}_{RAHo}(\mathbf {h}_{c},-\mathbf {h}_{d})\}. \end{aligned}$$

(9.2)

Thus we can define the following objective function under both position and orientation goals:

$$\begin{aligned} {\begin{matrix} F^{xo}_{RAH}(\mathbf {q}_{RAH}) = w_{{RAH}{x}} \sum \limits _{i = 1}^m {\left\| {{x_{{RAH}_i}} - {x_{{RAHgoal_i}}}} \right\| ^2} + w_{{RAH}{o}} \sum \limits _{i = 1}^m d_{RAH{o_i}}(\mathbf {h}_{c_i},\mathbf {h}_{d_i}) \end{matrix}} \end{aligned}$$

(9.3)

where $w_{RAHx}$ and $w_{RAHo}$ are the weights that adjust the relative importance of the translation and rotation goal for each finger. These weights can be set according to the specifications of each study.

Moreover, in order to generate anthropomorphic robot motion, we incorporate in the objective function a criterion of functional anthropomorphism. Let $\mathbf {s}_{elbow}$ $\in R^3$ denote the position of human elbow and $\mathbf{s}_{j}$ the vector of the robot joint positions in 3D space. For n points $s_1, s_2, \ldots ,s_n$, the sum of distances between the human elbow and the robot joints positions (excluding “shoulder” and the end-effector), is given by:

$$\begin{aligned} D = \sum \limits _{j = 1}^n {\left\| {{\mathbf{{s}}_{elbow} - \mathbf{s}_j}} \right\| ^2} \end{aligned}$$

(9.4)

The objective function $F_{RAH}$ for the whole arm hand system, can be defined under position, orientation and anthropomorphism goals, as follows:

$$\begin{aligned} F_{RAH}(\mathbf {q}_{RAH})&= w_{{RAH}{x}} \sum \limits _{i = 1}^m {\left\| {x_{RAH_i}} - {x_{RAHgoal_i}} \right\| ^2} \nonumber \\&\qquad +\,w_{RAHo} \sum \limits _{i = 1}^m d_{RAH{o_i}}(\mathbf {h}_{c_i},\mathbf {h}_{g_i}) + w_{D} D \end{aligned}$$

(9.5)

where $w_{RAHx}$ and $w_{RAHo}$ are weights that adjust the relative importance of the translation and rotation goals (for each finger) and $w_{D}$ denotes the weight that adjusts the importance of the anthropomorphism criterion. The aforementioned weights can be selected according to the specifications of each study.

Thus, the problem of mapping human to robot motion with functional anthropomorphism for the case of arm hand systems, can be formulated as:

$$\begin{aligned} \text {min } F_{RAH}(\mathbf {q}_{RAH}) \end{aligned}$$

(9.6)

                  s.t.

$$\begin{aligned} \mathbf{{q}}_{RAH}^- < \mathbf{{q}}_{RAH} < \mathbf{{q}}_{RAH}^+ \end{aligned}$$

(9.7)

where $\mathbf{{q}}_{RAH} \in R^{n}$ is the vector of the joint angles and $\mathbf{{q}}_{RAH}^-$, $\mathbf{{q}}_{RAH}^+$ are the lower and upper limits of the joints, respectively. More details, regarding the proposed mapping scheme, can be found in [37].

3.3 Learning Navigation Function Models in the Anthropomorphic Robot Low-D Space

In this work, we choose to control a robot arm hand system in a closed-loop fashion, in order to reach and grasp anthropomorphically a series of everyday life objects. In this respect, we propose Navigation Function (NF) based controllers that use “fictitious” obstacle functions (Fig. 9.5). The “fictitious” obstacles are learned in the low dimensional space of the anthropomorphic robot kinematics and apply repulsive effects on the robot arm hand system, so as to reach anthropomorphic configurations. A scheme based on NF models, is able to produce humanlike robot motion guaranteeing at the same time convergence to the desired goal [29]. Navigation Functions (NF) were first proposed by Rimon and Koditschek [39, 40]. Some characteristics of the NF based models are the following: (1) they provide closed-loop motion planning, (2) guarantee convergence to the desired goals, (3) have highly nonlinear learning capability, (4) provide continuous and smooth trajectories, (5) embed anthropomorphism (synthesizing appropriate, “fictitious” obstacle functions), (6) can generalize to similar, neighboring configurations (goal positions).

The initial formulation of the NF is for a priori known sphere worlds, however, application to geometrically more complicated worlds is achieved using diffeomorphisms which map the actual obstacles to spheres. In this work, B-splines are used to learn the structure of the NF obstacle functions. More precisely, given a desired configuration $q_d$ for the robot arm or hand, the control law is constructed as follows:

$$\begin{aligned} u\left( t \right) = - K_p \left( {\nabla _q \phi } \right) \left( {x_t } \right) \end{aligned}$$

(9.8)

where $\phi$ is the navigation function responsible for: (1) driving the arm or hand to its final configuration and (2) generating new anthropomorphic robot trajectories, similar to those used for training. $K_p >0$ is a constant gain matrix and x is the system’s state. The navigation function is given from the following relationship:

$$\begin{aligned} \phi = \frac{{\gamma _d }\left( x \right) }{{\left( {\gamma _d ^k \left( x \right) + \beta } \right) ^{\frac{1}{k}} }} \end{aligned}$$

(9.9)

where x is the configuration, $\gamma _d \left( x \right) = \left\| {x - x_d } \right\| ^2$ is the paraboloid attractive effect, $\beta = \prod \nolimits _{i \in I_0 } {\beta _i }$ is the aggregated obstacle repulsive effects and $k \in$ N $\backslash \left\{ {0,1} \right\}$ is a tuning parameter. More precisely, for the training of the NF models we use the anthropomorphic robot motion that we derived from the human to robot motion mapping scheme. These data are represented in a lower dimensional manifold using the Principal Components Analysis (PCA),¹ as a standard dimensionality reduction technique. Such a technique is commonly used to cope with the redundancy of human hand architecture and to describe its synergistic organization, as discussed in Chaps. 2, 3, 5 for the analysis of human hand control, and in Chaps. 8 and 15 to design under-actuated robotic systems and under-sensed human hand pose reconstruction devices, respectively. However, synergistic coordination is not only limited to the hand: coordinated synergistic movements can be observed for the whole arm-hand system [30] and dimensionality reduction techniques can also be profitably employed in this case.

Fig. 9.5

The training procedure for the NF based models

The output of the NF models, is then back-projected in the high dimensional space in order to control the robot arm hand system. In this case, no online human to robot motion mapping is required and computational effort diminishes. Moreover, we manage to guarantee anthropomorphism as well as to transfer skills from humans to the robot arm hand system, using a learn by demonstration approach.

In this work, NF models are trained in a task-specific way. For doing so, we use the approach described in [31, 32], discriminating the following task features: (1) subspace to move towards, (2) object to be grasped and (3) task to be executed with the grasped object. Thus, different NF models are trained offline for different tasks and then “stored”. Furthermore, different NF models are trained for the robot arm and the robot hand. All models require as input the “goal” position in the low-d space of the anthropomorphic robot kinematics. The goal position can be provided by a vision system. The final scheme is able to produce adaptive robot behavior—similar to humans—by switching to different grasping primitives, based on online feedback (from the vision system).

3.4 A Vision System Based on RGB-D Cameras

In order for the proposed NF based methodology to be able to update the “goal” position of the task to be executed (based on online feedback), we have developed a vision system based on RGB-Depth cameras (Kinect, Microsoft). The developed vision system performs: (1) object recognition and (2) object pose estimation. A block diagram of the NF based scheme with the vision system incorporated, is presented in Fig. 9.6. For the development of the different vision modules, the Point Cloud Library has been used [41].

Fig. 9.6

Block diagram of the NF based scheme, with the vision system included

4 Task Specific, Robust Grasping with Tactile Sensing

In this section we present a set of optimization schemes capable of deriving task-specific, robust grasps under a wide range of uncertainties, utilizing tactile sensing.

4.1 A Scheme for Deriving Task Specific Grasping Postures

In order to derive task-specific, robust grasps, we propose a grasp selection algorithm based on the concept of Q distance, originally proposed for curved objects by Zhu et al. [28]. In this work, instead of just guaranteeing the force closure property as presented in [28], we obtain configurations that compensate disturbances in particular task-specific directions, exerting low forces. More details, regarding the utilization of the Q distance metric for deriving grasps in a task-specific manner, can be found in [27].

To formulate an optimization problem that derives task-specific grasping postures, we minimize the Q distance metric, using the joint displacements ($q\in \mathbb R^{n_q}$) and the wrist position/orientation ($w\in \mathbb R^{6}$) as decision variables. The optimization problem becomes:

$$\begin{aligned} \min \quad d_Q(\mathbf 0 ,\mathbf co (W)) \end{aligned}$$

(9.10)

         s.t.

$$\begin{aligned} q_{min} \le q \le q_{max} \end{aligned}$$

(9.11)

$$\begin{aligned} fkine(q)\in \partial O \end{aligned}$$

(9.12)

$$\begin{aligned} q_{abd/add}^{j}\le q_{abd/add}^{j+1} \end{aligned}$$

(9.13)

$$\begin{aligned} p' \notin O \end{aligned}$$

(9.14)

where $\mathbf{0 }$ is the origin of the wrench space, $\mathbf co (W)$ denotes the convex hull of the primitive wrenches and $d_Q(\mathbf 0 ,\mathbf co (W))$ the Q distance metric. Equation (9.11) sets the inequality constraints of the joint limits ($q_{min},q_{max}$), Eq. (9.12) ensures that the fingertips will be in contact with the object surface ($\partial O$) employing appropriate equality constraints, Eq. (9.13) prevents collisions between the abduction/adduction DoF of the different robot fingers ($q_{abd/add}^{i}$) and Eq. (9.14) prevents collisions between the robot hand and the object, ensuring that no point (belonging to a set $p'$ of finite discrete points) lying on the robotic hand will penetrate the object. For the rest of this chapter, we will refer to these constraints with the abbreviation RHC (Robot Hand Constraints).

4.2 A Scheme that Provides Optimal Force Transmission and Robustness Against Positioning Inaccuracies

In the previous section, we discussed the task specifications and the kinematic constraints that need to be satisfied to derive a task-specific grasp, but in most cases robot hands are also subjected to joint torque constraints. In this work we employ, the force transmission ratio $r_k$ and compatibility index c introduced in [19], in order to derive robot hand configurations that exert the required grasping forces with minimal joint torque effort. The transmission ratio and the grasp compatibility index are defined, as follows:

$$\begin{aligned}&\qquad {r_k=[u_k^T(J_iJ_i^T)u_k]^{-1/2}} \\&{c_i=\displaystyle \sum _{k=1}^{l}r_k^2=\displaystyle \sum _{k=1}^{l}[u_k^T(J_iJ_i^T)^u_k]^{-1}} \end{aligned}$$

where $u_k,\quad k=1, \ldots , l$, denote the desired directions for the contact forces and $J_i$ the Jacobian of the ith finger. In this work we use frictional hard contacts, restricting each force to lie inside the corresponding friction cone. Thus, for each contact point the unit vectors $u_k$ can be chosen to be aligned with the edges of the linearized friction cone [42]. The compatibility index for the case of the robot hand, becomes:

$$\begin{aligned} {c=\displaystyle \sum _{i=1}^{n_p}w_{f_i}c_i=\displaystyle \sum _{i=1}^{n_p}w_{f_i}\displaystyle \sum _{k=1}^{n_g}[u_k^T(J_iJ_i^T)u_k]^{-1}} \end{aligned}$$

where $w_{f_i}$ are weighting factors for each finger. The maximization of the compatibility index c yields an optimal posture with respect to the force transmission metric, however a deviation between the actual and desired joint positions may be inevitable. Thus, we have to guarantee that the robot hand will be able to perform the given task, despite the fingertip positioning inaccuracies. For doing so, we utilize the concept of independent contact regions (ICR), adopting the approach described in [25], to determine whether a point on the object boundary qualifies to be a member of the ICR. Finally, we formulate an optimization problem that provides optimal force transmission and robustness against positioning inaccuracies, as follows:

$$\begin{aligned} \min \quad \frac{\displaystyle 1}{\displaystyle c} \end{aligned}$$

(9.15)

         s.t.

$$\begin{aligned} RHC \end{aligned}$$

(9.16)

$$\begin{aligned} d_Q^{-}(\mathbf 0 ,\mathbf co (W))<0 \end{aligned}$$

(9.17)

$$\begin{aligned} p_{i}\in ICR_i \end{aligned}$$

(9.18)

The inequality $d_Q^{-}(\mathbf 0 ,\mathbf co (W))<0$ of Eq. (9.17) ensures that the force closure property will hold. Equation (9.18) constraints the deviated contact points $p_{i}$ to belong to their corresponding Independent Contact Regions. It must be noted that a task-specific grasp configuration results in larger ICRs [43], so the grasp configuration derived in previous sections is ideal to initiate this second optimization algorithm.

4.3 A Grasping Force Optimization Scheme Utilizing Tactile Sensing

In this section, we utilize tactile sensors that are appropriately attached on the robotic fingertips, in order to relax the magnitude of uncertainties regarding: (1) joint displacements and (2) contact points deviations. Then we use the derived/updated information regarding the contact points, in order to perform a grasping force optimization. Using this scheme we are able to generate a set of contact forces that balances external disturbances, preventing object deformations and requesting minimal joint torque effort. In this work, we use the 4256e Grip System (Tekscan) and the active region of each fingertip is covered with a 4$\,\times \,$4 tactile array. The sensels’ sensors allows us to compute the position of force/contact centroid, as follows:

$$\begin{aligned} x_{cof}=\frac{\displaystyle \sum _{i=0}^{3}x_i\sum _{j=0}^{3}p_{ij}}{\displaystyle \sum _{i=0}^{3}\sum _{j=0}^{3}p_{ij}}, y_{cof}=\frac{\displaystyle \sum _{j=0}^{3}y_j\sum _{i=0}^{3}p_{ij}}{\displaystyle \sum _{j=0}^{3}\sum _{i=0}^{3}p_{ij}} \end{aligned}$$

(9.19)

where $p_{ij}$ is the normal force value at each sensel, $x_i$ is the x coordinate of ith column and $y_j$ the y coordinate of jth row of the 4$\,\times \,$4 array. In order to map the centroid coordinates $(x_{cof},y_{cof})$ to 3-D coordinates on the robot fingertip surface, we exploit the point cloud of the robotic fingertip. All robot fingers have the same fingertip, so the following procedure applies for all fingers. Initially we match the 4 corner sensels of the tactile array with their actual position $p_i^{corn}, i=1, \ldots , 4$, on the point cloud and we compute the distance between them and all other nodes of the point cloud. Then, assuming that the sensor firmly covers the fingertips surface and given a contact centroid $(x_{cof},y_{cof})$, we determine the corresponding coordinates P(X, Y, Z) on the robot fingertip point cloud, minimizing the following function:

$$\begin{aligned} \mathrm{{min}}\{\sum _{i=1}^{4}(dist_i(X,Y,Z)-arraydist_i(x_{cof},y_{cof}))^2\} \end{aligned}$$

(9.20)

where $dist_i(X,Y,Z)$ denotes the distance from $p_i^{corn}$ to P(X, Y, Z) on the point cloud and $arraydist_i(x_{cof},y_{cof})$ the distance between the ith corner sensel and the contact centroid on the tactile array. Such an approach assumes that the distance between two points remains invariant both in 2D and 3D coordinates (Fig. 9.7).

Fig. 9.7

Distances on the fingertip and the tactile array respectively

Following the grasping notation, we can denote the contact forces, as follows:

$$\begin{aligned} f_c=-G^+w_{ext}+E\lambda , \end{aligned}$$

(9.21)

where $w_{ext}$ is the external disturbance, $G^+$ is the pseudoinverse of grasp matrix G (see also Chaps. 8, 12 and 13), E is a matrix whose columns form a basis for the nullspace of G and $\lambda$ is an arbitrary vector. The first term of (9.21) is responsible for the compensation of the external wrench $w_{ext}$ and $E\lambda$ denotes the set of internal forces [44]. The internal forces have a null resultant wrench to the object and are very significant for grasping, as they can control the robot hand’s ability to squeeze arbitrarily tight the object, ensuring stability. Thus, we formulate a linear optimization problem setting as decision variables the internal forces (vector $\lambda$), as follows:

$$\begin{aligned} \quad \mathrm{{min}}\sum f_{n_i} \end{aligned}$$

(9.22)

         s.t.

$$\begin{aligned} {-V'_i(f_{c_i}-n_i\Vert f_{c_{i_{max}}}\Vert )\le \mathbf 0 } \end{aligned}$$

(9.23)

$$\begin{aligned} |\tau _{i_k}|\le |\tau _{i_{k_{max}}}|-|\delta \tau _{i_{k_{max}}}| \end{aligned}$$

(9.24)

$$\begin{aligned} f_{n_i}\ge 0 \end{aligned}$$

(9.25)

where $f_{n}$ are the normal force components of the contact forces $f_{c}$. Equation (9.22) sets the friction cone constraints (represented by L-sided convex polyhedral cones to reduce computational complexity [45]), Eq. (9.23) sets the torque constraints and Eq. (9.24) constraints the contact forces values to be positive or zero. The presented algorithm searches for a set of internal forces that minimize the sum of the normal forces and therefore the grasp effort, satisfying simultaneously both the friction and torque constraints.

5 Results and Experimental Validation

In this section we present extensive experimental paradigms of the proposed methods, focusing on the two different scenarios: (1) reaching and grasping using closed loop, anthropomorphic grasp planning methodologies, (2) achieving robust, task-specific grasps under a wide range of uncertainties, utilizing bioinspired optimization principles and tactile sensing.

Using the aforementioned methods, we are now able to synthesize a complete scheme for closed-loop, robust, anthropomorphic grasp planning. The steps followed by the proposed scheme, are the following:

• 1. A vision system, performs object detection and object pose estimation.

• 2. The object shape and position information trigger a task-specific Navigation Function model (object-specific and subspace-specific).

• 3. The Navigation Function model produces anthropomorphic trajectories for the arm hand system to reach and grasp the identified object, ensuring convergence to the desired pose.

• 4. The robot fingers stop moving when the fingertips’ tactile sensors detect contact with the object.

• 5. The actual joint positions and the contact centroids are obtained from the encoders and the tactile sensors respectively.

• 6. The contact centroids are mapped to the corresponding positions on the robot fingertips.

• 7. The positions of the contact points on the object are computed, solving the robot arm hand system’s forward kinematics.

• 8. A grasping force optimization scheme is employed to compute a set of sufficient forces, for the robot hand to stably grasp the object.

5.1 Closed-Loop, Anthropomorphic Grasp Planning Scenario

All experiments were performed with the Mitsubishi PA10 DLR/HIT II robot arm hand system. A vision system based on RGB-D cameras (Kinect, Microsoft) was used to track everyday life objects, located in arbitrary positions and orientations in 3D space. In Fig. 9.8, the robot arm hand system is depicted while reaching and grasping anthropomorphically a rectangular object, in order to execute a specific task (e.g., to throw the object into the waste basket).

Fig. 9.8

Three instances of a reach to grasp motion, are depicted. a Reaching. b Grasping and lifting. c Performing the task

A video of the first experiment, can be found at the following url:

https://www.youtube.com/watch?v=cazfjEKnsxo.

5.2 Task-Specific, Robust Grasping Scenario

For the task-specific robust grasping experiments we considered the stable grasp of a cylindrical object, which is filled with liquid. We showed that the proposed methodology derives robust grasps that hold the force closure property even when the object is rotated, facing disturbances caused by the center of mass changes. Four different poses of the object are depicted in Fig. 9.9. These poses are used to model the task disturbances. The rotation is implemented about the z axis and the liquid is hypothesized to be symetrically distributed about this specific axis. In the subfigures of Fig. 9.9, the black dots denote the center of mass for each pose, while the object coordinate frame is determined by the blue axes. As it can be noticed, the object’s weight causes external forces along both the x and y axis, as well as external moments about the z axis of the object coordinate frame.

Fig. 9.9

Task description. a Pose I. b Pose II. c Pose III. d Pose IV

The bottle used for the experiment, had a radius of 2.25 cm and a height of 13 cm. For the derived configuration we considered an 8-sided linearized friction cone. The friction coefficient selected was quite conservative with $\mu =0.3$. The maximum contact point deviation (on the object) caused by the DLR/HIT II joint errors, was found to be 4 mm. For the ICRs computation, four deviated contact points were considered, at a distance of 4 mm from the nominal contact point. The friction coefficient and object model uncertainties, were considered prior to the ICRs computation, as discussed in [46]. Finally in order for the robot hand to perform the specified task, a set of internal forces were computed that satisfy both the friction and joint torque constraints, as presented in Sect. 9.4.3.

Table 9.1 Experimental data (q: degrees, $\tau$: Nm, “a/a”: abduction/adduction DoF, “f/e”: flexion/extension DoF)

In Table. 9.1 we present the derived angles q and torques $\tau$. For the computation of internal forces the uncertainty of the contact points $\delta p_{max}$ is 1 cm and the center of mass uncertainty is 3 cm. The robot hand dynamic model was considered to be the flexible joint model presented in [47] and was utilized in order to exert the derived forces. Thus, we have:

$$\begin{aligned} \tau =g(q)-\tau _{ext}=K(\theta -q) \end{aligned}$$

(9.26)

where q denotes the link position vector, $\theta$ denotes the motor position vector in link coordinates and g(q) the gravity term. Moreover, K is the stiffness matrix and $\tau _{ext}$ the external torque vector. After contact detection for a given q we may calculate the motor displacements required to exert the desired internal forces $f_{ext_d}$, as:

$$\begin{aligned} \theta = K^{-1}(g(q)-J_i^Tf_{ext_d})+q \end{aligned}$$

(9.27)

The term g(q) can be computed using the DH parameters and the nominal masses of the DLR/HIT II robot hand [47]. A video of the second experiment (see Fig. 9.10), can be found at the following URL:

https://www.youtube.com/watch?v=lkpSgamV0b8.

Fig. 9.10

Three snapshots of the reaching, grasping–lifting and task execution phases. a Reaching. b Grasping–Lifting. c Performing the task

6 Conclusions and Discussion

In this chapter we presented a complete scheme for closed-loop, robust, anthropomorphic grasp planning, following a learn by demonstration approach. For doing so, we captured multiple reach to grasp movements of the human arm hand system and we used them in order to generate anthropomorphic robot trajectories. Then, task-specific Navigation Function models were trained in the low-d space of the anthropomorphic robot kinematics, mimicing the synergistic organization of the human hand-arm system. The NF models were used to formulate a closed loop control scheme, that ensures humanlikeness of robot motion and guarantees convergence to the desired goals. Human-inspired optimization principles were proposed in order to derive task-specific, robust grasp configurations. Tactile sensors mounted on the robot fingertips were used to confront uncertainties regarding the joint displacements and the contact points positioning, relaxing also the computation of sufficient contact forces which result to stable grasps. A vision system based on RGB-D cameras was used to provide online feedback, perform object detection and object pose estimation and trigger appropriate task-specific NF models.

The proposed approach can be used by various robot arm hand systems, in order to reach and grasp anthropomorphically a plethora of everyday life objects, even under a wide range of uncertainties.