Fix #142

[cmower]

Description

Fix #142.

Todo

• Completed implementation of new feature.
• Add unit-test for the new feature.
• Ensure all unit tests pass.
• Address all issues raised by unit-tests.
• Consider adding an example that demonstrates the use of the new feature.
• Added code reviewer.

[cmower]

@joaomoura24, I think this makes sense to just test a dual arm robot for FK against robotics toolbox for the same model (i.e. nextage).

Please let me know if you think we should add any other tests?

Otherwise, good to merge and close #142?

[joaomoura24]

Yeah, that 's ok - and to be honest I haven't found any issue so far with dual arm. I was just mentioning the lack of testing.
Ready to merge once you find fit.