Add gravity compensation to the openarms teleoperation

[michel-aractingi]

What this does

Adds gravity compensation with Pinocchio for the openarms leader teleoperator.

• Script examples/openarms/teleop_with_gravity_compensation.py
  • Teleoperates the follower left arm with the leader left arm.
  • Add gravity compensation to the leader's left arm to allow moving it easily.
• Script examples/openarms/gravity_compensation_one_arm.py
  • Example of gravity compensation of the follower's left arm.

The urdf used can be found in https://github.com/michel-aractingi/openarm_description/blob/main/openarm_bimanual_pybullet.urdf

TODO

• Extend to teleoperate with both left and right arms. For some reason the gravity compensation on the right arm didn't work. Suspect a urdf issue.
• Investigate urdf. A few issues found in the urdf that were manually fixed.
• Add the urdf path to the config of the leader openarms class
• The left and right arms are flipped. In the code you will notice that we control the right arm eventhough the left arm moves on the real robot. I suspect a small bug in the can bus naming.
• Add impedance control.
• Add force control and feedback