[WIP] Create new AIKIDO IK solver class.

[sniyaz]

Background: We want to start bringing new IK solvers (like ikFast) into our stack. Ideally each Robot object would have some associated IK object that we can swap out easily. The key issue right now is that the DART IK solver API is a little confusing and doesn't handle analytic solvers that well (it assumes that everything is a gradient-based solver).

Proposed Solution: Make an AIKIDO-specific IK solver class that we use in place of the DART one. This PR takes a first stab at this, and demonstrates how to make an optimization-based solver. This really just hides a lot of the complexity of the DART solver, and is a lot easier to work with.

Remaining Issues: This is a draft PR, and there's a lot to sort out. In no particular order:

1. What should the RV of IK be? Right now I'm just using Eigen::VectorXd because I don't think we should use allocated states, but this kind of clashes with the rest of AIKIDO.

2. Where should this be? Right now it's just alongside the other Robot stuff.

3. Naming conventions for sub-classes. Also, calling the base class InverseKinematics is a bit confusing since that's what DART does as well.

---

Before creating a pull request

• [x] Document new methods and classes
• [x] Format code with make format

Before merging a pull request

• [ ] Set version target by selecting a milestone on the right side
• [ ] Summarize this change in CHANGELOG.md
• [ ] Add unit test(s) for this change

[codecov[bot]]

Codecov Report

Merging #616 (d545d95) into master (72cc227) will not change coverage. The diff coverage is n/a.

:exclamation: Current head d545d95 differs from pull request most recent head ea44573. Consider uploading reports for the commit ea44573 to get more accurate results

@@           Coverage Diff           @@
##           master     #616   +/-   ##
=======================================
  Coverage   76.13%   76.13%           
=======================================
  Files         206      206           
  Lines        7231     7231           
=======================================
  Hits         5505     5505           
  Misses       1726     1726           

[amalnanavati]

In terms of the questions, here are my two cents:

1. I think Eigen::VectorXd makes sense, which is the same effective retval of DART's IK. Further, we are returning a vector of Eigen::VectorXd, and we don't know which solution the user would want to use. Hence, it doesn't make sense imo to preemptively allocate states for them – we should leave that to the user.
2. My initial leaning is to have a separate IK folder within the robot folder, but I see that you have a commit where you specifically undid that. What made you do that? Understanding the factors you considered there could help us decide where to put this code.
3. Do we need a naming convention for subclasses? I was thinking stuff like GradientDescentIK, IKFast, and whatever the other default names are for IK solvers.
4. I agree that having the same class name within AIKIDO and DART is confusing. Speaking as someone who was just familiarizing myself with AIKIDO a few months ago, I found that it took some time to determine which classes were in AIKIDO and which were in DART. With that said, it's not immediately obvious to me what a better name would be. RobotInverseKinematics? But afaik IK is only for robots... Perhaps InverseKinematicsSolver? (DART has an InverseKinematics class, and then has Solver classes and subclasses (e.g., GradientDescentSolver), but does not have an InverseKinematicsSolver. But then again, the fact that both InverseKinematics and Solver are classes within DART could also be confusing.)

[sniyaz]

@amalnanavati Responded to your nits, also some thoughts:

1. Agreed!
2. It made things feel a little cluttered if that makes sense- there aren't that many classes right now. But maybe when we add more our feelings on this will change?
3. Yeah, fair. Perhaps naming convention is over-rated.
4. Yeah I agree- maybe avoiding some degree of confusion is impossible here. I still think on balance this will be better since 1) we can give robots an actual IK method now and 2) we can at least switch between IK types in a consistent manner. (Hopefully this actually manages to hide most IK interaction from the user, which is actually my goal here- every time I've had to use one of the actual IK classes it's made me want to put my head through a wall)

[sniyaz]

Whoops, this shouldn't be WIP anymore huh?

[sniyaz]

Also I remember that @brianhou said he'd like an example of what an IkFast solver would look like with this API, so I included a first cut of that as well!

[amalnanavati]

Or well looks like CI failed, so we should fix that first. I'm happy to re-review then.