Frame class and auto-updating for forward kinematics

[mxgrey]

This pull request includes two significant new features: a Frame subclass (as well as an Entity subclass) and smart automatic updating for forward kinematics.

Frame

The Frame class allows a greater amount of expression when defining and requesting data. For example, you can now request positions, velocities, and accelerations with request to an arbitrary reference frame by specifying a Frame class as an argument for various functions (as an example, see the getLinearVelocity() function).

Entity

The Entity class is simply a base class for anything that exists within a Frame. At the moment, that only includes Frames (because Frames exist within Frames), but in the future it can include things like a Point class (useful for expressing center of mass or collision points), an Axis class, a Transform class, and perhaps many other things.

Auto-Updating

The other -- and possibly more significant -- feature of this pull request is auto-updating for forward kinematics. From the user point of view, the impact of this feature is simply that there is no longer ever a need to call Skeleton::computeForwardKinematics(). More specifically, forward kinematics will always be computed when it's needed and will only perform the computations that are necessary for what the user requests. This works by performing some bookkeeping internally with dirty flags whenever a position, velocity, or acceleration gets set, either internally or by the user.

On one hand, this results in some overhead due to the extra effort of bookkeeping, but on the other hand we have much greater code safety, and in some cases performance can be dramatically improved with the auto-updating (for example, the computeForwardKinematics() function was inefficient when only one portion of the BodyNodes needed to be updated rather than the entire Skeleton, which is common when performing inverse kinematics on a limb or on some subsection of a Skeleton).

Speed Test

To determine whether the bookkeeping overhead is worth the auto-updating, I wrote the speedTest app (note, this is not a unit test because it can take a while to run). When comparing this branch with the grey/speed_test branch (which is the same as the current master, but with the speed test added), the difference in performance between the two branches was not appreciable. I ran the speedTest on each branch ten times in "ideal" conditions (I freshly restarted my PC and ran nothing but the terminal) and neither branch was consistently faster than the other. Ultimately, the worst performance of this auto-updating branch was less than 1% slower than the worst performance of the speed_test branch.

By default, the speedTest app is testing the speed of dynamic simulation, but it also includes a mode for testing the speed of pure forward kinematics. Comparing the results in that mode, I consistently found that the speed_test branch tends to have slightly better performance for computing all three (position, velocity, and acceleration); it seems to be a tossup when computing position and velocity (without computing acceleration); and then this auto-updating branch tends to win by a significant margin when computing only position (likely because the amount of bookkeeping overhead is minimized when only positions are being computed).

Caveats

It's important to note that the auto-updating comes with one important caveat for all DART developers now and in the future: there are certain data members that must not be modified or even accessed outside of the appropriate functions, even for internal use. Some of these include BodyNode::mWorldTransform, BodyNode::mVelocity, BodyNode::mAcceleration, and even others that might not be expected like Joint::mJacobian, Joint::mJacobianDeriv, BodyNode::mArtInertia. There are too many to list right here, but they should all be noted in the header comments. The reason these members should not be used directly is because they are carefully monitored by the auto-updating system, so anything that writes to them when it shouldn't will invalidate results, and anything that reads from them when it shouldn't might end up using outdated data.

This raises a question that I'd like to pose: Should we append something to the names of these "untouchable" data members similar to how the Hungarian Notation encodes information in variable names? Normally I would not advocate such a naming convention, but it might help future (and even current) developers if there were a clear way of immediately knowing that they should not use a variable directly. I don't really have any good suggestions for what the code should be, though. Maybe append "_dnu" (for "do not use") to the ends of the variable names?

TODO

Mark functions that are now deprecated (there are many…).

Final Comment

The changes that were necessary for the auto-updating to work fast and reliably were pretty exhaustive. Don't hesitate to ask if you find any changes peculiar, or if you're wondering why some were necessary. I had good reasons for each and every change, and I don't mind explaining those reasons.

[mxgrey]

The latest commit adds Jacobian functions that match the new velocity and acceleration functions. They allow the user to specify a frame of reference as well as offsets, and they clearly distinguish between spatial and classical vector terms.

The only issue is that Appveyor has been crashing when it attempts to build the latest changes for Win32. There have been no problems for Win64 or for any of the build tests on Travis-CI. Moreover, I have successfully built this branch in Win32 on two different Windows machines with no issues. This leads me to believe that Win32 compilation on Appveyor is simply broken (maybe it needs a system update?) and should therefore be ignored for the time being.

If anyone else has a Windows development environment set up, it would be very helpful if you could attempt to build this branch and see if it gives you any issues. The issue on Appveyor is that it attempts to compile BodyNode.cpp but just hangs there for about five minutes and then quits with an exit code of 1. It does not give any description of why it failed.

[jslee02]

The only issue is that Appveyor has been crashing when it attempts to build the latest changes for Win32. There have been no problems for Win64 or for any of the build tests on Travis-CI. Moreover, I have successfully built this branch in Win32 on two different Windows machines with no issues. This leads me to believe that Win32 compilation on Appveyor is simply broken (maybe it needs a system update?) and should therefore be ignored for the time being.

I did some tests on this issue and it looks resolved. I replaced all the int in BodyNode to size_t.

[mxgrey]

Wow! Out of curiosity, how did you know to do that? Is this an issue that's been encountered before, or did you have some way of debugging it that I'm unaware of?

[jslee02]

I just got a hint from here. It just a warning message about int and size_t, but it's from out side of our code and causes the error. So I thought it must have something to with BodyNode's int and size_t.

Actually, I first wonder that there is a way to get more detailed build log from AppVeyor but seems not possible (or my question is not clear).

[mxgrey]

Thanks for figuring this out! I had been ignoring that message because I couldn't imagine how a signed/unsigned conflict would cause Win32 to crash. I still don't understand how that's possible, but I'm certainly glad the issue is resolved.

[jslee02]

I also don't understand how but yeah it's resolved. :)

At some point, we need to be more restrict to the data types DART uses.

[mxgrey]

Agreed. I generally prefer using size_t since that's what gets used by STL, and it makes sense for ordered lists that start from 0 and go up. But we have the unfortunate conflict with Eigen which uses int, presumably so that it can have a -1 represent dynamically sized matrices.