Added convenience functions to help with setting FreeJoint, EulerJoint, and BallJoint positions

[mxgrey]

I found that setting FreeJoint and other joint types that have rotations can be confusing or can require more lines of code than it ought to, so I made some functions to help users convert transform and rotation types into joint position vectors that they can pass into Joint::setPositions(VectorXd).

[jslee02]

Why this function is made as a template function instead of taking Eigen::Matrix3d?

[mxgrey]

It's templated because Eigen uses a variety of ways to express rotations, and using a templated argument ensures that all of them can be covered by one function. I modelled it after the Isometry3d::rotate(~) function.

It would probably still work if it used a Matrix3d argument, since Eigen is pretty smart about converting its types, but the template usage here doesn't really hurt anything, because you don't need to use the template explicitly. If you look at an example of usage you'll see that C++ can infer the template argument so you don't even need to treat it like a templated function.

[jslee02]

Eigen uses a variety of ways to express rotations

Do you mean like Quaternion3d? Is there another types that express rotations in Eigen?

[mxgrey]

Right, Quaternion3d is an example. AngleAxisd is another one. I'm not sure if there are more than those three.

[jslee02]

I see. It seems all the ration expressions can be converted to Eigen::Matrix3d so that it works with math::logMap(Eigen::Matrix3d).

[jslee02]

How about to create inverse conversion functions like FreeJoint::convertToTransfom(~) and BallJoint::convertToRotation(~), and use all the conversion functions inside the joint classes for consistency.

For example,

void FreeJoint::integratePositions(double _dt)
{
  mQ.linear()      = mQ.linear() * math::expMapRot(mJacobian.topRows<3>()
                                                   * mVelocities * _dt);
  mQ.translation() = mQ.translation() + mVelocities.tail<3>() * _dt;

  //mPositions.head<3>() = math::logMap(mQ.linear());
  //mPositions.tail<3>() = mQ.translation();
  mPosition = convertToTransfom(mQ);
}

[mxgrey]

Making inverse versions and using them all inside the update functions makes perfect sense.

[mxgrey]

I've added a convertToTransform() function for the three joint types, as well as a convertToRotation() function for BallJoint and EulerJoint. They're also being used in the update functions for those joints.

[jslee02]

I've been using Eigen::Isometry3d::linear() to get the rotation part of a transformation. Do you know what's the difference between linear() and rotation()?

[mxgrey]

Good question; I haven't given it much thought, I just use rotation() out of habit.

Glancing through the Eigen headers, it looks like the difference is that linear() just returns a reference to the rotational part of the Isometry3d, whereas rotation() enforces orthonormality before returning a copy of the linear part (so it's not a reference).

It sounds like it's a trade off between efficiency -- where linear() is the winner -- versus numerical accuracy -- where rotation() is the winner. Also, if you want to set the linear part directly, you have to use linear().

It might be better to use linear() here since it would be more efficient, and I suspect it's reasonable to expect that the incoming Isometry3d has an okay rotational part.

[jslee02]

Let's use linear() consistently, and let's see if the orthogonality does matter.

[jslee02]

Changed rotation() to linear(). As I mentioned, let's use linear() consistently, and let's see if the orthogonality does matter.

If the build tests are happy, then +1.