Exploiting upper-limb functional principal components for human-like motion generation of anthropomorphic robots

Movements were recorded using a 3D motion tracking system with active markers (Phase Space). Ten stereo-cameras working at 100Hz tracked the 3D position of active markers, which were placed on rigid supports and attached to upper limb links (see Fig. 2).

To map the recorded movements on an analytic kinematic model, we developed a two-phase procedure, which consisted of a preliminary calibration of the model, followed by a Kalman-based identification. In this work we used the same kinematic model discussed in [39], i.e. a serial manipulator with three rigid links connected by seven revolute joints (see Fig. 3). This model can be completely defined using 14 parameters: 2 parameters for bone length and 12 parameters to identify the position of the markers with respect to the kinematic chain (more technical details can be found in [39]). This set of parameters (p_G) was calibrated, for each subject, by solving the following optimization problem:

where r_k is the residual function:

at time frame k, calculated as the difference between the measured position of markers y_k and their estimation via Forward Kinematics (FK) f(q_k,p_G). The FK is function of q_k, i.e. the current estimation of joint angles, and p_G, i.e. the parameters of the kinematic model. The optimization problem is constrained by D_q, i.e. the range of motion of the joints, and D_p, i.e. the maximum variation of parameters with respect to a preliminary manual estimation. N_p refers to the number of samples used for the calibration – 10 equally time-spaced samples in our case, as in [39, 42]. Given the calibrated vector p_G for the specific subject, the upper limb pose is completely described by 7 joints angles \([q_{1}, \dots, q_{7}]^{T}\).

The calibrated model was then used as a component for a Kalman-based identification procedure. More specifically, if we consider, at time frame k, the joints angle vector q_k as the state of a stochastic process with w_k and v_k process and observation zero mean Gaussian noises, with covariance Q_k and R_k respectively, we have that the system can be written as

Recursively, the state q_k can be estimated from q_k−1 with the following implementation of a Kalman filter [43]: Prediction of the future state \(\hat {q}_{k|k-1} = \hat {q}_{k-1}\); Update of the state estimation as \(\hat {q}_{k|k} = \hat {q}_{k|k-1} + K_{k}\tilde {r}_{k}\). The prediction correction is the product between the residual values vector \(\tilde {r}_{k} = y_{k} -f(\hat {q}_{k|k-1})\) and the Kalman Gain K_k, defined as product between the covariance matrix estimation of the predicted state P_k|k−1, the Jacobian matrix \(H_{k} = \frac {\partial (f(q))}{\partial q}\) and the inverse of the residual covariance.

Finally, to effectively and jointly process different acquisitions with different temporal lengths, a time normalization is required. To this end, we used Dynamic Time Warping. More specifically, given a reference signal extracted from the dataset q_ref(t), all the trajectories were normalized in time with respect to (w.r.t.) the reference one, by solving the following minimization problem

where S_i and T_i are the time-stretch and the translation parameter respectively.

In the following, we will briefly summarize the main theoretical concepts behind functional Principal Component Analysis (fPCA), and discuss their application to the investigation of human upper limb motions. Let us consider a generic upper-limb movement \(q(t) : \mathbb {R} \rightarrow \mathbb {R}^{7},\) where t∈[0,t_fin] is the time variable, and 7 is the number of the upper limb degrees of freedom (DoFs) of the kinematic model we considered for our analyses. The goal of fPCA targets the identification of a suitable reduced functional representation, which can closely approximate q(t) (joint trajectories). For these reasons, fPCA can be regarded as a functional extension in the temporal domain of Principal Component Analysis (PCA). Indeed, as Principal Components identify inter-joint couplings that account for most kinematic pose variability, functional PCs (fPCs) are functions that allow to describe most of the movement variability over time, at joint level. Using the fPCA framework, a generic upper limb motion q(t) can be described as a weighted sum of a set of base functions S_i(t), or functional Principal Components, that is:

where \( \alpha _{i} \in \mathbb {R}^{\mathrm {n}}\) is a vector of weights. n is the dimension of q(t), in our case equal to 7. \(S_{i} (t)\in \mathbb {R}^{\mathrm {n}}\) is the i^th basis element and s_max is the number of basis elements. The operator ∘ is the Hadamard product, i.e. the element-wise product [44]. \(\bar {q} \in \mathbb {R}^{7}\) is the average posture of q evaluated as

while \(S_{0} : \mathbb {R} \rightarrow \mathbb {R}^{7}\) is the zero-order synergy, i.e. the average trajectory across all the trajectories q in the dataset, for all the tasks and subjects.

fPCA is used to identify a basis of functions \(\{S_{1},\dots,S_{s_{\text {max}}}\}\) that maximizes the explained variance of the movements in the collected dataset. The first fPC S₁(t) is the function that solves the following problem

where R is the number of trajectories that compose our dataset. The other fPCs S_i(t) are the functions that solve the following constrained optimization problems

This results in an ordered set of functions that - given a number of basis elements s_max - maximizes the explained variability of the joint trajectories of the dataset. This opens the possibility to use these functions as a smart basis to generate HL movements in a latent space. In the remaining of this work, we present an algorithm that exploits functional Principal Components extracted from the analysis of human upper limb movements for the generation of point-to-point robotic/artificial motions, with and without the presence of obstacles along the trajectory. These movements naturally embed HL characteristics, (see next section for the proposed method).

To test the effectiveness of this approach, we performed simulations of both free-motion and obstacle-avoidance cases. We used a robotic arm simulator with the same kinematic description we used during the experiments with human subjects. Hereinafter we refer to this simulator as dummy human. We considered four motions that are representative of the upper limb movement workspace (see the Results section): T.1) accounts for right-to-left side motions of the trunk, which typically sub-serve the execution of actions for maneuvering objects on a table; T.2) is representative of movements towards a position in front of the subject. T.3) relates to arm elevation towards a position upon the subject’s head; T.4) deals with reaching movements toward the subject’s own face, which are usually executed for self-care tasks, e.g. self-feeding etc. Numerical values of the 7 joint angles in the initial and desired final conditions, q₀ and q_fin, respectively, for each of the four tasks are reported in Table 1.

In our simulations, we considered three scenarios for each task: no obstacle (i.e. free motion), one obstacle, and two obstacles. In the latter two scenarios, we decided to place the obstacles along the optimal trajectories computed for the free-motion case. In this manner, we forced the algorithm to modify the initial guess. Details on the obstacle location – with respect to the Inertial System of Reference placed as in Fig. 3 – and dimension (expressed in terms of the center and radius of a sphere surrounding the obstacle, respectively) are reported in Table 2.

Let us consider the case in which we also need to avoid one or more obstacles, while performing the point-to-point motion. We can generalize the generation of the human-like trajectory as

Two terms can be distinguished in this cost function. The first contribution guarantees that the desired initial and final poses are achieved, as for the free motion case (12). The second term takes into account the distance w.r.t. obstacles. For the sake of conciseness, and without any loss of generality, we assume here N_O spherical obstacles. We call \(P_{\mathrm {O}} = \{P_{\mathrm {O}_{1}},\dots,P_{\mathrm {O}_{N_{\mathrm {O}}}}\}\) the set containing the Cartesian coordinates of all the centers of these obstacles.

P(q,P_O) is a potential-based function [45] that sums up, for each obstacle, a term inversely proportional to the minimum distance between the obstacle and the closest joint trajectory, i.e.

where m_i is the distance between the arm and the i−th obstacle, defined as The distance between the k−th point of contact with forward kinematics h_k, and the i−th sphere is

with \(R_{O_{i}}\) radius of the sphere.

The problem of motion generation with obstacle avoidance does not have a closed-form solution, hence the optimal trajectory is calculated via numerical optimization. To do this, we took inspiration by the ordering of fPCs basis, according to a descending amount of the associated explained variance, and implemented an incremental procedure reported in Algorithm blue1.

The proposed approach calculates for each step the optimal trajectory that minimizes the error in starting and final position while maximizing the distance from the obstacles. We consider as initial condition the one specified by (15) and (16). If the corresponding solution is sufficiently far from the obstacles, this choice already defines the globally optimal solution. If the obstacles are not very close to the aforementioned trajectory, then solving (17) with M=1 would fine tune the initial guess, achieving good results.

In case of obstacles very close to or even intercepting the free-motion trajectory, at least one more fPC should be enrolled to suitably solve the problem. The more are the basis elements enrolled, the more complex are the final trajectories that can be implemented. However, a higher cardinality of enrolled elements usually comes with a larger computational cost and dramatically increases the number of local minima, thus boosting the complexity of the problem.