Skip to content

[UFE] Commanded Torques Observable

Craig McDonald requested to merge feature/ufe-cmd-trq into devel-ufe

Description

The main purpose is to improve our UFE estimate by using the correct commanded joint torques. The client sends a command to the firmware, which is then augmented with RGC, UGC, and safety systems. Our disturbance observer assumes that robot weight is perfectly cancelled by RGC commands, so RGC still needs to be subtracted out of the total joint torque commands.

NOTE: A lot of files were touched in this MR because I added a lot of documentation to remove more XML warnings generated by the compiler. I've tried to enumerate below all the changes that are not just documentation.

In the file BurtSharp/CoAP/Constants.cs, I added missing summaries to all the coap routes and reorganized the file so they are all grouped properly. Most importantly I added the route "/group/cmd_trq", but I also added some routes that already existed in firmware but not in BurtSharp, such as "/tool/position" and "/tool/velocity".

In the file BurtSharp/CoAP/MsgTypes.cs I added the CommandedTorquesUpdate, which is the struct expected for /group/cmd_trq.

Added some flexibility to how we call the Update function in DisturbanceObserver so we can update in two steps: 1) provide the joint positions from the server update to run dynamics calculations, 2) provide the commanded joint torques from the new commanded torque update to subtract out from the RNE calculations.

RobotController now handles the subscription to the commanded torques observable when we set HasDisturbanceObserver to true.

Added the commanded joint torques (and rgc and ugc torques) to the DisplayBasicInfo example.

Added the ability to adjust the UGC gain in the DisturbanceObserverTest sandbox program so that we can confirm that this does effect the user force estimate. (If we were not getting commanded torques back from the firmware, this UGC force would not be known to the disturbance observer and not show up in our user force estimate.)

What to focus on

  • Subscription to /group/cmd_trq is working properly, demonstrated in both Example04-DisplayBasicInfo and Sandbox/DisturbanceObserverTest.
  • RobotController initializes and updates the disturbance observer correctly.
  • DisturbanceObserverTest demonstrates that firmware generated forces are accounted for.
Edited by Craig McDonald

Merge request reports