Strategies for Dividing q_u and q_a.

[hjsuh94]

Related / done simultaneously with #27

• [ ] Teleport Grasp, Choose Goal, then Trajopt.

  1. Teleport to a grasping configuration with a good B matrix.
  2. Choose a "subsubgoal" that heads towards the subgoal with some step size.
  3. Solve trajopt to go to the subsubgoal.

• [ ] Teleport Grasp, Choose Goal, then Rollout.

  1. Teleport to a grasping configuration with a good B matrix.
  2. Choose a "subsubgoal" that heads towards the subgoal with some step size.
  3. Rollout towards that subgoal with some horizon.

• [x] Choose Goal, Choose Grasp, then Trajopt.

  1. Choose a "subsubgoal" that heads towards the subgoal with some step size.
  2. Choose a grasping configuration on the "subsubgoal".
  3. Solve trajopt to go to the "subsubgoal" with specified grasping configuration.

[hjsuh94]

As we discussed with @pangtao22 and @lujieyang today, a lot of samples currently seem to be wasted on configurations which do not make contact, and therefore have quite degenerate ellipses.

Biasing our samples towards contacting configurations may not necessarily help because the finger really needs to make and break contact through multiple trees in order to successfully rotate the tree.

To address this problem, it now seems inevitable to use some kind of more long-horizon information on the contact configurations. We can either do this with:

1. Finger teleporting
2. Use the full power of trajopt to get to a better configuration.

which is consistent with our options above.

[hjsuh94]

The third strategy doesn't seem to work too well without investing a lot into engineering issues.

1. It takes an ungodly amount of 20 minutes.
2. Under low weight on Qa, trajopt does not seem to reach the correct grasp configuration.
3. Under high weight on Qa, trajopt tries to push the ball outside the reach.

It would seem that we would need a separate motion planner to handle teleporting....