Added initial position and initial velocity properties

[mxgrey]

There are now properties that can be given to the joints of a robot for initial position and initial velocity. Calling the resetPosition and resetVelocity functions will set the joint to its initial position and velocity respectively, instead of zeroing them out. By default, these properties are zero.

The SkelParser will set these properties based on the initial positions and velocities given in the .skel file. However, neither the URDF nor the SDF parsers support this, because I could not find a relevant tag in either of those formats.

[mxgrey]

I've incorporated the changes proposed in #438 to circumvent the issue of out-of-bounds initial positions for URDFs. We may want to consider something similar for SDF.

[mkoval]

👍 Looks good to me.

[jslee02]

I'd like to add the adjusting initial positions when it's out-of-bounds for SDF as well.

Also, how about printing warning messages when someone attempts to set the initial positions out-of-bounds?

[mxgrey]

Adjusting the initial position for SDF sounds good.

I'm not sure about warning messages, though. Having that would mean the order in which you set properties will affect whether warnings are printed. If someone adjusts their initial position before adjusting their joint limits, it would print a warning. I'm not completely against it, but I feel like it would be fine to just trust the user to keep their joint properties sane.

That being said, it might be interesting to have a sanityCheck function that would go through and check that lower limits are smaller than upper limits, and initial/current position values are within limits, and whatever else we can think of.

[jslee02]

Sanity check sounds good rather than warning messages.

As a start, we could make the function to return bool, then consider to change the function to return enum (or exception object) when we found that we want to handle insanity case differently depending on the error.

[mxgrey]

Should the sanity check also check whether the current position/velocity/acceleration/force are within limits, or should ignore the current state and only check the properties?

[mxgrey]

I've accounted for position limits in the SDF parser and added a sanity checker to Joint.

[jslee02]

Should the sanity check also check whether the current position/velocity/acceleration/force are within limits, or should ignore the current state and only check the properties?

We could consider to allow the user to choose testing items using mask bit but I'm not sure if it's necessary for now. So let's check the state (position and velocity) and consider extending the function when the use case is clear.

👍