[RFC] Expanding Moteus Communication for Debug

[lucienpeach]

Introduces new accessor functions within joint_moteus.h to allow for inclusion of additional registers as defined in the moteus controller reference.md. Includes supporting architecture via options to allow user specification of desired registers to be read in controller definition.

Change List

• Provides new accessor functions to request additional registers from the moteus, as defined in the QueryCommand struct within moteus_protocol.h. These additional registers include:
  • q_current, the current in the Q phase measured in amperes.
  • d_current, the current in the D phase measured in amperes.
  • temperature, the current board temperature, measured in degrees Celsius.
  • voltage, the current input voltage.
  • fault, a fault code which will be set if the primary mode is 1 (Fault).
• Adds fault code enumeration (pulled from fw/error.h from the moteus repository) to enable reading of fault code information, if desired.
• Adds joint vector getter GetJoints() to mjbots_hardware_interface.h which returns a vector of shared pointers to objects of the JointMoteus class. This allows the new accessor functions in joint_moteus.h to be referenced from the controller level to allow for logging of the respective registers. These new accessor functions include:
  • get_temperature(), which provides the temperature register.
  • get_q_current(), which provides the q_current register.
  • get_d_current(), which provides the d_current register.
  • get_voltage(), which provides the voltage register.
  • get_fault(), which provides the fault register.
• Provides new boolean options within the MjbotsHardwareInterface class which dictate the request sent to the moteus by the pi3hat and the register packets to be sent from the moteus to the pi3hat. These new options include:
  • send_current, which includes packets from q_current and d_current registers if true.
  • send_voltage, which includes packets from voltage register if true.
  • send_temperature, which includes packets from temperature register if true.
  • send_fault, which includes packets from fault register if true.

Upcoming Changes

• Create a joint_moteus.cpp implementation file to clean up joint_moteus.h and prevent this file from becoming too convoluted.
• Streamline code to eliminate static resolution definition and reduce the number of options calls required. This would allow the user to specify the resolution of a register while simultaneously identifying that register to be included in packets from the moteus.
  • This proposed change would require numerous type-related changes and edits within a number of low-level files, the specifics of which ought to be a topic of discussion and deliberation.

Testing

Hardware testing on the DOQ robot is planned to verify the effectiveness of this code in practice. This testing will primarily seek to validate logging of new registers from the moteus.

Questions

There are a number of ways to build off this code and optimize it. Discussion of changes proposed above and suggestions on additional new changes are greatly appreciated.

[lucienpeach]

Builds upon previous changes to allow requests to be sent via a QueryCommand struct that replaces the previous boolean method of requesting data while bolstering user optionality through resolution selection. Includes initial support for open-loop control and introduces safety checks via warning messages and kodlab::ActivateCtrlC() to alert the user of input resolution errors and to prevent unsafe and improper closed-loop control.

Change List

• Introduces a source file joint_moteus.cpp to streamline header vs. implementation for the JointMoteus class.
• Adds a library of preset QueryCommand structs at the JointMoteus class level for common combinations of requests and resolutions. This library is not comprehensive and thus the user can make adjustments at the controller level as desired.
• Condensed UpdateMoteus() to accept a reply_message object of type QueryResult, as opposed to requiring a large number of individual reply fields (reply_pos, reply_vel, etc.) as inputs. This goes hand-in-hand with the previously stated change to allow for QueryCommand structs as user inputs.
• Because of the additional freedom provided to the user through the passing of a QueryCommand struct, it is now possible for a new and inexperienced user to encounter previously unseen issues. To prevent this from happening, and to inform the inexperienced user of these issues, a number of additions have been made:
  • Adds LOG_WARN messages within accessor functions at the JointMoteus class level which inform the user if the resolution of a requested data field is set to kIgnore. If the resolution is set to a different value, the accessor function provides the respective field at the resolution specified in the QueryCommand struct. These messages are of "warning" status and thus do not kill the control loop.
  • Adds initial support for open-loop control via the introduction of a new option open_loop which is stored within the options struct. If this value is set to true, the control loop will not check to ensure that position and velocity resolutions are set to a value other than kIgnore (since these readings are not necessary for open-loop control).
  • Introduces a new function in the JointMoteus class, is_open_loop(), which checks to ensure that the resolutions of position and velocity are set to values other than kIgnore. If either of these values are set to kIgnore in the QueryCommand input struct, is_open_loop() returns a boolean value of true, indicating that the current resolution configuration requires open-loop control.
  • Outputs a LOG_FATAL message and runs kodlab::ActivateCtrlC() if options.open_loop is set to false but is_open_loop() outputs a value of true, indicating that open-loop control is necessary for the current resolution configuration but not yet specified. This effectively kills the control loop from within the constructor definition, preventing the user from running closed-loop control without reporting position or velocity.
• Provides new accessor functions in the JointMoteus class:
  • get_velocity(), which overrides the respective virtual function in the JointBase class and enables LOG_WARN message support.
  • get_position(), which overrides the respective virtual function in the JointBase class and enables LOG_WARN message support.
  • get_servo_torque(), which overrides the respective virtual function in the JointBase class and enables LOG_WARN message support.
  • get_measured_torque(), which overrides the respective virtual function in the JointBase class and enables LOG_WARN message support.
  • get_mode(), which allows allows the user to directly access the mode (circumventing the current architecture, which requires accessing GetJointModes() via the MjbotsHardwareInterface class). Additionally provides support for LOG_WARN messages.
  • get_query_command(), which allows the user to access the current QueryCommand struct.
• Introduces the all_unset() function within QueryResult in the moteus namespace, which is used to determine if the fields within a given QueryResult struct are populated or are set to default values. Added a new enum value kUnset to both Mode and Fault enums within the moteus namespace to represent these unset/default values (set to -1).
  • In conjunction with the any_set() function which exists within QueryCommand in the moteus namespace, these additions are intended to provide more accurate insight into potential drops in CAN communication via terminal messages.
• Minor changes to settings and naming conventions.
  • Changed send_pd_commands bool to use_pd_commands to better encompass the utility of this option.
  • Adjusted maximum commanded torque resolution res.maximum_torque to kInt16 resolution from kInt8 to prevent cutoffs of 0.5 Nm when use_pd_commands is set to true.

Testing

Testing on DOQ has confirmed proper communication with the robot and functionality of safety features in both open- and closed-loop control schemes (toggled via the open_loop option). Plotting has demonstrated reasonable values for all requested fields and control loop communications function without packet loss at 1000 Hz even with a fully comprehensive QueryCommand struct that returns all possible data fields.

Questions

Open to suggestions on QueryCommand library changes or additions. Interested in thoughts of more robust open-loop support in future PRs.