Combine GlobalInverseKinematics with nonlinear IK.

[hongkai-dai]

We consider to combine GlobalInverseKinematics (which solves a mixed-integer program) with nonlinear IK in two ways

1. We can solve the nonlinear IK to find an initial guess of q, and then compute the binary variables in global IK corresponding to this q. We can then solve global IK starting from this binary variable values.
2. When we call GlobalInverseKinematics to solve IK as a mixed-integer optimization problem through branch-and-bound (bnb), at each node of the bnb tree, we could take the current candidate mixed-integer solution and solve a nonlinear IK to find the robot posture. The idea is that at each node of the bnb tree, we have the value of the link orientation/position. Although they don't satisfy the SO(3) constraint yet, we could still project them to SO(3), and then reconstruct a robot posture that matches with these projected SO(3) pose as close as possible. We can take this reconstructed posture as the initial guess of IK and solve a nonlinear optimization problem.

[hongkai-dai]

For approach 1, one question is how to compute the binary variable values from q. We have two ideas

1. Compute the pose from that q, then in GlobalInverseKinematics, add constraints to lock each rotation/position variable to that pose, and then solve the global IK to find the binary variables. I think solving this problem should be quick, since each rotation matrix uniquely determines the binary variable values.
2. Given the rotation matrix values, compute the binary variable values in the closed form, by looking at how we implement the McCormick envelope constraint. I think this computation would be even faster than idea 1, but might take more efforts to implement? Especially given we have many different ways to relax the SO(3) to mixed-integer constraints (with McCormick envelope, with linear or logarithmic number of binary variables, etc).

[RussTedrake]

I'll report the most interesting results of my experiments here using homecart (bimanual) as a benchmark.

I implemented a SetInitialGuess(q) which calls gurobi with constraints assigning the translation and rotation variables to solve for the integer variables, and then set the solution of that program as an initial guess for the normal problem. For a global IK that has only a posture cost (+ joint limits from the constructor) with the default options, on this machine the solve times are:

• without SetInitialGuess: mean 47s.
• with SetInitialGuess (including the SetInitialGuess time): mean 7s.

That's pretty encouraging. Note that no nonlinear IK program was required for this particular experiment.

The gurobi solve inside SetInitialGuess takes ~ 0.18 seconds.