Loop validated cache

[jdcaporale]

Adds a Loop-tied cache class and integrates into the control loop.

Key changes:

• add virtual to ValidatedCache member functions for inheritance
• LoopId class, that houses static member we use to invalidate ValidatedLoopCache class instances
  • Housed in a uint32_t, which is large enough to avoid repeats offering unique loop ids
• ValidatedLoopCache class which auto invalidates when the control loop updates
• MJBotsControlLoop increments LoopId before every call of Update()

Design decisions / possible issues:

• uint32_t last_loop_id_ and LoopId loop_id_ members in the cache increases the memory overhead, especially for caching small data types, could be smaller if we use uint8_t but without the same uniqueness properties (at 1kHz, uint8_t would repeat 4 times a second)
• Implemented operator= overload to make using caches easier, but it does hide the validation from the user and could lead to mistakes

This was tested locally on hardware using print statements in a simple control loop.

class Test_Validated_Loop_Cache : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog> {
  using MjbotsControlLoop::MjbotsControlLoop;
  kodlab::ValidatedLoopCache<float> myValue;
  void Init() override {
    myValue = 0.5;
  }
  void Update() override {
    std::vector<float> torques(num_joints_, 0);
    robot_->SetTorques(torques);
    std::cout<<kodlab::LoopId::get()<<std::endl;
    if(!myValue.valid()){
        myValue = myValue.get() + 1;
    }
    // if(myValue.valid()){
    //     myValue = myValue.get() - 1;
    // }
    std::cout<<myValue<<std::endl;
  }
  void PrepareLog() override {
    // Populate log message with data from current control loop cycle
    for (int servo = 0; servo < num_joints_; servo++) {
      log_data_->positions[servo] = robot_->GetJointPositions()[servo];
      log_data_->velocities[servo] = robot_->GetJointVelocities()[servo];
      log_data_->modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
      log_data_->torques[servo] = robot_->GetJointTorqueCmd()[servo];
    }

    // Fill remaining log message fields with zeros
    for (int servo = num_joints_; servo < 13; servo++) {
      log_data_->positions[servo] = 0;
      log_data_->velocities[servo] = 0;
      log_data_->modes[servo] = 0;
      log_data_->torques[servo] = 0;
    }
  }
};

[ShaneRozenLevy]

Looks good. A few small things to discuss/document.

[ethanmusser]

Objections to LoopId, but looking good otherwise.

[ethanmusser]

This is a mutable singleton with some possible issues.

First, philosophically, why use a singleton here? It goes against the ISO guidelines, and the resulting behavior could instead be maintained in a local scope. E.g., a loop ID is maintained in the control loop and all ValdatedLoopCaches are constructed with a reference/pointer to that ID (and can use the same invalidation logic).

That said, since this is a pretty high-level, integral thing, then a singleton could be fine. But:

• this singleton could suffer from the static initialization order fiasco if LoopId were to be referenced in another static variable initialization;
• loop_id_ can be arbitrarily modified by children of LoopId;
• LoopId can be multiply constructed (which could be confusing).

You could alleviate some of these issues by making LoopId a Meyer's Singleton. I think something like this would work.

class LoopId {
 public:
  static LoopId& get() {
    static LoopId loop_id;
    return loop_id;
  }

  static void set(uint32_t id) {
    auto loop_id = LoopId::get();
    loop_id = id;
  }

  static void increment() {
    auto loop_id = LoopId::get();
    loop_id++;
  }

  LoopId(const LoopId&) = delete;
  LoopId& operator=(const LoopId&) = delete;

private:
  LoopId() {}
  ~LoopId() {}

};

[ShaneRozenLevy]

LGTM, besides why loop_cache.cpp exists.

[ShaneRozenLevy]

What is the purpose of this file?

[jdcaporale]

Just placeholder making sure the new version is good. Could move implementation into it if that is something we want, but otherwise I will delete.

[ShaneRozenLevy]

I'm always of the opinion that the implementation should live in the source file when possible.

[jdcaporale]

Since this is not templated, it's an easy move, so I will.

[jdcaporale]

@ethanmusser Thoughts about this implementation? Took some of your advice, I think it should be ok now, and maybe even threadsafe with the addition of atomic.

[jdcaporale]

Suggested change

	last_loop_id_ = loop_id_.get();
	last_loop_id_ = LoopId::get();

[jdcaporale]

Suggested change

LoopId loop_id_; // Loop id, tied to control loop