Create a state-independent integration function for Joint types

[mxgrey]

Currently, we offer a function Joint::integratePositions(double timestep) which integrates the Joint's velocity state into the Joint's position state. This is useful for simulation, but for performing inverse kinematics we do not necessarily want to integrate the current velocity directly into the current position. Instead, we want to compute the change in joint positions resulting from some Jacobian-based gradient descent.

Therefore, it would be nice to also offer a function like

Eigen::VectorXd Joint::integratePositions(const Eigen::VectorXd& q0, const Eigen::VectorXd& v, double dt) const

Or to avoid allocations, maybe

void Joint::integratePositions(const Eigen::VectorXd& q0, const Eigen::VectorXd& v, double dt, Eigen::VectorXd& result) const

Or we could offer both and have the first one just call the second one.

[psigen]

@mxgrey: if you aren't already, you might want to take a look at the StateSpace API that @mkoval and the rest of the PRL put together for aikido. It is solving a superset of this problem, but this is one of the problems we were addressing:

https://github.com/personalrobotics/aikido/tree/master/aikido/src/statespace

@mkoval: is there a good place to see the high-level discussion (the PR discussion was a bit large). Or any words of advice from our experience?