Using the Functional Reach Test for Probing the Static Stability of Bipedal Standing in Humanoid Robots Based on the Passive Motion Paradigm

The goal of this paper is to analyze the static stability of a computational architecture, based on the Passive Motion Paradigm, for coordinating the redundant degrees of freedom of a humanoid robot during whole-body reaching movements in bipedal standing. The analysis is based on a simulation study that implements the Functional Reach Test, originally developed for assessing the danger of falling in elderly people.The study is carried out in the YARP environment that allows realistic simulations with the iCub humanoid robot.


Introduction
In humans the ability to stand up on two legs is a necessary prerequisite for bipedal walking.Moreover, there is ample neurophysiological evidence that standing and walking are rather independent control mechanisms.Therefore, we suggest that also humanoid robots should be trained first to master the unstable standing posture in a generality of situations and then learn to walk.
We shall address this issue in relation with the humanoid robot iCub [1], which has the size of a three-year-old child (height is 105 cm and weight is 14.2 Kg) and has 53 degrees of freedom (DoF): 7 DoFs for each arm, 9 for each hand, 6 for the head, 3 for the trunk and spine, and 6 for each leg.iCub is still unable to stand or walk but only to crawl, as baby toddlers of the same age.Therefore, the goal of this paper is to carry out a preliminary study of the computational processes that may allow iCub to achieve the sensorimotor competence that is necessary for bipedal standing.
The study builds upon what has already been achieved in the bimanual coordination of iCub's movements [2,3], using the Passive Motion Paradigm which is a biomimetic, force-field based computational model based on the equilibrium point hypothesis.The model has been evaluated and validated both in a simulated environment and in real movements.However, the present study is limited to the simulation stage for "developmental constraints, " because the sensorimotor system of iCub has not matured enough to achieve the features that are necessary for standing (postural control system) and walking (bipedal locomotion system) in a biomimetic, compliant way.Compliant motion is currently operating on the proximal joints of the upper and lower limbs.Biomimetic postural control requires a compliant ankle and this development will become available in the near future.
As a matter of fact, the whole body postural control system has two basic components: static (P1) and dynamic (P2).P1 requires that for each time instant the projection of the Center of Mass (CoM) on the ground remains inside the support base (the convex hull of the points of contact of the body with the ground).P1 is a problem of constrained coordination among the highly redundant DoFs of the human/ humanoid body, but satisfying such coordination constraints is not sufficient for maintaining balance unless the underlying dynamic controller operates with very high levels of stiffness of the joints, with particular reference to the ankle joints.This is not what happens in the human case, at least for healthy subjects who are characterized by a low level of stiffness, smaller than the rate of growth of the toppling torque due to gravity [4,5], thus inducing persistent sway movements during standing.In contrast, high-stiffness upright posture characterizes pathological conditions such as Parkinson's disease [6]: this entails apparently enhanced stability (the size of the sway movements is smaller in PD patients than in controls) but also higher sensitivity to unexpected perturbations and thus higher danger of falling.In humans robust dynamic stabilization with low levels of stiffness is achieved by intermittent control mechanisms [7][8][9], which generate small, stabilizing control bursts on top of the body postures determined by whole-body synergies.
The focus of this paper is on P1 because the current implementation constraints of the robot do not allow a low-stiffness dynamic stabilization of the standing posture, but selfadjusting static stabilization is the necessary prerequisite for achieving a general purpose mastery of the standing posture.As already mentioned, we intend to address this problem by using a biomimetic, force-field based computational model, which takes inspiration from the Passive Motion Paradigm (PMP) [10], extended to include terminal attractor properties [11].We already used this approach for the coordination of bimanual movements of the humanoid robot iCub and for modeling whole-body reaching (WBR) movements in humans [12].Here we investigate the feasibility of applying this model to the coordination of WBR movements in iCub, with particular emphasis on a specific form of WBR, namely, the Functional Reach Test (FRT).FRT has been invented as a dynamic clinical measure of balance [13]: it measures the distance between the length of the arm and the maximal forward reach in the standing position, while maintaining a fixed base of support.FRT has been tested for both validity and reliability and is used in patients with diagnoses as different as stroke, Parkinson's disease, vestibular hypofunction, multiple sclerosis, and hip fractures.FRT has also been associated with an increased risk of fall and frailty in elderly people who are unable to reach out at least 15 cm during bipedal standing.
Since the current state of the iCub's competence for the standing posture has still some "pathological" aspects, as regards the danger of falling, the improvements coming from better design and better control could be appropriately evaluated with a test similar to FRT, used with humans.

FRT Network for iCub
The network architecture which has been applied for allowing iCub to carry out the Functional Reach Test is an extension of the architecture developed for modeling whole-body reaching movements in humans.The architecture is composed of four parts: (1) task subnetwork, (2) focal subnetwork, (3) postural subnetwork, and (4) temporal coordination unit (see Figure 1).
The three subnetworks are stable dynamical systems with terminal attractor characteristics, which are provided by the temporal coordination unit.This unit generates a ( Here  0 is the initiation time and  is the duration of the coordinated forward-reaching movement; () is a minimum jerk time base generator.The general rationale of PMP, as a synergy formation mechanism for highly redundant articulated systems, is to express the goal of an action as a force field applied to the end effector and task-specific constraints as additional force fields applied to task-related body parts.In our case, the task-specific constraint is to keep the projection of the center of mass on the ground inside the support base determined by the two feet and the associated force field is applied to the hip, according to a hip-balancing strategy.The task and focal subnetworks are responsible of the former force field and the postural subnetwork takes care of the latter.

Task Subnetwork.
It generates a moving target   () in 3D space which attracts both hands of iCub (represented by the time-varying vector   ()) with a force field, induced by the focal sub-network.  evolves from the initial position of the hands, which are supposed to be jointed, to a final position   .In order to maximize the forward reach, in agreement with the nature of the FRT, the final position should be placed somehow beyond the reachable target area.In any case, a peculiar feature of the PMP approach to synergy formation is that it always implies well-formed transformations that do not collapse in the vicinity of singular configurations; in particular, if a chosen target is unreachable, given the values of robometric parameters and task-related constraints, the model guides the end effector to the final state which is closest to the target.
The investigated protocol corresponds to the most common form of FRT, that is, the bimanual test.We might also implement, in the same framework, a unimanual paradigm in which one hand is attracted by a forward-moving target and the other is either fixed to the body or is used as a further counterbalancing tool.The support base is a function of the position of the feet and in FRT they are supposed to be parallel and symmetric with respect to the body.In FRT simulations we positioned   just outside the reachable workspace, in the anterior-posterior direction.However the exact position is not critical, emphasizing the robustness of the computational model.

Focal Sub-Network.
It generates an attractive force field  foc of elastic type which is applied to both hands, implementing the focal part of the task whose goal is to allow the hands to reach or at least to approach as much as possible the target:  foc =  foc (  −   ).The force field  foc is mapped into a torque field  foc , from the task space to the joint space, by using the following transformation:  foc =     foc , where   is the Jacobian matrix of the overall kinematic chain (from feet to hands).
The admittance matrix  transforms the torque field into a movement vector q of the overall kinematic chain (note that this is not a full admittance matrix but only its viscous component). expresses the degree of participation of each individual DoF of the redundant kinematic chain of the whole body to the common synergy.Therefore, by modulating the relative values of this matrix, it is possible to implement different equivalent synergies, which may enhance the range of motion of a DoF with respect to the others.The movement vector q is then mapped from the joint space to the task space by the same Jacobian matrix, generating a prediction of the hand trajectory   and thus closing the loop.
The dynamics induced in the network by the temporal coordination unit allows the hand to reach the final position at the same time in which the final target is reached by the moving target, but there is no guarantee that, in the process, the CoM remains within the support base.Thus, if only driven by this mechanism, iCub would reach the final position but fall forward immediately after.The postural subnetwork is intended to prevent such unfortunate event.

Postural Subnetwork.
It modifies the torque field  foc , generated by the focal sub-network, by adding a "postural" component  pos that takes into account the position of the CoM on the support base.The two torque fields are then superimposed, thus generating a total torque fields that combines the focal drive and the postural stabilization:  tot =  foc +  pos . pos is computed by projecting from the task space to the joint space the postural field  pos which is defined, in the vein of the so-called "hip strategy, " as a force applied backward to the hip, in order to counteract the forward shift of the CoM induced by the focal field.This force was implemented in the model by a nonlinear function that diverges to high values when the CoM position   approaches the forward limit  max of the support base:  pos = − pos   /( max −   ).The motion   of the CoM is derived from the motion of the whole kinematic chain by using a different Jacobian matrix   that takes into account only the ankle and knee joints.The activation of the postural field is meant to induce the following effects: (1) a smaller forward shift of the CoM; (2) a backward shift of the hip; (3) a forward tilt of the trunk associated with the lowering of the CoM.It is worth noticing that this complex control pattern is not explicitly programmed but is implicitly coded by the dynamics of the network.The postural field  pos is then mapped from the task space to the joint space by the following transformation:  pos =     pos .The focal and postural fields are superimposed, generating the combined motion through the common admittance matrix.
In summary, the integrated dynamics of the interacting subnetworks are characterized by the following equations, which achieve a balance between the forward pull, applied to the hand, and the backward pull, applied to the hip: The simulations of the iCub Functional Reach Test, which correspond to the integration of the equations above, used the iCub's simulator [14] implemented under the YARP opensource middleware [15].

Simulation Experiments with FRT Network
The computational architecture described in the previous section was tested by using the iCub simulator.The "robometric" parameters of iCub (length, mass) are summarized here: These angular values are absolute, referring to the horizontal line in the sagittal plane.With this posture, the initial position of the hand has a distance of 29.05 cm with respect to the vertical line passing through the ankle joint and the CoM is shifted 3.52 cm forward.The final position   of the target was set 50 cm forward, which is slightly beyond the maximum reachable forward distance, and the limit for the CoM displacement ( max ) was set equal to 13 cm, considering that the length of iCub's foot is 15 cm.
The control parameters of the FRT network and the values used in the simulations are as follows: (i) gain of the focal field  foc = 700 N/m; (ii) gain of the postural field  pos = 2 N; (iii) admittance matrix of the whole kinematic chain .In the reported experiments the matrix is 5 × 5 because the following five joints are involved: ankle, knee, hip, shoulder, elbow.For simplicity, we chose the matrix to be diagonal, also because this allows us to choose its values in a rational way.What is important, from the point of view of synergy formation, are the relative values of the matrix diagonal.For example, if they are all equal it means that all the joints have the same weight in the participation to the common action; if one is much smaller than the others, then the corresponding joint will change very little its angular value with respect to the other joints; if one admittance value is much greater than the others, then the corresponding joint will be the one that will move the most.The relative values of the matrix diagonal can be scaled up and down with little effect on the overall synergy because the nonlinear gating provided by the Γ function tends to normalize the overall gain.In the simulation example we set the five elements to the following values:  1 (ankle) = 0.02 rad/Nms;  2 (knee) = 0.01 rad/Nms;  3 (hip) = 0.3 rad/Nms;  4 (shoulder) = 0.1 rad/Nms;  5 (elbow) = 0.07 rad/Nms.The hip admittance has the highest value in order to facilitate the counterbalancing of the forward shift of the CoM, induced by the arm-reaching movement, with a suitable backward shift of the pelvis.For the same reason the admittance of the knee and elbow is relatively smaller.But we can change the pattern of admittance values in a large range as a function of specific task or physical constraints of the robot.
With these parameter values we could obtain the simulation results illustrated in Figures 2 and 3.In particular, Figure 2 shows the initial and final postures of the FRT.The end-effectors of iCub reach forward at a distance of 46.75 cm, which is shorter than the target distance (50 cm) because the backward pull of the postural force field allows the forward shift of the CoM (11.95 cm) to remain inside the planned limit of 13 cm.Thus static stability is preserved and the robot body is stretched forward as much as possible.The increase of the hand forward reach is 17.7 cm, with respect to the initial posture.Incidentally, this value is greater than the threshold of 15 cm which is considered clinically relevant, in the sense that people who are unable to exceed such forward reach in the test have a significant risk of falling.
Figure 3 shows the time profile of the relevant variables.Figure 3(a) displays the intensities of the focal and postural force fields, respectively, together with the profile of the temporal coordination unit Γ() (dashed), which provides terminal attractor characteristics to overall model.Figure 3(b) shows the rotations patterns of the five joints (ankle, knee, hip, shoulder, elbow) from the initial to the final posture.Please note that some angles evolve monotonously from initial to termination time whereas others do not.In particular, the elbow joint angle remains equal to 0 throughout the whole movement for two reasons: (1) it was set to 0 initially in agreement with the FRT protocol and (2) it remained 0 because both force fields were directed horizontally (the focal field forward and the postural field backward, resp.). Figure 3(c) plots the forward displacements of the hand and the CoM: these curves evolve monotonously, as should do, to the final shift values that must be compared to the final position of the target and the maximum admitted forward shift of the CoM, respectively.It turns out that the hand stops about 5 cm before the target, because the latter is outside the workspace of the robot; the CoM stops a few millimeters before the prefixed limit of stability.Finally, Figure 3(d) displays the speed profiles of the hand and the CoM, respectively: they appear to be bell shaped and synchronized, in agreement with the basic findings of the research in WBR in general [16][17][18], which shows indeed that the two parts of the WBR strategy are not independent but strictly coupled by a common action generation mechanism.Figures 3(a), 3(b), and 3(c) also display the time course of the Γ function, emphasizing its role in the ordered coordination and synchronization of so many different variables.
How robust is the proposed synergy formation model for testing the static stability of the standing posture in humanoid robots?The question can be analyzed from two points of view: (1) how effective is the model for inducing an optimal or quasi optimal forward reach?and (2) how capable is it in maintaining static stability, although with a narrow margin?
In order to answer such questions we carried out a sensitivity analysis of the model with respect to the main parameters of the model: (1) the final position of the target   ; (2) the gain factors of the two force fields, namely,  foc and  pos ; (3) the relative values of the admittance matrix .
Figure 4 shows how the modulation of   in a large range influences the variation of the final reach of the hand (  ) and the forward shift of the CoM (  ).For values of   which are inside the range of reachable positions, up to about 45 cm, there is a proportionality between   , on one side, and   or   , on the other.When   exceeds 55 cm there is a saturation for both   , at about 48 cm, and   , which approaches the threshold of 13 cm, without ever crossing it (the maximum value is 12.89 cm).Therefore, in the Functional Reach Test iCub cannot stretch the hand beyond 48 cm and such performance is weakly dependent on the position of the target, provided that it is greater than 55 cm.Moreover, with the nominal values of the parameters, used for the simulations illustrated in Figures 2 and 3, static stability is preserved for any final position of the target.Of the two gain parameters ( foc and  pos ) the former one may have some influence on performance (how far iCub can reach forward, given a final position of the target) and the latter can affect, in principle, the static stability of the synergy.However, the influence is very mild.In particular, if we double the value of  foc (from 700 to 1400 N/m), the forward reach is increased by less than 1 cm (from 46.75 cm to 47.29 cm); if it is halved to 350 N/m, the forward reach is decreased by about 6 cm.Therefore  foc = 700 N/m seems to be a lower bound on the gain of the focal field.Stability is preserved in all cases in the sense that   remains always behind the threshold.
As regards the gain of the postural field, the nominal value  pos = 2 N pushes the CoM quite close to the limit (11.95 cm vs. 13 cm) but preserves static stability.Without such field, that is, by setting  pos = 0, the stability limit would be overcome by several centimeters.By reducing  pos from the nominal value the CoM will be pushed closer and closer to the limit but it is necessary to go as low as  pos = 0.005 N before losing static stability.
The admittance matrix  has an effect on the final posture of the body, when equilibrium is reached; however the influence on the task-related variables is very mild.For example, if all the elements of the diagonal are set all equal to 0.1 Nms/rad, the forward reach of the hand is changed from 46.75 cm to 46.80 cm and the forward shift of the CoM is slightly reduced from 11.95 cm to 11.93 cm.Similar results are obtained by changing the elements of the matrix by 50% or more in different combinations.

Discussion
The proposed synergy formation model is just an example of a large class of complex, whole-body tasks that can be captured by the Passive Motion Paradigm.In a recent paper [19], PMP was proposed as an alternative to optimal control for motor cognition in human and humanoid neuroscience.The next step will be to integrate in the formalism the dynamics derived from physical interaction between the body and the environment.
One of the most remarkable features of the model is that it is "self-adaptive" with respect to the articulation of the underlying body schema for the main following reasons: degrees of freedom can be added or deleted, for example, for incorporating in the body schema manipulated tools with internal dynamics or for taking into account the reduced mobility due to some kind of impairment; moreover, the synergy formation capabilities of the model remain intact, provided that the modifications of the Jacobian matrices are learned through an appropriate training: see the appendix for a possible, simple procedure of approximation and learning.
Another aspect of such computational robustness is that there is no need to compute the timing and the specific velocity profiles of all the joints, because they are implicit consequence of the internal model simulation process.In this sense, the curse of dimensionality that in most cases affects the efficiency of planning/control methods in highly redundant robots does not apply to the proposed model.
Clearly there is a link between WBR tasks like functional reaching and APAs (Anticipatory Postural Adjustments) which have been studied by many authors [20][21][22].In fact, any voluntary movement of the upper arms is intrinsically a source of disturbance to the posture and the stability of the whole body.It has been demonstrated that APAs are not reflexes but coordinative structures superimposed on the postural stabilization processes.This is also the case for WBR, in general, and for the FRT task, in particular.However, there is a difference: in WBR and FRT there is a strong coupling between the movements of the lower and upper extremities because they are both directly involved in both concurrent tasks, namely, the focal and postural task.In contrast, in APA experiments there are no targets to be reached and simple arm raises or trunk flexions/extensions are performed in order to perturb the standing posture: as a consequence, the postural system can operate independently of the arm system and this  lowers the degree of correlation between the two kinds of movements.
Finally, let us consider the possible impact of biomedical robotics and biomechatronics technology on healthcare.There are many reasons for assuming that humanoid robotics can give a significant contribution to this theme in a very general sense.There is indeed a widely shared vision that a new generation of robotic technologies-robot companions for citizens-can help our society to come to terms with the special needs of an ageing population, in such a way to remain creative, productive, autonomous, and independent.For example, this is the vision of the FET Flagship Initiatives RoboCom, which envisages a new generation of soft, sentient machines that will help and assist humans in activities of daily living.We believe that the computational model presented in this paper goes in that direction because it allows a humanoid robot to acquire a degree of competence in focal/postural activities which is a prerequisite for safe, soft, friendly interaction between a robot companion and a needing human.

Figure 1 :
Figure 1: PMP network for the iCub humanoid robot that carries out the Functional Reach Test (FRT).  and   are the 3-dimensional position vectors of the final and moving targets, respectively;   is the position of the hand;   is the position of the CoM;  is the 5-dimensional joint rotation vector (ankle, knee, hip, shoulder, elbow).  is the 3 × 5 Jacobian matrix of the whole kinematic chain, used by the focal sub-network;   is the 3 × 3 Jacobian matrix of the partial kinematic chain (from ankle to hip) which is used by the postural subnetwork. foc ,  pos are the force field generators of the two subnetworks;   ,   are the corresponding force fields;   ,   are the related torque fields. is the 5 × 5, diagonal admittance matrix.Γ() is the temporal coordination function.

Figure 2 :
Figure 2: Initial and final poses of iCub in the Functional Reach Test.

Figure 3 :
Figure 3: (a) Time course of the forces generated by the focal and the postural subnetworks, respectively, (continuous lines); time-base generator (dashed line).(b) Time course of the joint rotation angles, after subtracting the mean value:  1 (ankle) = 1.38 rad;  2 (knee) = 1.60 rad;  3 (hip) = 0.83 rad;  4 (shoulder) = 5.95 rad;  5 (elbow) = 0 rad.The angular values are absolute, relative to the horizontal line.(c) Forward shift of the hand (Functional Reach), related to the forward position of the target (50 cm with respect to the ankle) and forward shift of the CoM, related to the maximum stable position on the support base ( max = 13 cm).(d) Velocity profiles of the hand and the CoM.Panel (a) also displays the time course of the Γ function (dashed line).

Figure 4 :
Figure 4: Top panel: final position of the hand in the forward direction (  ) as a function of the final position of the target (  ).Bottom panel: final position of the CoM (  ) as a function of the final position of the target (  ); stability requires that   is smaller than a critical threshold  max (dashed line).