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

2. 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

3. 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

4. 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

5. 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.

6. 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.

7. 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.

8. 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.

9. 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.

10. 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.

11. SHOW VIDEO

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

13. 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.

14. Questions?