google-cla[bot]
commented
3 years ago Thanks for your pull request. It looks like this may be your first contribution to a Google open source project (if not, look below for help). Before we can look at your pull request, you'll need to sign a Contributor License Agreement (CLA).
:memo: Please visit https://cla.developers.google.com/ to sign.
Once you've signed (or fixed any issues), please reply here with @googlebot I signed it! and we'll verify it.
What to do if you already signed the CLA
Individual signers
- It's possible we don't have your GitHub username or you're using a different email address on your commit. Check your existing CLA data and verify that your email is set on your git commits.
Corporate signers
- Your company has a Point of Contact who decides which employees are authorized to participate. Ask your POC to be added to the group of authorized contributors. If you don't know who your Point of Contact is, direct the Google project maintainer to go/cla#troubleshoot (Public version).
- The email used to register you as an authorized contributor must be the email used for the Git commit. Check your existing CLA data and verify that your email is set on your git commits.
- The email used to register you as an authorized contributor must also be attached to your GitHub account.
ℹ️ Googlers: Go here for more info.
PaulSchreiner
erwincoumans
Here is the first version of the spherical joint implementation.
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. This would entail creating a struct for S as is done in RBDL.
CMakeLists.txt
Added spherical_joint_test.cpp and pendulum_spherical_joints.hpp
forward_dynamics.hpp:
integrator.hpp
kinematics.hpp
mass_matrix.hpp:
link.hpp:
inertia.hpp:
spatial_vector.hpp:
tiny_algebra.hpp:
tiny_matrix6x3.hpp:
multi_body.hpp:
Like I wrote in my mail, there seems to be an issue with the implementation still. The simulation gets unstable for certain initial conditions or when there is more than a few links attached, I am not sure the two are related.
The initial condition case: I have been looking at what the bug can be and at first I thought it might be an issue with the inertia matrix but I couldn't find an error in the implemented logic. Then I thought it might have to do with how I initialized the joint. I initialized it exactly the same way as in the pendulum experiment: Initializing the joints with the center of mass at a pure y-axis position. Then having the initial quaternions as q0=[0,0,0,1] the simulation runs as expected. However, setting q0=[0,0,sqrt(1/2), sqrt(1/2)] renders it unstable. However, when I initialize it as having its CoM in the pure (-) z direction I can initialize using q0=[sqrt(1/2), 0, 0, sqrt(1/2)] and q0=[0,sqrt(1/2),0, sqrt(1/2)] and the simulation runs fine in both cases.
Still when initializing with more than ~7/8 spherical joints chained together renders the simulation unstable very quickly, I am wondering if this could be an integrator issue, i.e. that euler steps are not enough?
I will have a look at how RBDL does this for spherical joints.