Support for continuous joints and fixed joints in URDF

[Cfather]

Hi, here's a simple add-up for support for continuous joints and fixed joints appearing in the URDF of some of the robot arms.

1. Continuous joints are just revolute joints that can rotate 360 degrees.
2. Fixed joints assume 0 position, velocity, and acceleration all the time, which might be useful to describe complex structures like grippers. An additional function called "remove_fixed_joints" has been added to remove fixed joints and merge bodies that are connected through fixed joints into one body. In "URDF_to_spatialv2_model", it is called by default.

I did not include sufficient tests for the fixed joint features yet. By modifying "jcalc", all functions should still work for a model with fixed joints technically. The robot states still need to include these fixed joints but the states will only be served as placeholders since they will not actually change the position, velocity, and acceleration of the fixed joints.

Please feel free to edit the code directly to match your original style. I'm happy to help with any additional related features. Thanks for your review in advance!

[pwensing]

Thanks, Bohao! Adding fixed joint support was something that I've had on the roadmap. So it's great to see the support in this PR.

Let's work together to finish this out. Do you have some example URDFs that you're looking to support that you might be able to share? If so, perhaps we can add some unit tests that compare some select computations (e.g., inverse dynamics and mass matrix) with the robot systems toolbox for verification.

Regarding the fixed joint, there is one other place that I think needs to be addressed. See the jinfo.m file that specifies the DoFs for the joint:

spatial_v2_extended/v3/dynamics/jinfo.m

Line 1 in e54050f

function [nq,nv] = jinfo( jtyp)

I'm expecting this could return 0 for both nv and nq for the fixed joint? And then the algos would pull out a 0x1 for the components of the configuration and its rates. With that, I think the following might not need to be the case:

The robot states still need to include these fixed joints but the states will only be served as placeholders since they will not actually change the position, velocity, and acceleration of the fixed joints.

[pwensing]

This is a great start! With an example system, a unit test (happy to help on that one), and resolution to the operation in remove_fixed_joints, I think this will be ready to merge in. Thanks for all your efforts to pull this together!

[pwensing]

👍

[pwensing]

Have you tried using the resulting model in any of the conventional algorithms? My suspicion is that leaving an "empty" body in the list will lead to trouble. For example, the empty XTree cannot be applied to anything, and the empty parent would cause the kinematics propagation to fail as well.

[pwensing]

This line seems to be assuming that you are removing the body from the list and decrementing the body labels of everyone else in the tree by one? However all the old model data (e.g., for I, Xtree, etc.) remains stored at the original index.

I might be mentally parsing this incorrectly -- if so, I think an example URDF will help a great deal to instill confidence.