[exotica_core] Implement velocity & acceleration joint limit exposure

[the-raspberry-pi-guy]

This PR addresses the fact that there is no way to get velocity/acceleration limits from a URDF using EXOTica. The edits here add this functionality. In detail, the changes I have made are:

• kinematic_tree.h
  • Add an enum JointLimitType for use in the joint_limits vector. Easier to remember and read better.
  • Moved the constexpr for inf and pi to the start of this file, and in global scope. Previously they were defined locally inside ResetJointLimits in kinematic_tree.cpp - they are useful outside of this function.
  • Add function headers for the SetJointLimitsVelocity, SetJointLimitsAcceleration and overloaded SetFloatingBaseLimitsPosXYZEulerZYX and SetPlanarBaseLimitsPosXYEulerZ
• kinematic_tree.cpp
  • Changed the joint_limits vector to have dimension=4 to store upper, lower, velocity, acceleration limits
  • Added setters/getters for joint limit velocity/acceleration
  • Overloaded SetFloatingBaseLimitsPosXYZEulerZYX and SetPlanarBaseLimitsPosXYEulerZ to also have the option to specify velocity and acceleration limits (without breaking old API). Default the 2 arg version to set velocity and acceleration to inf.
  • In ResetJointLimits, set the acceleration limit to inf as workaround as Vlad suggested due to the issue I uncovered in ros_control: it doesn't parse acceleration limits from the URDF by default for some reason, described here.
• Pyexotica
  • Add bindings for these additions, including overloaded bindings (done by following this example)

[wxmerkt]

I am not sure that we should extend the joint position limit matrix. I would instead add extra vectors for velocity limits and acceleration limits. What do you think @VladimirIvan?

[the-raspberry-pi-guy]

I am not sure that we should extend the joint position limit matrix. I would instead add extra vectors for velocity limits and acceleration limits. What do you think @VladimirIvan?

That was Vlad's decision, but can be amended.

[wxmerkt]

Once we update our KinematicTree to quaternions, we will have different sizes of configuration and tangent vectors. It's also not standard in the community and other software to mix position, velocity, and acceleration limits. In a model we want to keep track of (a) configuration (position) limits, (b) velocity limits, (c) acceleration limits [if defined, not always!], and (d) effort limits.

[the-raspberry-pi-guy]

Once we update our KinematicTree to quaternions, we will have different sizes of configuration and tangent vectors. It's also not standard in the community and other software to mix position, velocity, and acceleration limits. In a model we want to keep track of (a) configuration (position) limits, (b) velocity limits, (c) acceleration limits [if defined, not always!], and (d) effort limits.

This makes sense to me though will see @VladimirIvan thoughts to why I implemented it this way.

[the-raspberry-pi-guy]

So what exactly is the consensus on this? Have a new vector with velocity and acceleration limits? Or 2 new vectors? Or keep in the same vector? I won't instantiate acceleration limits by default - so does that mean having them as inf still?

Consensus saves my computer struggling through extra builds ;)

Also, I notice that my PR failed some integration checks - what does this mean exactly? Is this a Kinetic issue? I am developing on Melodic and so unsure how to fix.

[wxmerkt]

1. Add a new vector for each effort and acceleration limit
2. You can populate the effort limit vector with information from the URDF. Acceleration limits should be inf by default?
3. Add a boolean flag "has_acceleration_limits" and get_has_acceleration_limits, get_acceleration_limits(), set_acceleration_limits(a_max)
4. Expose effort limits with get_effort_limits
5. You can check why the build server failed by clicking on "Details" next to Travis. It is most likely either build or formatting. In your case it's formatting: https://travis-ci.org/github/ipab-slmc/exotica/builds/686184166 - check the documentation for how to apply formatting. Short answer: Install clang-format-3.9 and run the "apply-format" script in the root of the repository.
6. Try ccache to save yourself some compilation time. Even though changing KinematicTree will force a recompile of everything...

[the-raspberry-pi-guy]

In the latest commits, I have made the requested changes. Velocity and acceleration limits are now in separate vectors, acceleration defaults to inf, and has_acceleration_limits defaults to false (and changes to true when they are set using the setter).

ccache was helpful in speeding up my compile times (whole build of EXOTica is 20-30 mins on my PC).

For the formatting @wxmerkt, do you mean run the command given in the PR window: find -name '*.cpp' -o -name '*.h' -o -name '*.hpp' | xargs clang-format-3.9 -style=file -i. When I run this it runs without error and returns nothing? Or are you referring to something else?

[the-raspberry-pi-guy]

Implemented all of the changes - that should be everything: name changes, checking velocity/acceleration sizes, setting to inf/throwing errors if incorrect, checking for bounds before joint limit reset etc.

Also ran the apply_format script again so this should pass all formatting integration tests.

[the-raspberry-pi-guy]

That should be everything

[the-raspberry-pi-guy]

Removed default values and == true

[wxmerkt]

Here is an overview of what got changed by this pull request:

Clones added
============
- exotica_core/src/kinematic_tree.cpp  2

See the complete overview on Codacy