Fixing Inverse Kinematics for 6.0

[mxgrey]

This PR fixes some issues with Inverse Kinematics which pertain to FreeJoint, BallJoint, and EndEffector. It is targeted to be merged after #659.

There are two primary issues which were identified and fixed in this PR:

(1) Because FreeJoint and BallJoint positions cannot be integrated by simple addition, the Inverse Kinematics for these joints behaved incorrectly when they were not at zero. This PR fixes it by taking advantage of Joint::integratePositions(double). In the future, we might want a position integration which is agnostic to the current state. E.g:

Eigen::VectorXd Joint::integratePositions(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities) const

(2) When EndEffectors and ShapeNodes were being attached to their BodyNodes, they were not recognized as being JacobianNodes, because downcasting a pointer during construction is generally a bad idea. This PR fixes the issue, but now child BodyNodes are identified as child JacobianNodes via a separate pipeline than any other Node type, which is kind of smelly. In the future, we should probably unify the whole construction pipeline for BodyNodes and all other Node types. BodyNodes might be kind of special, but I think we can still work out a better process than what we have right now.

I'm now going to attempt to patch these issues for 5.0 and 5.1 without affecting their ABIs. Wish me luck.

---

This change is

[jslee02]

Reviewed 4 of 9 files at r1.
Review status: 4 of 9 files reviewed at latest revision, 11 unresolved discussions.

---

dart/dynamics/BodyNode.cpp, line 1443 [r2] (raw file):
This has nothing to do with the main purpose of this PR, but it would be good to use std::find instead of just find.

---

dart/dynamics/BodyNode.cpp, line 1463 [r2] (raw file):
ditto: std::find

---

dart/dynamics/BodyNode.cpp, line 1472 [r2] (raw file):
ditto: std::find

---

dart/dynamics/InverseKinematics.cpp, line 98 [r2] (raw file):
It seems this is a stop gap solution as we don't have a function of position integration which is agnostic to the current state you mentioned. Could we create an issue for this and leave a note here with the issue number?

---

dart/dynamics/InverseKinematics.cpp, line 100 [r2] (raw file):
Since #681, we should use std::size_t rather than size_t for consistency. auto i = 0u would be an alternative if you like.

---

dart/dynamics/InverseKinematics.cpp, line 717 [r2] (raw file):
ditto: std::size_t

---

dart/dynamics/InverseKinematics.cpp, line 721 [r2] (raw file):
ditto: std::size_t

---

dart/dynamics/InverseKinematics.cpp, line 727 [r2] (raw file):
ditto: std::size_t

---

dart/dynamics/InverseKinematics.cpp, line 728 [r2] (raw file):
ditto: std::size_t

---

dart/dynamics/InverseKinematics.cpp, line 1052 [r2] (raw file):
ditto: std::size_t

---

dart/dynamics/InverseKinematics.h, line 511 [r2] (raw file):
ditto: std::size_t

---

Comments from Reviewable

[jslee02]

Reviewed 4 of 9 files at r1, 1 of 1 files at r2.
Review status: all files reviewed at latest revision, 19 unresolved discussions.

---

a discussion (no related file):
This is not about the IK fix, but I've wondered if we can move out the nested classes in InverseKinematics.h outside of it for better readability. For example:

class InverseKinematics : public common::Subject
{
  [...]

  class Function;

  [...]
};

class InverseKinematics::Function
{
  [...]
};

---

dart/constraint/BalanceConstraint.cpp, line 414 [r2] (raw file):
ditto: std::size_t

---

dart/constraint/BalanceConstraint.cpp, line 418 [r2] (raw file):
ditto: std::size_t

---

dart/dynamics/InverseKinematics.cpp, line 1042 [r2] (raw file):
ditto: std::size_t

---

dart/dynamics/InverseKinematics.h, line 676 [r2] (raw file):
Could we rename this that doesn't have the suffix _t something like ExtraDofUtilizationMethod to follow the convention?

---

unittests/testForwardKinematics.cpp, line 222 [r2] (raw file):
Maybe this test is not for #499.

---

unittests/testForwardKinematics.cpp, line 236 [r2] (raw file):
ditto: std::size_t

---

unittests/testForwardKinematics.cpp, line 237 [r2] (raw file):
ditto: std::size_t

---

Comments from Reviewable

[jslee02]

Reviewed 4 of 9 files at r3, 5 of 5 files at r4.
Review status: all files reviewed at latest revision, 4 unresolved discussions.

---

Comments from Reviewable

[mxgrey]

Review status: all files reviewed at latest revision, 4 unresolved discussions.

---

unittests/testForwardKinematics.cpp, line 222 [r2] (raw file):
Woops, copy/paste error.

---

Comments from Reviewable

[jslee02]

---

Reviewed 3 of 3 files at r5.
Review status: all files reviewed at latest revision, all discussions resolved.

---

Comments from Reviewable