ERK'2021, Portorož, 175-178 175 Optimizing end-effector force during the sit-stand task on the Talos humanoid bipedal robot Luka Miˇ skovi´ c 1;2 , Rebeka Kropivˇ sek Leskovar 1;2 , Andrej Gams 1;2 , Tadej Petriˇ c 1;2 1 Odsek za avtomatiko, biokibernetiko in robotiko, Inˇ stitut ”Joˇ zef Stefan”, Jamova cesta 39, 1000 Ljubljana 2 Mednarodna podiplomska ˇ sola Joˇ zefa Stefana, Jamova cesta 39, 1000 Ljubljana E-poˇ sta:fluka.miskovic, rebeka.leskovar, andrej.gams, tadej.petricg@ijs.si Abstract In this paper, we present the demanding task of a hu- manoid bipedal robot Talos getting up from a squat pos- ture by finding the optimal force applied to the robot’s end-effector by pulling it. The corresponding force acts together with the Talos’ own torque in the joints. First, we recorded demonstrations of sit-to-stand motions from hu- man subjects using the Optitrack motion capture system and mapped the demonstrated motions to the humanoid robot. We derived a cost function that was minimized using fmincon and a genetic algorithm. The results are presented in the simulation using Matlab and MuJoCo. The work is part of the Switch project, whose goal is to enhance robot learning performance by role reversal be- tween robots and humans, and to implement complex hu- man dynamics into robot behavior. The first step is that the human helps the robot stand up, and the second step is for the robot to help the human to stand up. In this paper, the first procedure is elucidated. 1 Introduction We humans are able to perform dynamic and dexterous movements with relative ease: walking, jumping, and dance are just one of a few examples. Standing up from a chair is one of these everyday tasks that appears to be easy, but is actually quite complex and dynamic and can become challenging and dangerous as we age. The suc- cess of this task requires whole body coordination, bal- ance and high fidelity control. In the field of robotics, we are interested in movements such as standing up and seek to better understand their complexities, as this al- lows us to improve current state-of-the-art robotic sys- tems. Traditional robots require users to possess pro- gramming skills, which makes robots inaccessible to the general public [1]. On the other hand, future generations of home and service robots, including humanoid robots, are expected to perform tasks in more natural and highly dynamic environments created for humans, which is why it is necessary for us to study the natural human move- ment and how to implement it into robot control systems. One such method is learning from demonstration (LfD), which has been used to help robots independently implement manipulation behavior by observing move- ment performed by human demonstrators as seen in [2]. In LfD applications, example trajectories can be captured in various ways, e.g. kinesthetic demonstration, motion- sensor demonstration, teleoperation, and methods like vi- sual observation [3] [4]. After example trajectories are recorded, Inverse kinematics is often used to calculate the joint angles [3], however such operations need relatively high computational cost. In [5] a particle filter is used for joint angle imitation instead, in order to realize a real- time imitation of a humanoid robot through observation of human demonstration. Figure 1: The concept of humanoid helper robot in house- hold environment. In general, to achieve robot learning from demonstra- tion, we need to address three challenges: the problem of correspondence, generalization, and robustness against perturbation [6]. The problem of correspondence occurs due to the robots and human differences in kinematics, i.e. they do not have links and joints in the same place. Due to this, learning by demonstration is only feasible when the demonstrated movement can be generalized, for instance with different target positions. However, accu- rate repetition of the observed movement is unrealistic in a dynamic environment, in which obstacles can suddenly 176 appear [1]. Due to this fact, in LfD robustness against perturbation is needed. With all the studies mentioned the robots in human- physical collaboration are still facing a shortcoming due to their limitations in observing and adapting to human dynamics. The goal of the Switch project is to develop methods that can address these known limitations and al- low the robot to efficiently observe human dynamics in real-time and learn anticipatory models from demonstra- tion. The first step achieving this goal is for the robot to be able to observe a human picking it up, before switch- ing the roles. Due to safety precautions however, we must first find out the amount of force needed to safely pickup the robot. This is why the goal of this paper is to find the external forces needed to lift the Talos on its feet. To do so, we use two Matlab optimization methods by first recording the movements of humans standing up from chairs and later transferring the recorded move- ments to the humanoid robot Talos. The paper is orga- nized as follows. The section 2 presents the kinematics of Talos and the methods used. It is also explained how the cost function is derived. Section 3 shows the results obtained. Furthermore, the discussion follows in Section 4 after the result. 2 Methods 2.1 Humanoid bipedal robot Talos The robot used in this study was the bipedal humanoid robot Talos. Its kinematics is explained in this chap- ter so as to better understand the difference between hu- man. The bipedal humanoid TALOS, built by the com- pany PAL-Robotics, has 32 degrees of freedom, of which two are in the head, seven per each arm, one per hand, two in the waist and six per each leg. TALOS is fully torque-controllable thanks to the torque sensor feedback in all joints, which enables powerful sensing and multi- contact motions. The closed loop torque control can be used for whole-body control inverse dynamics and safe interaction with the environment [7]. Figure 2 shows the kinematics of Talos. Figure 2: Kinematics of TALOS 2.2 Optimization Force profile: In this section is described the process of finding the optimal force applied to the robot’s end- effector by pulling the Talos from its sitting position. To describe a force we used a parametric description. In this description, force is defined as a linear combination of radial basis functions (RBF). As proposed in [8], among many possible radial basis functions, those with Gaussian kernels were used. They are defined as: ( x) =e (x c) 2 2 ; (1) wherec defines the center, defines the width of the ker- nel function andx is the phase parameter (see [8] for de- tails). To describe a force trajectory we use a parametric de- scription: f(x) = P m j=1 w j j (x) P m j=1 j (x) ; (2) where m is the number of kernel functions, and w j are the weights that define the path. We define as a row vector with components: j (x) = j (x) P m j=1 j (x) ; (3) which yields: f(x) = (x)w; (4) where w is a vector with elementsw j . The corresponding weights can be found by solving Eq. (4) using pseudoinverse: w = ( ^ T ^ ) 1 ^ T ^ f; (5) where ^ f are the path values and ^ basis vectors for all path steps. To get the optimal force profile the weights of radial basis functions need to be optimized. This is done by optimizing the cost function. If not stated otherwise we usedm = 6 RBF functions. Cost function: The cost function used in this study was composed of four quadratic functions. It is defined as: J = m X n=1 K n f n ; K n 0; (6) where f n denotes the quadratic loss functions, K n are the weights, which determine the importance of each quadratic loss function, andm is the number of quadratic loss functions. The first quadratic loss function takes the sum of all the positions of the Talos’ head in time and tries to bring them to the desired position. This ensures that in the end, the robot stands upright. Talos’ positions and orientations were obtained in simulation during its movement, using MuJoCo dynamic library. The first quadratic loss func- tion is defined as: 177 f 1 = tm X t=0 (p H p H;d ) 2 ; (7) wheret m denotes the total time during getting up,p H is the measured head position, andp H;d is the desired head position. To increase the stability of the robot, a second quadratic loss function was used to ensure that the pitch angle of the Talos’ feet is minimal, or ideally zero, throughout the standing up process. Accordingly, the sec- ond quadratic loss function is derived as: f 2 = tm X t=0 2 ; (8) where is the pitch angle of the robot’s feet. Similarly, the third quadratic loss function minimizes the velocity of the torso, which as well increases the sta- bility of the robot and is defined as: f 3 = tm X t=0 _ p T 2 ; (9) where _ p T is the torso velocity. The last part of the cost function finds the weights of the Gaussian kernel functions and is defined as: f 4 = m X j=1 w j 2 ; (10) wherew is the vector with the weights of Gaussian ker- nel functions, which is found through the optimization process. Finally, with the values of the weights w n from the Eq. 6 adjusted, the lower and upper bounds of optimiza- tion and the initial guess defined, the optimization prob- lem was solved by finding the solution vectorw that min- imizes the cost function valuef val , defined as: f val = min w J(w): (11) The cost function was optimized using two different Matlab functions: fmincon and the genetic algorithm. Fmincon finds a constrained minimum of a function of several variables through an iterative process. In our case, the ’active-set’ algorithm was set in options. On the other hand, genetic algorithm generates a population of points at each iteration and modifies a population of individ- ual solutions. Here, the best point in the population ap- proaches an optimal solution [9]. Movement of human subject’s six joints during sit-to- stand motion were captured using Optitrack motion cap- ture system. These angles were mapped to the six joints of the humanoid robot: ankle, knee, hip, torso, shoulder, and elbow. The human subject was about the same height as the robot. This enabled the robot to imitate natural hu- man movement during standing up as close as possible. The force applied to the robot’s arms can be understood as an external perturbation force that acts on the robot’s end-effector and helps it to stand up. The Talos’ joints are controlled in position mode. The resulting force calculated with the optimization is applied to the end effector of the Talos robot, i.e., the hands. By multiplying the force by the rotation matrix, we ensure that the direction of the force is aligned with the robot arms. 3 Results When using fmincon solver, the value of the cost function converges faster compared to the genetic algorithm which we can see in Figure 3. By analyzing the time required for each method to find the optimal solution, we have found that fmincon takes about 41 seconds, while the genetic al- gorithm takes 4922 seconds on a typical desktop pc with i7 processor. Figure 3: Comparison of convergence between the fmin- con solver and the genetic algorithm. The resulting optimized force profiles found using fmincon and genetic algorithm methods are shown in Fig- ure 4. For comparising we also show the results of force optimization withm = 20 RBFs. Figure 4: Optimized forces with different solvers and number of RBFs Figure 5 shows the result of Talos standing up in sim- ulation environment. 178 Figure 5: Talos robot in MuJoCo in sitting position (top) and standing position (bottom) 4 Discussion In this paper, we presented a way to find the optimal force path needed to lift the bipedal mobile robot Talos to the standing position by pulling its hands, using two differ- ent optimization methods in Matlab. The results of the experiment show that the bipedal mobile robot Talos is able to stand up successfully using the methods proposed in this paper. If at least the approximate trajectory of the force is not known before real-life experiments, it could be a potential safety hazard. Analyzing the graphs, we can see that both the fmin- con solver and the genetic algorithm resulted in very similar solutions (force profile) when the weights of 6 RBFs were optimized. However, increasing the number of RBFs resulted in overfitting and more profile curva- ture, as can be seen in Figure 4. We also found that the fmincon method takes less time to find the minimal solution compared to the genetic algorithm. This is to be expected since fmincon uses the local gradient to tune the parameters, which means it is faster, but it might get stuck in a local minimum, while the genetic algorithm generates a random trial and then shuffles them to generate a new generation that ends up finding a global optimum. In general, both methods can be used for similar op- timization problems, but in our case, due to the fact that both methods gave the same result, fmincon can be con- sidered as the preferred method. The work presented in this paper is an important step to get a first impression about the shape and amplitude of the profile. This allows us to gain an approximation of the force profile needed to safely lift Talos from a seated position in real life. With this knowledge, Talos will be able to help a person stand up in the future. Acknowledgement: This work was supported by Slove- nian Research Agency grant PR-10489 and N2-0153. References [1] Z. Zhu and H. Hu, “Robot learning from demonstration in robotic assembly: A survey,” Robotics, vol. 7, no. 2, 2018. [2] H. Ravichandar, A. S. Polydoros, S. Chernova, and A. Bil- lard, “Recent Advances in Robot Learning from Demon- stration,” Annual Review of Control, Robotics, and Au- tonomous Systems, vol. 3, no. 1, pp. 297–330, 2020. [3] Y . Ou, J. Hu, Z. Wang, Y . Fu, X. Wu, and X. Li, “A Real-Time Human Imitation System Us- ing Kinect,” International Journal of Social Robotics, vol. 7, no. 5, pp. 587–600, 2015. [Online]. Available: http://dx.doi.org/10.1007/s12369-015-0296-9 [4] R. Dillmann, “Teaching and learning of robot tasks via observation of human performance,” Robotics and Au- tonomous Systems, vol. 47, no. 2-3, pp. 109–116, 2004. [5] Y . Kondo, S. Yamamoto, and Y . Takahashi, “Real-Time Posture Imitation of Biped Humanoid Robot Based on Par- ticle Filter with Simple Joint Control for Standing Stabi- lization,” Proceedings - 2016 Joint 8th International Con- ference on Soft Computing and Intelligent Systems and 2016 17th International Symposium on Advanced Intelli- gent Systems, SCIS-ISIS 2016, pp. 130–135, 2016. [6] P. Pastor, H. Hoffmann, T. Asfour, and S. Schaal, “Learning and generalization of motor skills by learning from demon- stration,” pp. 763–768, 2009. [7] O. Stasse, T. Flayols, R. Budhiraja, K. Giraud-Esclasse, J. Carpentier, J. Mirabel, A. Del Prete, P. Soueres, N. Mansard, F. Lamiraux, J. P. Laumond, L. Marchionni, H. Tome, and F. Ferro, “TALOS: A new humanoid research platform targeted for industrial applications,” IEEE-RAS International Conference on Humanoid Robots, pp. 689– 695, 2017. [8] L. ˇ Zlajpah and T. Petriˇ c, “Unified Virtual Guides Frame- work for Path Tracking Tasks,” Robotica, vol. 38, no. 10, pp. 1807–1823, 2020. [9] C. Mathworks, “Statistics and Machine Learning Toolbox TM User’s Guide R2020a,” MATLAB Mathworks Inc, vol. 2020a, no. A, p. 7984, 2020. [Online]. Available: www.mathworks.com