Add checking if joint is in chain

[joaomoura24]

Description

Describe the additions made in this pull request.

Todo

• Completed implementation of new feature.
• Add documentation for the new feature.
• Ensure all unit tests pass.
• Address all issues raised by unit-tests.
• Added code reviewer.

[cmower]

I didn't check too much the code, but all the unit-tests are passing so the Jacobian should be computed correctly - perhaps we can add a unit test for dual arm (using the nextage) in a separate PR. If this solves @duISIR's issue then I'm happy for @joaomoura24 to merge directly.

[joaomoura24]

Ok, then I will go ahead and merge.
I'll add an issue for adding unit test for dual arm in case we want to start using more serious work on it, but not for the next month.