Inverse kinematics strategies for physical human-robot interaction using low-impedance passive link shells

Abstract This paper presents an investigation of the effectiveness of different inverse kinematics strategies in a context of physical human-robot interaction in which passive articulated shells are mounted on the links of a serial robot for manual guidance. The concept of passive link shells is first recalled. Then, inverse kinematics strategies that are designed to plan the trajectory of the robot according to the motion sensed at the link shells are presented and formulated. The different approaches presented all aim at interpreting the motion of the shells and provide an intuitive interaction to the human user. Damped Jacobian based methods are introduced in order to alleviate singularities. A serial 5-dof robot used in previous work is briefly introduced and is used as a test case for the proposed inverse kinematics schemes. The robot includes two link shells for interaction. Simulation results based on the different inverse kinematic strategies are then presented and compared. Finally, general observations and recommendations are discussed.


Introduction
Industrial applications in which human workers and robots share a common workspace are nowadays common in industry. In the past years, numerous studies have touched on the advancements of robots that are safe enough to assist humans, whether it be in industry or at home. Different safety metrics and safety-related issues are introduced in ref. [1]. A survey of the different forms of human-machine cooperation in assembly is presented in ref. [2], which explores different safety systems as well. In ref. [3], the development of a collaborative human-robot manufacturing cell compatible with the safety standards is described. A systematic evaluation of safety in human-robot interaction, covering the most significant injury mechanisms, is proposed in ref. [4]. If they are to fulfill their purpose, these robots should not only be safe but also simple and intuitive to use.
Different approaches are studied throughout the literature to safely control a manipulator for the application of physical human-robot interaction (pHRI). In most cases, a rigid manipulator is controlled through an admittance control scheme. Some control schemes also use function approximation techniques to improve the robustness of controllers in the presence of noise (see for instance [5] and [6]). In ref. [7], a velocity-based variable impedance control using the differentiation of the force to infer human intention is presented. Reference [8] presents a variable admittance control approach to improve system intuitiveness, using desired velocity and acceleration for the inference of human intentions. In ref. [9], a variable admittance control to deliver an optimal bilateral force amplification is introduced while a new variable admittance control law that guarantees the stability of the robot is proposed in ref. [10]. Admittance control requires the use of a force/torque sensor to read the user's inputs and infer their motion of a robot by manipulating any of the links that are equipped with shells. However, the strategies that should be used to exploit the motion of the shells was not explored in detail.
In this context, the objective of this paper is to study different strategies for the inverse kinematics based on the input provided by a human user, that is, based on a measured motion of the link shells. In other words, this paper addresses the issue of interpretation of the motion of the link shells to produce the motion of the robot that will most closely correspond to the intentions of the user. The strategies investigated are then applied to the arrangement presented in ref. [23], namely a 5-dof robot with two low-impedance passive link shells mounted on the third and fifth links, in order to compare their effectiveness.

Inverse kinematics and trajectory planning of serial robots
In physical human-robot interaction, a user moves a robot from its current position rather than following a prescribed Cartesian trajectory. The robot's instantaneous motion, and hence its velocity, is then more relevant than its absolute position and orientation. As such, the objective of the paper is not to revisit the kinematics of the robot itself but to develop means of interpreting the motion of the shells with respect to the robot in order to produce an intuitive motion of the robot. Because of that, velocity control is used (as opposed to position control) because the trajectory imparted to the shell by the user is unknown a priori. The velocities of the end-effector are obtained from the joint velocities through the Jacobian matrix, as where t is the six-dimensional end-effector velocity vector,θ is the joint velocity vector, and J is the Jacobian matrix. The velocity vector t is defined by where ω andṗ are respectively the angular velocity vector and the velocity of the reference point on the end-effector. Given Eq. (2), the Jacobian matrix can be written as where A and B are the matrices related to the rotational and translational components, respectively. Most likely, the desired trajectory is defined in the Cartesian space rather than in the joint space. Particularly, the approach described here interprets the inputs from link shells as Cartesian velocities of their associated links. The inverse problem must then be solved to yield the joint velocities from the desired end-effector velocity, namelyθ where J I is a generalized inverse and where the Jacobian's dependence on the joint coordinates is dropped for convenience. Several methods can be used to solve the inverse kinematics, depending on the context. The most common methods are now briefly reviewed.

Inverse Jacobian
If the Jacobian matrix is square and non-singular, then one can write However, in a context of physical human-robot interaction, this solution can be applied only to special cases. Indeed, the inverse kinematics must be solved for a link of the robot on which a shell is mounted.
In general, the degree of freedom of the link is different from the number of joints between the base of the robot and the link. In this case, the Jacobian matrix is therefore not square. Also, it is desired that the user be able to control the robot even when it is close to singular configurations.
The above considerations make this solution not generally applicable to pHRI robots in which the human user can apply motions to different links, which is the case in this work.

Pseudo-inverse Jacobian
If the Jacobian matrix is not square but of full rank, the inverse problem can be solved by taking the pseudo-inverse rather than simply the inverse of the matrix. The pseudo-inverse solution is also known as the Moore-Penrose [24] inverse, noted J † . One then has For a full-rank matrix, the Moore-Penrose generalized inverse yields the least-square solution for an overdetermined system, namely meaning here that there are more degrees of freedom at the point of application of the inverse kinematics than there are joints between the base of the robot and the point of application. For a full-rank underdetermined system of equations, the Moore-Penrose generalized inverse corresponds to the minimum norm solution, namely meaning here that there are more joints between the base and the point of application of the inverse kinematics than there are degrees of freedom at the point of application.
In the case of an overdetermined system, Eq. (7) yields the Cartesian velocity vector that is as close as possible (in the sense of the least squares) to the prescribed Cartesian velocity vector.
In the case of an underdetermined system, Eq. (8) yields the solution with the smallest norm for the joint velocity vector that produces the prescribed Cartesian velocity vector.
The pseudo-inverse method provides a solution for non-square Jacobian matrices, but requires nonetheless a matrix inversion. The problem of a rank-deficient matrix remains.

Damped pseudo-inverse Jacobian
As mentioned above, when the robot is in a singular configuration, the Jacobian matrix is not of full rank. This means that a matrix inversion is not possible. However, this problem is not unconquerable.
In order to avoid the inversion of a matrix which is not of full rank, one can modify the matrix to make it invertible while slightly altering the solution. This method was first used in refs. [25,26]. For instance, considering Eq. (7), the damped pseudo-inverse can be written as where λ is the damping coefficient and I, the identity matrix. Using this approach, a solution that does not exactly meet the required Cartesian velocities is obtained. Nevertheless, it can be shown that by choosing an appropriate damping coefficient, a solution suitable for the application can be obtained. Using the damped pseudo-inverse of the Jacobian, it can be guaranteed that the maximum interaction force between the user and the shells will remain small, even near singular configurations. With other approaches, the interaction force may be higher, although limited.

Transpose Jacobian
Another possible approach to solve the inverse kinematic problem is to consider the robot as a quasistatic system and to assume that the desired speed at the point of application of the inverse kinematic is in fact a virtual force. Then, the resulting moment at the joints can be found from the transpose of the Jacobian matrix [27,28]. In other words, the generalized inverse is taken as and Eq. (4) becomes where α is a scaling factor. While this method does not require a matrix inversion, it is not exactly equivalent either (hence the θ notation rather thanθ ). In order to determine the value of α that minimizes the error introduced by the use of Eq. (10), it is first noted that (JJ T t) T t ≥ 0 for all J and t. Indeed, one has If the angles θ are changed by a sufficiently small α ≥ 0, then the end-effector position should change by t ≈ αJJ T t. The value of α that minimizes the error between t and αJJ T t is then obtained by

Singular value decomposition
An alternative method to compute the pseudo-inverse of a matrix is the singular value decomposition. If J is the Jacobian matrix, then its singular value decomposition can be written as [29] where U and V are orthogonal matrices and D is a diagonal matrix containing the singular values, σ i . The rank r of J is equal to the number of non-zero singular values. The singular value decomposition of J can be rewritten as where u i and v i are the i-th columns of U and V, respectively. Substituting Eq. (14) into Eqs. (7) or (8), one obtains Then, Eq. (16) can be rewritten as This method requires the computation of the reciprocal of non-zero scalars only, but it requires a singular value decomposition.
When the robot is near a singularity, at least one of the singular values σ i is close to zero, making the inversion near singularities unstable. Similarly to Eq. (9), one can damp this behavior by introducing a Figure 1. Representation of the one-degree-of-freedom macro-mini manipulator, figure taken from [13].
In the selectively damped least squares method [30], a different damping value is chosen for each singular value σ i . The damping coefficients depend not only on the current configuration of the manipulator but also on the distance between the end-effector and the target position.

Serial manipulator with low-impedance passive link shells
As mentioned above, this paper aims at developing strategies to exploit the concept of low-impedance link shells proposed in refs. [22] and [23]. The concept behind the low-impedance shell introduced in refs. [22] and [15] is now briefly recalled.
The low-impedance shell draws from the macro-mini architecture [11,12] in which a small (mini) robot is attached to a larger (macro) active robot to control the motion of the end-effector and allow a low-impedance interaction with a human user. The mini manipulator can be either active or passive. The goal of this architecture is to decouple the macro's impedance from the task and from the human user when used in a context of pHRI. In some cases, the mini manipulator also carries the payload [13,14,15]. Also, the mini robot can be passive, in which case the user feels the payload's impedance.
The concept of the macro-mini architecture is explained in detail in refs. [13,14,15,22] and briefly recalled here for convenience. Figure 1 illustrates a one-degree-of-freedom macro-mini manipulator for simplicity. The mini manipulator B is mounted on the macro manipulator A. The range of motion of the mini robot B, relative to the macro robot A, is given by 2L. Its neutral position x 2 = 0 is determined, usually the center of its reachable range, relative to the macro robot A. When the user manipulates the mini robot B away from its neutral position, the macro robot A moves in order to follow it. Therefore, the user indirectly guides the macro robot A throughout the workspace. The maximum speed and acceleration that the user can impart to the mini robot B depends on the ability of the macro robot A to catch up with the mini robot. The complete kinematic analysis is presented in refs. [13,14,15,22].
The principle of the macro-mini manipulators can be applied to a serial robot by mounting lowimpedance shells around the links of the robot, as shown in Fig. 2 and described in refs. [22] and [23]. The shell is a low-impedance passive mechanism mounted on the link of a serial manipulator, rather than its end-effector. It is then decoupled from the structure of the robot and the payload. Although the shells are mounted on passive mechanisms, their joints are equipped with encoders so that the relative motion between the shells and the links can be measured. Given the robot's speed and desire to maintain the shell in its neutral configuration by following it, the user interaction is decoupled from the robot's impedance. The mechanism connecting the shell to the robot link includes preloaded springs and mechanical limits that tend to return the shell to a neutral configuration when no external forces are applied on it.   [23].
For the purpose of this paper, the 5-DOF serial manipulator introduced in ref. [22] is used and its architecture is recalled here for convenience. The general architecture is presented in Fig. 2. The architecture is based on two clusters of joints: one with three motors near the base and the other with two motors near the end-effector.
When a single shell is used near the end-effector, the 6-DOF shell developed in ref. [22] works well. However, in such a case, only the last link of the robot can be manipulated by the human user. In order to enhance the interaction between a user and the robot, it is desirable to implement additional shells on the links of the robot closer to the base. Since these links have fewer degrees of freedom, using 6-DOF shells becomes inefficient. Furthermore, the high number of input signals becomes more complicated to interpret.
For this reason, the 6-DOF shell used in ref. [22] is replaced by two 3-DOF sensitive shells in ref. [23]. The 3-DOF shells are easier to balance and require lower pre-loads which means smaller and more intuitive interaction forces for the user. They also use revolute joints rather than prismatic joints. Instead of using a Gough-Stewart architecture, a planar 3-RRR parallel mechanism is used, thereby providing two translational degrees of freedom in the directions orthogonal to the link axis and one rotational degree of freedom around the link axis, as illustrated in Fig. 3. All sensors are positioned at the base of the shell, which simplifies the wiring.
The shells are mounted respectively on the third and fifth links of the 5-DOF serial manipulator. Each of the shells has two translational DOFs in the plane perpendicular to the link axis and one rotational DOF around the link axis.

Inverse kinematics and trajectory planning strategies
The different strategies presented in Section 3 are general approaches to solving the inverse kinematics problem at the velocity level. The objective of this research is to use these strategies in conjunction with the input motions measured between the shells and the links on which they are mounted. To this end, these general strategies must be adapted.
The mapping of the desired motion -measured by the encoders mounted on the shell mechanisms -onto the joint space of the robot is described in ref. [23] and is briefly repeated here for convenience. Considering link i of the serial n-DOF robot, one can write where J i is the Jacobian matrix, with dimension (6 × i), of link i andθ i is the vector containing the first i joint velocities. Consider a shell mounted on link i, which has i DOFs for the user inputs. The user input vector associated with shell i is noted c i which contains i components.
One can easily eliminate the rows of Eq. (20) that do not have any component corresponding to the DOFs of the shell, so as not to constrain them unnecessarily, by using a selection matrix S i . Matrix S i is of dimension ( i × 6). By expressing the Jacobian matrices in their respective link's space, S i is defined as a matrix whose components are all zero except for i unit components spread between the rows. The j-th component of each row is one if the j-th row of J i is to be kept.
Using the selection matrix S i , Eq. (20) is reduced to the relevant DOFs as and, therefore, S i J i is of dimension ( i × i). The expression S i t i is a function of c i . In order to select which joints react to which shell, a reduction matrix R i is introduced, of dimension (i × k i ) where k i ≤ i is the number of joints that should be reacting to the shell of link i. Reduction matrix R i is a matrix whose components are all zero except that each column has its j-th component equal to one if the j-th joint is active. Then,θ ir = R T iθ i contains only the active components ofθ i . Eq. (21) can be further reduced to where W i = S i J i R i is the selected and reduced Jacobian matrix and t is = S i t i . Eq. (3) shows that the Jacobian matrix can be partitioned into two submatrices, A and B, the first for the rotational velocities and the second for the translational velocities. Since both submatrices use different units and different scales, the selected and reduced Jacobian should be partitioned, as where W ir corresponds to the rotations and W it , to the translations. Vector t is is partitioned as well to yield where t isr represents the rotational components of t is and t ist , the translational components. Finally,θ ir =θ irr +θ irt (27) which is equivalent to considering shell i as two distinct sensors, one for the rotations and the other for the translations. In order to solve Eqs. (25) and (26) for the joint velocities, W ir and W it must be inverted. These matrices may not be square, which means that the simple matrix inverse (Section 3.1) cannot be used in this context. Also, the method should be applicable in singular configurations, which rules out the straightforward use of the pseudo-inverse method (Section 3.2). Therefore, the damped pseudo-inverse (Section 3.3) is used.
For the singular value decomposition (Section 3.5), a different approach can be used. Since this method treats all singular values individually, different options are presented. One can choose to damp all of them as in Eq. (19), whether or not they are near a singularity (meaning the singular value is close enough to zero). On the other hand, one can also decide to damp only those near singularities or even not to consider them at all, that i, equating their contribution to zero in the summation of Eq. (18), since the robot should not be able to move in the singular desired direction.

Direct rotations
The architecture of the custom-built serial manipulator used in this work is such that the rotational input of the shells is aligned with their respective joint axis. Therefore, one could map the rotational input directly to the current joint only with a proportional function and the two translational inputs to the preceding joints using one of the aforementioned methods. Mathematically, this method could be written asθ ir = ηr i e i +θ irt (28) where η is simply a scaling factor, r i is the rotational input, and e i is a vector whose components are all zero except for the i-th component which is equal to 1. In this case, t i becomes a two-component vector of the translational inputs.

Simulation and comparison of the trajectory planning schemes
To summarize, the method proposed in ref. [23] -and summarized in Section 5 -to map the inputs from the link shells onto a serial manipulator joint velocities can be implemented using any of the five approaches described in Section 3. From these approaches, this paper studies particularly the effectiveness of the Damped Pseudo-Inverse (Section 3.3), the Jacobian transpose (Section 3.4), and the Singular Value Decomposition (Section 3.5) inverse kinematics strategies in order to assess their capability to provide an intuitive response to the user inputs. To this end, simulation results based on an assumed input trajectory are presented in this section. It can be shown that Eqs. (9) and (19) yield the same results. Indeed, the singular value decomposition is just another algorithm to compute the general solution of a linear system of equations. For this reason, comparing the simple damped singular value decomposition grants no additional information. Rather, a variable damping coefficient λ 2 is used in Eq. (19), given by where λ represents the maximum damping coefficient when σ i = 0.

Robot parameters
The robot parameters used for the simulation correspond to the serial manipulator developed in ref. [22], as given in Tables I and II.

Trajectory
A trajectory for the low-impedance shell is needed to compare the different strategies studied. However, since the robot's response may differ depending on the strategy used, it is not possible to specify a shell trajectory in the fixed reference frame. Moreover, the shell's location in space is constrained by the link's pose and cannot be prescribed arbitrarily. For these reasons, determining a Cartesian shell trajectory a priori is difficult. A quasi-Cartesian trajectory is used instead, as described here. The procedure is illustrated in Fig. 4 considering a 1-DOF shell for simplicity. First, a general harmonic motion is determined, given by as well as a feasible direction of motion u d , expressed in the link's space. For instance, u d must be in the plane perpendicular to the link's axis for the 3-DOF shell presented earlier. For the 1-DOF example, u d is defined by the only possible direction of motion, shown by the dotted line in Fig. 4. At t 0 = 0, the shell is at rest, meaning it is at the origin of the link's coordinate system located in the neutral configuration.
At t 1 = t 0 + t, where t is given by the sampling frequency, the shell is moved in the desired direction a distance d 1 = d(t 1 ) − d(t 0 ) based on the harmonic function given in Eq. (30) where the matrix Q l is used to express u d in the Cartesian space, rather than the link space. The robot moves according to the method chosen for solving the inverse kinematics. At t 2 = t 1 + t, the shell might not be in a feasible location relative to its associated link due to the reaction of the robot. The shell position is then projected onto its feasible space to simulate its compliance to the constraints imposed by its architecture during motion. For example, Fig. 4(c) shows that p s (t 1 ) is no longer on the dotted line representing its feasible locations. However, it should be kept on this line during motion, hence the projection p sp (t 1 ). Then, the shell is moved once more in the desired direction a distance and the cycle starts anew. The maximum range of the shell relative to the link is verified for each time step, [p s (t i )] l ≤ p max , where [p s (t i )] l represents the shell's position relative to its link and p max is the maximum range of motion of the shell. The joints' acceleration and speed are also limited to the values given in Table II, and λ 2 = 0.1 is used for each of the methods using a damping coefficient.

Results
In order to compare the different methods for solving the inverse kinematics, the angle ψ between the link's Cartesian velocity and the shell's motion is studied. Indeed, a smaller angle ψ means that the robot better follows the user's inputs.
As previously mentioned, two low-impedance shells are attached to the robot, mounted on the third and fifth links. Since the architecture of the joint upstream from the shell, relative to the shell, is the same for both shells, one can study the motion of either one. The shell on the third link is chosen here.
The results obtained vary depending on the direction of u d relative to the robot's configuration. For this reason, a direction referred to as optimal is chosen. The initial configuration is chosen as the one shown in Fig. 2 and u d is parallel to the fifth link, in this configuration. This direction is called optimal for two reasons. First, a force in this direction would produce a pure moment around the second joint's axis, regardless of the robot's configuration. Second, the same force would produce a pure moment around the first joint's axis for the robot's initial configuration. Choosing such a direction should yield ψ = 0 in the initial configuration, that is, the direction of motion coincides with the motion of the shell, hence the optimal direction.
Three trajectories are studied. The parameters of Eq. (30) are given in Table III for each of the trajectories. Trajectory 1 represents a normal trajectory where both the robot and the shell remain within their limits. This means that the joints do not reach their maximum speed and the shell does not reach its physical limits. Trajectories 2 and 3 represent trajectories where both the robot and the shell reach their limits. The former uses a high amplitude while the latter uses a high frequency to achieve this.   The results obtained for angle ψ, that is, the angle between the direction of motion of the shell and the link, are presented in Figs. 5, 6, and 7 for the three trajectories and the three methods for inverse kinematics.
The results show little difference (a few degrees) between using the damped pseudo-inverse Jacobian strategy and the singular value decomposition using a variable damping coefficient, as expected. For all three figures, the angles ψ for the Jacobian transpose method are larger than those for the other two strategies, only a few degrees for Figs. 5 and 7, and about double for Fig. 6. Figures 5 and 7 illustrate similar results if the difference in frequency is omitted. Figure 6 shows a different behavior. Compared to trajectory 1, trajectory 2 corresponds to increasing the velocity and acceleration by a factor of 5 while trajectory 3 corresponds to increasing the velocity by a factor of 3 and the acceleration by a factor of 9. It should be pointed out that the key factors in characterizing the motion are the velocity and acceleration, whose effect is clearly seen here.  As previously mentioned, Fig. 2 shows the robot's initial configuration. Link 3 is parallel to link 1. The joint axes are aligned with their respective link. Vector u d is in the direction of link 5. When shell 3 is moved, joints 1 and 2 move according to the right-hand rule. As the position of joint 2 (θ 2 ) increases, u d aligns itself more and more with the axis of joint 1. In other words, when θ 2 has moved by 90 • , u d is parallel to link 1. This means that at this point, joint 1 can only produce a motion of the link that is perpendicular to u d , a purely parasitic motion. The Jacobian would be singular for this particular direction and configuration. The robot could then be reduced to one DOF when considering link 3. Only joint 2 would be active at this point.
During the motion, θ 2 increases and the angle between u d and link 1 decreases, which means that the parasitic nature of the motion of joint 1 increases. The Jacobian matrix accounts for this and reduces the involvement of joint 1. At some point, the reduced motion of joint 1 equates and then overcompensates its parasitic motion. Then, the system approaches the one-DOF robot mentioned earlier. If θ 2 were to reach 90 • , the angles shown in Fig. 6 would drop back to zero. They would then rise again as θ 2 moves away from 90 • , which explains the oscillation.
This phenomenon is also visible for the Jacobian transpose strategy, although much less drastically. The point at which the reduced motion of joint 1 equates the parasitic link motion it induces happens at a higher value of θ 1 . This point is closer to the change in direction of the trajectory, which decreases greatly the oscillations. This means that joint 1 moves more than for the other two strategies, incurring a larger parasitic motion. Hence, the angles between the link's Cartesian velocity and the shell's Cartesian motion are larger.
The results show that the Jacobian transpose strategy yields larger values of angle ψ for all three figures. Figure 6 shows a maximum value of angle ψ slightly over 25 • where the other two strategies yield about 13 • . Experimentation would be required to determine if a user can perceive the difference of about 12 • as the Jacobian transpose strategy confers other advantages which are non-negligible. The Jacobian transpose strategy does not require a matrix inversion and the oscillation of alignment are greatly reduced.

Conclusion and future work
As previously mentioned, this paper's objective is to quantitatively compare different strategies to solve the inverse kinematics when used in conjunction with the low-impedance sensitive shells developed in refs. [22] and [23]. To this end, the different strategies are presented and then adapted to fit the current application. The manipulator and the low-impedance sensitive shell are described as well. Then, a simulation is conducted to compare quantitatively the different strategies.
Since the relative motion between the shell and its associated link is taken as the desired link Cartesian velocities, the instantaneous angle between the shell's motion and the link's velocity is studied. Also, since a Cartesian trajectory for the shell cannot be determined beforehand, a procedure is developed to generate such a trajectory while taking into account the architecture of the robot and the shells.
The results from the simulations show that the performances vary greatly depending on the robot's configuration and the direction of the shell's motion. Furthermore, the Jacobian transpose strategy tends to yield the worst results while the damped pseudo-inverse and the singular value decomposition achieve similar results, as expected. Nevertheless, the advantage that the Jacobian transpose grants by avoiding the matrix inversion is not to be dismissed.
In the future, experimentation should be conducted to assert whether a human user can discriminate between the different results obtained in the simulations. If it is not the case, then the Jacobian transpose strategy may be preferable to the other two because of its computational simplicity.