Pi3HatInterface.h

Go to the documentation of this file.

// SPDX-License-Identifier: Apache-2.0
// Copyright 2022 Stéphane Caron
/*
 * This file incorporates work covered by the following copyright and
 * permission notice:
 *
 *     SPDX-License-Identifier: Apache-2.0
 *     Copyright 2020 Josh Pieper, jjp@pobox.com.
 */
 
#pragma once
 
#include <mjbots/pi3hat/pi3hat.h>
#include <pthread.h>
#include <spdlog/spdlog.h>
 
#include <Eigen/Geometry>
#include <condition_variable>
#include <functional>
#include <map>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <string>
#include <thread>
#include <utility>
#include <vector>
 
#include "vulp/actuation/ImuData.h"
#include "vulp/actuation/Interface.h"
#include "vulp/actuation/moteus/protocol.h"
#include "vulp/utils/realtime.h"
 
namespace vulp::actuation {
 
using Pi3Hat = ::mjbots::pi3hat::Pi3Hat;
 
class Pi3HatInterface : public Interface {
 public:
  Pi3HatInterface(const ServoLayout& layout, const int can_cpu,
                  const Pi3Hat::Configuration& pi3hat_config);
 
  ~Pi3HatInterface();
 
  void reset(const Dictionary& config) override;
 
  void observe(Dictionary& observation) const override;
 
  void cycle(const moteus::Data& data,
             std::function<void(const moteus::Output&)> callback) final;
 
 private:
  void run_can_thread();
 
  moteus::Output cycle_can_thread();
 
  Eigen::Quaterniond get_attitude() const noexcept {
    const double w = attitude_.attitude.w;
    const double x = attitude_.attitude.x;
    const double y = attitude_.attitude.y;
    const double z = attitude_.attitude.z;
    // These values were floats so the resulting quaternion is only
    // approximately normalized. We saw this property in d7fcaa97fa.
    return Eigen::Quaterniond(w, x, y, z).normalized();
  }
 
  Eigen::Vector3d get_angular_velocity() const noexcept {
    const double omega_x = attitude_.rate_dps.x * M_PI / 180.;
    const double omega_y = attitude_.rate_dps.y * M_PI / 180.;
    const double omega_z = attitude_.rate_dps.z * M_PI / 180.;
    return {omega_x, omega_y, omega_z};
  }
 
  Eigen::Vector3d get_linear_acceleration() const noexcept {
    const double a_x = attitude_.accel_mps2.x;
    const double a_y = attitude_.accel_mps2.y;
    const double a_z = attitude_.accel_mps2.z;
    return {a_x, a_y, a_z};
  }
 
 private:
  const int can_cpu_;
 
  // pi3hat configuration
  const Pi3Hat::Configuration pi3hat_config_;
 
  std::mutex mutex_;
 
  std::condition_variable can_wait_condition_;
 
  bool ongoing_can_cycle_ = false;
 
  bool done_ = false;
 
  std::function<void(const moteus::Output&)> callback_;
 
  moteus::Data data_;
 
  std::thread can_thread_;
 
  std::unique_ptr<Pi3Hat> pi3hat_;
 
  // These are kept persistently so that no memory allocation is
  // required in steady state.
  std::vector<::mjbots::pi3hat::CanFrame> tx_can_;
  std::vector<::mjbots::pi3hat::CanFrame> rx_can_;
 
  ::mjbots::pi3hat::Attitude attitude_;
};
 
}  // namespace vulp::actuation