Spherical joint implementation

[PaulSchreiner]

It took a while but here is a working implementation of a spherical joint.

I have created (copy/pasted) a new example called spherical_joint_test, based on the pendulum_example_gui which works for both the Tiny and Eigen algebra's. I have not implemented it for any of the other algebra's.

The bug in the previous PR turned out to be an issue with an errornous transpose in mul_transpose() for matrix6x3, now it runs pretty stable. I could get it to explode but that was when I run a simulation with 50 undamped spherical joints after a good few seconds of simulation. To be sure that this is not related to the spherical joints I checked with the pendulum example and the same happens if I chain 50 undamped hinges.

I have made an implementation for the mass_matrix calculation too, but I am not 100% sure that it is correct and I haven't got time to do extensive tests at the moment.

This is what I have changed:

• I chose to implement the spherical joint using quaternions for numerical stability, avoiding to have time dependent joints as well as consistency with floating base.

• For the multi dimensional case of the joint subspace S, I created a tiny_matrix6x3 struct now, I am still considering to use vectors of motion vectors instead so it can be generalized for any dimension.

• Added spherical_joint_test.cpp and pendulum_spherical_joints.hpp

• CMakeLists.txt

  • added target for spherical_joint_test.cpp

• forward_dynamics.hpp:

  • Added switches for spherical joints

• integrator.hpp

  • added switch for spherical joints, implementing integration in the same way as the floating base is done

• kinematics.hpp

  • Changed type q and qd to VectorX

• mass_matrix.hpp:

  • Added switch for spherical joints, is not updating for parents yet!

• link.hpp:

  • added a case to set_joint_type()
  • added 3 dimensional versions of the temp variables (U, D and u) as well as invD to avoid redundant recalculation of the matrix inverse
  • created VectorX versions of Link::jcalc() and added cases for the spherical joint

• inertia.hpp:

  • Overloaded operator* for matrix6x3
  • Created a mul_transpose method for transpose multiplications of 2 matrix6x3 to form a 6x6 matrix in abi format.

• spatial_vector.hpp:

  • Overloaded some necessary operators to be able to multiply between matrix6x3 and motion/force vectors

• tiny_algebra.hpp:

  • added methods to multiply matrix6x3 with vector3, returning motion or force vectors
  • added quaternion to angle and quaternion to axis-angle methods

• tiny_matrix6x3.hpp:

  • implemented a 6x3 matrix struct in a similar way as how the 6x6 matrix is implemented
  • I did not implement a matrix3x6 object, instead I added an "m_transposed" member. Calling the Matrix6x3::transpose() method returns a copy of the object with the m_transpose flag set to true. The Matrix6x3 operator* is overloaded for the case that left and right are Matrix6x3, it will assert that the left hand side has m_transposed==true and then treats the multiplication accordingly. The advantage is that we don't need a Matrix3x6 object, the disadvantage is that it doesn't generalize for different matrix sizes.

• multi_body.hpp:

  • Created a shperical joint counter member + getter method, to account for the discrepancy between DoF's and size of q
  • Added switches to initialize() and attach() methods
  • Converted getters for joint-coordinates, -velocities, -torques and -accelerations to return VectorX's with size depending on the joint type

• eigen_algebra.hpp:

  • Implemented all necessary methods

• Other:

  • Change all member function usage to Algebra:: functions.

[erwincoumans]

Ah, very nice, thanks a lot! I didn't get a notification, need to set it up. Will check the PR soon, I was busy getting the CUDA laikago example wrapped up (see https://twitter.com/erwincoumans/status/1340056288902795266)

[PaulSchreiner]

That looks great with the CUDA suport, I am looking forward to trying that out myself!

[erwincoumans]

close-reopen to retrigger CI

[PaulSchreiner]

Hi Erwin, is this still active?

[erwincoumans]

Yes, will get back to this soon after the holidays. Happy new year Paul!

[PaulSchreiner]

Great, happy new year to you too! Enjoy the rest of your holidays.

[erwincoumans]

Hi Paul,

I had a quick look at the spherical joint test 1) Eigen version seems to work, Tiny version doesn't. See video. 2) the laikago_opengl_eigen_example (and a few others) are broken, in euler_integrate, the qdindex of some link is -2. You can easily reproduce it in 'Debug' mode using the free Visual Studio 2019 on Windows.

Can you check it? I'll also have a look. Thanks! Erwin

[erwincoumans]

Let's merge for now, it requires some small fixes in integrator.hpp to solve (2), will look into (1) later. Thanks for the contribution!

[PaulSchreiner]

Hi Erwin, I can see you fixed (2), I must have missed that if statement.

I am trying to run the spherical pendulum example from the current master but it fails telling me it can't find pendulum5spherical.urdf but I could reproduce the issue in my fork. I am guessing it is an issue within TinyAlgebra, maybe some of the functions I implemented there are not numerically stable.

I will have a look at it over the next few days but I am a bit bussy with a deadline the end of the month so I might be a bit slow.

Cheers, Paul.

[erwincoumans]

Yes, I just added the missing URDF file. In general, it is easier to create a URDF file instead of programmatic generating figures. The Tiny issue is likely a small bug somewhere, we'll figure it out (easy to compare side-by-side using Eigen)