Humanoid Full-Body Manipulation Motion Planning. with Multiple Initial-Guess and Key Postures

Transcription

1 Humanoid Full-Body Manipulation Motion Planning with Multiple Initial-Guess and Key Postures Ph.D Research Qualification Report Department of Mechanical Engineering Bowei Tang September 15, 2014 Bowei Tang, is with the Department of Mechanical Engineering at Carnegie Mellon University, supervised by Prof. Chris G. Atkeson and Prof. Hartmut Geyer

2 Abstract Whole body manipulation plays a very important role in the usage of humanoid robot, which meet the requirement of service and household in domestic environment, as well as disaster recover in extreme environment. Because of the high redundancy system, optimization methods are widely used to solve Inverse Kinematics (IK) of humanoid robot. However, most of current methods suffer a problem of getting stacked in local minimum or singularities, and lack of the ability to generate diverse motions. In this paper, we present an optimization method to solve high redundancy IK problems and generate a good optimal trajectory for humanoid robot full-body manipulation statics motion. Our algorithm could effectively avoid unexpected local minimum solutions caused by bad initial guess and develop diverse motion variations. The basic idea of our algorithm is to generate multiple diverse initial guesses for each manually set key postures, use traditional optimization method to generate optimal solutions based on these initial guesses. Then we evaluate those optimal solutions inside the whole motion process and find the best optimal solution series. Finally, we interpolate the discrete solution series into a smooth trajectory. The performance of our approach is implemented in Gazebo simulation using Atlas by Boston Dynamics.

3 1 Introduction The DARPA Robotics Challenge (DRC) is designed to evolve humanoid robots that can work in disaster and hazardous environments. Several tasks in DRC, such as the debris task and manipulation tasks, involve an accurate, reliable and fast whole body manipulation ability and performing kinematically complex motions. It has been widely known that getting an optimal trajectory of humanoid robots whole body manipulation is a very difficult issue due to the complexity of the humanoids kinematics and task requirements. In this paper, we introduce a full body trajectory optimization approach for the simulation phase of humanoid Atlas, a 28-DOF (degree of freedom) hydraulic robot built by Boston Dynamics. Because of the inaccuracy of high force hydraulic actuator and sensor, it is very difficult to apply torque control from a result of dynamic trajectory optimization directly into the whole body manipulation. Instead, we use statics motion to track the joint space trajectory. It has been hypothesized that many of these complex trajectories could be divided into the combination of a series of simple actions. The computational cost of the optimization of a single pose inverse kinematics is much less than the optimization of a whole trajectory. A reasonable integration of a trajectory across these stable key poses will remain stable. In that case, our approach could find better optimal solution of each key poses and reduce the optimization time greatly. The remainder of this paper is organized as follows: Section II introduced some of widely used methods to solve inverse kinematics problems with optimization approach. Section III presents our multi initial guess algorithms to find the optimal trajectory. Section IV demonstrate how our algorithm works on the Debris Task in DRC. Finally, Section V concludes the paper and discuss the advantage of our algorithm. 1

4 2 Related Work Generating relatively impressive motion trajectories can be very slow [1][2], or have n- ever been used on real robots [3] and usually do not take into account the richness of the information created by the available sensors. Due to the kinematics redundancy, there is no analytical unique solutions for inverse kinematics (IK) problem with larger than six degrees of freedom(dof) [4]. Instead, numerical techniques based on pseudo-inverse of Jacobian matrices J, the optimization methods and Rapidly-exploring Random Trees (RRTs) are widely used. Directly solve the IK problems using pseudo inverse of Jacobian Matrix [5][6][7] is not conservative and not always easy to determine the constraints. These issues motivated researchers to focus on resolving kinematic redundancies with optimization methods like extended Jacobian method[6], Particle Swarm Optimization[8], dynamic programming [9] [10], or non-linear optimization[11]. However, the complexity of humanoid robot makes the objective function not convex and full of local minimum. As a result, the solution always converge to a strange and unreliable pose. In that case, the optimization result is highly depends on initial guess. In order to get a reasonable initial guess for a better trajectory optimization result, many researches focus on tracking a reference pre-recorded human motion based on motion capture data [12] [13] for generation of physically-simulated humanoid character [14][15][16] that has human-like motions [17]. Human motion data is a very reasonable initial guess with the similar kinematic structure and by good scaling approach to transmit human kinematics parameter into humanoid s [18][19][20]. This method lack of flexibility for different unknown tasks and environments, so there are also algorithms that learn from examples and formulate feasible fullbody plans to traverse a new environment [21] or build a library of optimal trajectories [22]. With the development of computer hardware, more researches have been developed for 2

5 exploring optimal humanoid motion trajectory without pre-record data. Mordatch [3], Borno [1], Ramos [23] developed a class of approaches aiming at identifying functional principles of human movements. There are also research focusing on generating different kinds of motions [24][25] with great mount of computing cost. We extend previous work and argue that the trajectory generation process could be divided into the automatic planning phase and real time practice phase. During the planning phase, we generate multiple kinds of optimal trajectory libraries; during the practice phase, we use the trajectory libraries as initial guess and online optimize a modified trajectory for real environment. 3 Algorithm Instead of using neighboring optimal control method to generate local approximations to the optimal control [22], our approach focuses on integrating full-body trajectory from a series of globally optimal key poses and find local minimum solutions for each posture. Similar to previous research about humanoid animation that divided trajectory into key frames [26] or short space-time windows [1], we define panel points of the whole tasks as the key poses. For a single posture, with different initial guess, we could have multiple local minimum solutions that could all achieve task requirements and follow the constraints and penalties. Then we evaluate the solution series to get the global optimal pose series, associate these key poses as the variable and use these solutions as the value to find the best solution in the pose series. Finally, we generate the trajectory based on the optimal key poses. 3.1 Define Key Postures and Objective Functions The key postures should contain major differences in body pose, contact situation and action purpose. For a certain task, there are four kinds of key postures: the initial posture that connect to previous action, the final posture that connect to the next action, the task postures that is 3

6 required to finish the goal of the task, the preparation postures that could avoid obstacles and connect the task postures. A common way to define the key postures should be directly using human experience and work for effective Human-in-the-Loop control [27]. If we want to offer the robot a general framework towards autonomy and versatility, we may use reinforcement learning [28] [29][30] to train the robot. In this research, we did not focus on the learning phase, so we directly use human input for our simulation test. In one single key posture k, we use the same cost functions L k : L k (q) = ω i J i (q τ,p) (1) i where q is the joint angle set; τ is the joint torque set; p is the 6-D position of end effector, here it is the position of left hand. We want robot s operational hand to reach the target, so the cost function of end effectors is: J e f (q) = m i=1 ω i [P i p i (q)] 2 (2) where m is the number of end-effector ω i is a constant weight to measure the importance of the target i; P i is the goal of the i th end effector, p i (q) is the position of i th end effector computed through Forward Kinematics. By setting multiple end effectors, we could control the positions of all the necessary parts of the robot besides hands. We want the robot to stand balance in a static position. So we use: J c (q) = (c c) T (c c) (3) to move the center of mass c to the desired position c. We also want to penalize joint torques that balanced gravity at each posture, so we set the third term: J t (q) = τ (q) T Wτ (q) (4) 4

7 to reduce the sum of torques at each joints follows a series of manually designed weight. Here τ(q) is the joint torque set computed through Inverse Dynamics to balance the gravity; the weight matrix W is a diagonal matrix with the weights of each joints at the diagonal elements. Even with the penalties, the solution will still fall into a bad local minimum. For the purpose of accuracy, we also set the position of the executing end effector, left hand of Atlas, as a constraint. So we force the end effector into the right place with the right orientation by the constraints: p i (q) = P i,i = 1,2,...,m (5) We also want the robot to follow its kinematic limitations: q min q q max (6) and dynamic limitations: τ min τ (q) τ max (7) 3.2 Distribute initial guesses to cover the whole joint space In order to find different available postures for generating the approximate global minimum trajectory, we need to generate the initial guess as different from each other as possible [24] so that we could cover the whole working space the best. This is achieved by the diversity object function: E IG = M M ( d(qi,q j ) + Kd min (q i,q 0 ) i=0( ) ) + C i (q i ) j=0 i where q is the joint angle set, M is the total number of initial guess d(q i,q j ) = M k=1 (q i q j ) T W(q i q j ) is the distance measurement between two joint angle sets with a weight matrix W that distinguish the importance of each joint. q 0 is a zero definite vector in joint space. d min (q i,q 0 ) is the minimal distance of all joint set to zero vector. K is a weighting constant affect the distribute of the initial guesses. C i is the constraint function that could limit the (8) 5

8 initial guess close to the optimal solutions. With this objective function, we could make the initial guess joint set covers as much of the joint space as possible[24]. 3.3 Find the optimization solution of single key pose SNOPT is a general-purpose system for constrained optimization using sparse Sequential Quadratic Programming (SQP) method [31]. We use it to compute the optimal solution of the objective function (1) for posture k with the given initial guess ig i. q k i : argmin q k R 28 L k (q ig i ) 3.4 Evaluate trajectory In order to reduce computational complexity in trajectory optimization for humanoid robot, we just focus on the performance of the key poses and ignore the process between each nearby key poses. So the trajectory is reconstructed as a combination of a series of key poses. We define the solutions in each key pose as the variables: [ ] T X = S 1 S 2... S n (9) where S i is the solution of key pose i. Each key pose has a library of available optimal solutions from previous step. We use these solutions as the value to do optimization: L traj (X ) = ω i E i (X ) (10) i X : argminl traj (X ) (11) X Since all the available solutions are balanced and reach the target requirement, we do not set these two solid requirements as the penalties in this step Total torque The torque of each poses has been optimized, we do not focus on the maximum torque of the trajectory. Instead, we consider the sum of torques of each poses and find the minimum 6

9 trajectory: E t = n i=1 τ (q i ) T W t τ (q i ) (12) where n is the number of key poses. τ (q i ) is the torque vector of key posture i, W t is weight matrix for each joint to measure the importance of each joint torques Differences in joint space In order to reduce uncertainty, we want the robot to move as little as possible between each key pose. So the change of joint angles is also a penalty that is required to be minimized. E a = n+1 i=1 (q i q i 1 ) T W a (q i q i 1 ) (13) where n is the number of key poses, q 0 represent initial pose, q n+1 represent final pose. W a is weight matrix for each joint to measure the importance of each joint displacement. For example, we do not want much movement for the major joint such as hip, knee or waist, so we gives a large number of weights for them; but some joint are not that important for balancing such as wrist or neck, so they have smaller weights. 3.5 Generate trajectory We directly compute N = n i=1 m i trajectories and find the best key posture series with the least cost following Function (11). Here n is the total number of Key Postures, we have n = 4 in our experiment, m i is the number of solutions in Key Posture i. In order to get a continuous motion, we need to fill the points between the nearby postures. We use the Lagrange method to interpolate the inner points with the computed key posture series. q(t) = n j=1 q j n i=1 i j t t i,t km,t N (14) t j t i where t is the interpolated points, t i = im and t j = jm are the points at node i and j. n is the total number of key postures, N is the number of interpolated points between nearby key poses. 7

10 Through Lagrange Interpolation, we could get a smooth and continuous trajectory passing through all the key postures. However, even if this trajectory is built from the combination of the global solutions, we still need to consider the disturbance of the unknown environment. We then use the integrated optimal trajectory as the initial guess of the real motion at each point, then re-solve the Inverse Kinematics and find the optimal trajectory of each step. Since the preoptimized initial guess is already satisfied the task requirement and very close to the optimal motion, the process will be fast enough for online optimization. Our algorithm not only compute the motion planning of humanoid robot, but could also extended to many other motion planning problems with large workable space and Human-inthe-Loop control. 4 Experiment In this section, we demonstrate the performance of our algorithm through the Debris Task in DARPA Robotics Challenges using the 28 DOFs Altas humanoid created by Boston Dynamics. The motion process starts from a initial posture that the robot stand balanced with two arm place on the sides of its body and end up with the same posture. The end effector is the operational right hand. We also set the position and direction of two feet so that the robot will not step when doing the grasping action. In the grasping action of one wood strip, we manually define four Key Postures: Prepare, Grasp, Lift up and Throw, as Fig. 1 shows. 4.1 Compute different optimal solutions In each of specific pre-defined key posture, we distribute 15 Initial Guesses, thus we could find optimal solutions from these initial guesses. Fig. 2 shows four different postures that have the same propose with the same end effector position. 8

11 (a) Pre-grasp (b) Grasp (c) Up-Lift (d) Throw Figure 1: Four Key Postures for grasping the one strip in Debris Task (a) Action-1 (b) Action-2 (c) Action-3 (d) Action-4 Figure 2: Multiple Optimal Solutions for Grasping Posture 4.2 Generate Trajectory Once we have mi (mi 15) optimal solutions for key posture i, we could generate N = ni=1 mi trajectories. From these N trajectories, we could find the best one according to Func (10). The best trajectory is shown in Fig. 3. In addition, we could also find the next best action, third best action, which could give us many other diverse motion options. 5 Conclusion and Discussion We have presented a full-body trajectory optimization method that relies on manually set key postures with multiple optimal initial guesses. We show that this method could get a better trajectory with less movement and more stability during the whole motion process. Another important advantage of our approach is the ability to find diverse styles of actions for a certain task with the multiple initial guesses and optimal solutions, thus we could generate different 9

12 (a) Stand (b) Pre-Grasp (c) Grasp (d) Up-Lift (e) Throw (f) Stand Back Figure 3: Whole process of the grasping action trajectories for different purposes. Our approach could effectively avoid local minimum and find diverse workable motion styles in a less constraint working environment. However, this method suffers a disadvantage that if the working area is small, we could hardly find a workable solution for any key posture with finite initial guess. For example, in DRC task 1, Atlas robot needs to get its huge body out of the small car, which leaves us a very narrow working space. In that case, very few or even none of the multiple solutions from our multiple initial guesses will work. As a result, we need to combine the differential Inverse Kinematic method into our approach that could effectively connect to previous working posture. Future research will focus on developing an advanced key posture generator that could automatically define the key poses given a certain task and working environment. The basic framework of using multiple initial guess for diverse optimal trajectory could also be extended to dynamic trajectory optimization, thus increase the speed of dynamic online optimization. 10

13 References [1] M. Al Borno, M. De Lasa, and A. Hertzmann, Trajectory optimization for full-body movements with complex contacts, Visualization and Computer Graphics, IEEE Transactions on, vol. 19, no. 8, pp , Aug [2] S. Lengagne, P. Mathieu, A. Kheddar, and E. Yoshida, Generation of dynamic motions under continuous constraints: Efficient computation using b-splines and taylor polynomials, in Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on, Oct 2010, pp [3] I. Mordatch, E. Todorov, and Z. Popović, Discovery of complex behaviors through contact-invariant optimization, ACM Transactions on Graphics (TOG), vol. 31, no. 4, p. 43, [4] J. J. Craig, Introduction to robotics: mechanics and control. Pearson/Prentice Hall Upper Saddle River, NJ, USA:, [5] A. D Souza, S. Vijayakumar, and S. Schaal, Learning inverse kinematics, in Intelligent Robots and Systems, Proceedings IEEE/RSJ International Conference on, vol. 1, 2001, pp vol.1. [6] G. Tevatia and S. Schaal, Inverse kinematics for humanoid robots, in Robotics and Automation, Proceedings. ICRA 00. IEEE International Conference on, vol. 1, 2000, pp vol.1. [7] C. Chevallereau and W. Khalil, A new method for the solution of the inverse kinematics of redundant robots, in Robotics and Automation, Proceedings., 1988 IEEE International Conference on, Apr 1988, pp vol.1. 11

14 [8] C. Niehaus, T. Röfer, and T. Laue, Gait optimization on a humanoid robot using particle swarm optimization, in Proceedings of the Second Workshop on Humanoid Soccer Robots in conjunction with the, [9] W. Suleiman, E. Yoshida, J.-P. Laumond, and A. Monin, On humanoid motion optimization, in Humanoid Robots, th IEEE-RAS International Conference on, Nov 2007, pp [10] T. Mandersloot, M. Wisse, and C. Atkeson, Controlling velocity in bipedal walking: A dynamic programming approach, in Humanoid Robots, th IEEE-RAS International Conference on, Dec 2006, pp [11] M. Do, P. Azad, T. Asfour, and R. Dillmann, Imitation of human motion on a humanoid robot using non-linear optimization, in Humanoid Robots, Humanoids th IEEE-RAS International Conference on, Dec 2008, pp [12] N. Pollard, J. Hodgins, M. Riley, and C. Atkeson, Adapting human motion for the control of a humanoid robot, in Robotics and Automation, Proceedings. ICRA 02. IEEE International Conference on, vol. 2, 2002, pp vol.2. [13] A. Safonova, N. Pollard, and J. K. Hodgins, Optimizing human motion for the control of a humanoid robot, Proc. Applied Mathematics and Applications of Mathematics, vol. 78, [14] K. W. Sok, M. Kim, and J. Lee, Simulating biped behaviors from human motion data, in ACM Transactions on Graphics (TOG), vol. 26, no. 3. ACM, 2007, p [15] Y. Lee, S. Kim, and J. Lee, Data-driven biped control, ACM Transactions on Graphics (TOG), vol. 29, no. 4, p. 129,

15 [16] M. Da Silva, Y. Abe, and J. Popović, Simulation of human motion data using shorthorizon model-predictive control, in Computer Graphics Forum, vol. 27, no. 2. Wiley Online Library, 2008, pp [17] T. Asfour and R. Dillmann, Human-like motion of a humanoid robot arm based on a closed-form solution of the inverse kinematics problem, in Intelligent Robots and Systems, (IROS 2003). Proceedings IEEE/RSJ International Conference on, vol. 2, Oct 2003, pp vol.2. [18] A. Ude, C. Man, M. Riley, and C. G. Atkeson, Automatic generation of kinematic models for the conversion of human motion capture data into humanoid robot motion, [19] A. Ude, C. G. Atkeson, and M. Riley, Programming full-body movements for humanoid robots by observation, Robotics and Autonomous Systems, vol. 47, no. 2, pp , [20] K. Grochow, S. L. Martin, A. Hertzmann, and Z. Popović, Style-based inverse kinematics, ACM Trans. Graph., vol. 23, no. 3, pp , Aug [Online]. Available: [21] C. Dellin and S. Srinivasa, A framework for extreme locomotion planning, in Robotics and Automation (ICRA), 2012 IEEE International Conference on, May 2012, pp [22] C. Liu and C. Atkeson, Standing balance control using a trajectory library, in Intelligent Robots and Systems, IROS IEEE/RSJ International Conference on, Oct 2009, pp

16 [23] O. Ramos, N. Mansard, O. Stasse, and P. Souères, An advanced robotics motion generation framework for inferring the organisation of human movements, Computer methods in biomechanics and biomedical engineering, vol. 16, no. sup1, pp , [24] S. Agrawal, S. Shen, and M. van de Panne, Diverse motion variations for physics-based character animation, in Proceedings of the 12th ACM SIGGRAPH/Eurographics Symposium on Computer Animation. ACM, 2013, pp [25] S. Agrawal and M. van de Panne, Pareto optimal control for natural and supernatural motions, in Proceedings of the Motion on Games. ACM, 2013, pp [26] J. Chai and J. K. Hodgins, Constraint-based motion optimization using a statistical dynamic model, ACM Transactions on Graphics (TOG), vol. 26, no. 3, p. 8, [27] A. Leeper, K. Hsiao, M. Ciocarlie, L. Takayama, and D. Gossow, Strategies for humanin-the-loop robotic grasping, in Human-Robot Interaction (HRI), th ACM/IEEE International Conference on, March 2012, pp [28] J. Morimoto and K. Doya, Acquisition of stand-up behavior by a real robot using hierarchical reinforcement learning, Robotics and Autonomous Systems, vol. 36, no. 1, pp , [29] J. Peters, S. Vijayakumar, and S. Schaal, Reinforcement learning for humanoid robotics, in Proceedings of the third IEEE-RAS international conference on humanoid robots, 2003, pp [30] T. Hester, M. Quinlan, and P. Stone, Generalized model learning for reinforcement learning on a humanoid robot, in Robotics and Automation (ICRA), 2010 IEEE International Conference on, May 2010, pp

17 [31] P. Gill, W. Murray, and M. Saunders, SNOPT: an SQP Algorithm for Large-Scale Constrained Optimization, SIAM journal on optimization, vol. 12, no. 4, pp ,