Feature/humble/current based control

[moyashibeans]

Implementation of Current Mode and CurrentBasedPosition Mode & Proposal for Additional Features

I have implemented control for Current and CurrentBasedPosition.
I am proposing this as a draft pull request for the following reasons:

• There are questions regarding the implemented features and the method of implementation.
• It includes changes/additions to 4 features.
• It contains significant changes to the process.

If you approve this proposal, we will consider making improvements and other modifications/splitting the PR. If you do not approve, we will reject this proposal.

Tested Environment

• NVIDIA Orin Nano Developer Kit
  • Jetpack 5.1.1 (L4T 35.3.1)
  • ROS 2: Humble
• U2D2
• U2D2 Power Hub
• Dynamixel Servo Motors
  • XM 540-W150-T
    • TTL Protocol
  • XW540-T140-R
    • RS485 Protocol

Implemented Features

1. Current Mode and CurrentBasedPosition Control Mode
   • Change in mode switching method
   • Addition of modes
2. Addition of Interface for Joint State/Command
3. Control of both RS485 and TTL motors in a single circuit
4. Minor corrections

Changes

1. Current Mode and CurrentBasedPosition Control Mode

1.1. Addition of Mode Switching Method

Commits(Add): c06216f1eed03124ba4bc1d409a03b0acc5b0129, 86da4367412476249e2bdaabd6d00a922e3355b6,
Commits(Removal): b44fcaa315cb91aad107c6bbcaa22694d5e54334, 9186d0603c8580bb9e1ad51028bd6bb38f5568e6

• Add code:
  • Enabled mode switching through the prepare_command_mode_switch function using information from hardware_interface.
  • Allowed switching to CurrentBasedPosition Control Mode when both effort and position interfaces are active.
• Remove code:
  • Removed the method of switching modes based on the difference from prev_command_ value of the previous loop.
  • Changed to switch/change modes only when there are changes in the control_interface instead of every loop.

1.2. Addition of Modes

Commits: b59cdc4f3df7b176f1524d8c4e7c0adaf47aca35, faed640e35536898300a8e3c882e048896255ce4

• Conducted currentMode control via the effort_controller.
• Implemented CurrentBasedPosition Control Mode, controllable from commands via both effort_controller and position_controller interfaces.

2. Addition of Joint State/Command Interface

Commit: 193dc8512ad0ded41da339707ccc96db1c2eee2d

• Set the number of hardware_interface joints based on joint command_interface/state_interface information from urdf.

3. Control of both RS485 and TTL Motors in a Single Circuit

Commit: 3eb921d7cb2f669eece591addb83d6564798e757

According to U2D2's specifications, TTL and RS485 cannot be read simultaneously. To overcome this limitation, we split the joints by protocol, allowing for separate reads. This approach ensures functionality even when both protocols are used together.

4. Minor Corrections

• Typo fixed 85df0b974e2daa29fe9aa51480d29211bd59a878
• Made use_dummy work in UpperCase as well 909f7de545d57e2af19b857c2604f0d5fb06781e
  • This change was made to accommodate parameters passed via xacro args that become UpperCase.

Questions

• Regarding feature 1, in using prepare_command_mode_switch, it was necessary to define the behavior when commandInterfaces do not exist at startup.
  • For this reason, I added a NoControl mode to the ControlMode enum, although it does not exist in Dynamixel. Is this acceptable?
• For feature 2, it can be argued that reading interfaces from xacro is more correct. However, I do not see a necessity to intentionally limit the number of interfaces. Is the implementation of this feature necessary?
• Regarding feature 3, it became possible to control motors with different protocols. This was a necessary feature in the environment I used, but is it necessary to include this in the PR?

[youtalk]

Thank you for your great contribution! I'll review and check this ASAP.

[soham2560]

any updates on this?