Humanoid Full-Body Manipulation Planning with Multiple Initial Guesses and Key Postures Bowei Tang, Tianyu Chen, and Christopher Atkeson Carnegie Mellon University, United States

Introduction Humanoids are redundant Solving inverse kinematics (IK) for different task phases separately results in incompatible answers Bad IK solutions was a big problem in the DARPA Robotics Challenge We introduce a trajectory optimization approach which finds compatible inverse kinematics solutions

Basic Idea We divide a single manipulation task into several phases For each phase, we use optimization to solve redundant inverse kinematics With different initial guesses, we get multiple local minima for inverse kinematics solutions for each phase We connect an optimal combination of IK solutions as a smooth trajectory The basic idea of our algorithm is: (As shown in the slide) Left picture shows local minima Right picture shows connecting solutions

Related Work From Robotics and Computer Animation Jacobian pseudo-inverse RRTs Extended Jacobian method Particle swarm optimization General non-linear optimization … Just show this slide and then move on, don’t try to say anything

Task Phases For a grasping action, there are four task phases: Prepare Reach Lift-up Drop Away For a grasping action, as shown in the picture. We could manually define four phases: prepare, reach, lift-up and drop-away.

Finding multiple inverse kinematics solutions Distribute initial guesses for each IK optimization To optimize an IK solution, we use the cost function L Reach target Stay balanced Minimize torques Obey joint angle and torque limits Reward diverse solutions Use SNOPT to solve optimization problem with the given initial guess igi For each key posture, we use optimization to solve the Inverse Kinematics. With a diverse set of initial guesses, we could find multiple different optimal solutions. We use this cost function to optimize end-effector positions, COM positions and joint torques. Then we use SNOPT to solve the optimization problem with each initial guess.

Generate Trajectory Minimize distance between IK solutions for each phase With the multiple initial guesses, for each key postures we could get multiple solutions as shown in the figure. Each bubble represent a solution. We then connect those bubble in sequence and get those solution series. By evaluation those solution series, we could find an optimal one. Then we could connect each bubble of that series into a smooth trajectory.

2D Example Here is a very simple 2D example with a two DOFs arm. For each key posture, we can get 2 different solutions, thus we could combine 8 solution series in total. Then given a initial pose, we could optimize an optimal series with the less angular movement.

Previous Approach: Wild IK solutions Atlas Example in Gazebo simulation Here is the experiment of humanoid Atlas in Gazebo simulation. With the same grasping action, we can find a lot of different IK solutions with different initial guesses.

Compatible IK Solutions Atlas Example in Gazebo simulation By carefully choose the optimal combination of the solutions, we can connect a smooth trajectory with less angular movements and COM movement.

SHOW VIDEO

Our approach finds shorter trajectories with less COM movement and less cost in terms of our Optimization criterion.

Discussion Reduce computation cost Find diverse styles of actions for a certain task Not useful if the working space is small Task phases need to be manually defined The current implementation works well for quasi-static behaviors A library of IK solutions may improve performance Here is the discussion. With our algorithm, we could reduce computation cost. The time use is shown in our paper. We could also find diverse styles of actions give the same task, thus we can have multiple choices with different requirements. However, since we need a manual definition of the phases, if the working space is small or the key postures are hard to define, our algorithm could not guaranty the solution. Other than those situation, the current implementation works well for quasi-static behaviors. We will continue exploring a posture library method to further improve the preferment of the algorithm.

Questions?