Merge bc1de50 into 0e8c57f

CHANGELOG.md

@@ -57,6 +57,7 @@ All notable changes to this project will be documented in this file.
 - Bullet: Gravity parameter
 - Bullet: Initial orientation parameter
 - Optional constructor parameter to require the joystick to be present
+- Spine: check servo replies upon reception, report unusual modes
 - Unit tests for the internal agent interface
 
 ### Changed

vulp/actuation/BulletInterface.cpp

@@ -16,6 +16,8 @@
 
 #include "vulp/actuation/BulletInterface.h"
 
+#include <spdlog/spdlog.h>
+
 #include <algorithm>
 #include <memory>
 #include <string>
@@ -169,7 +171,10 @@ void BulletInterface::send_commands(const moteus::Data& data) {
       motor_args.m_maxTorqueValue = 0.;  // [N m]
       bullet_.setJointMotorControl(robot_, joint_index, motor_args);
     }
-    servo_reply_[joint_name].result.mode = command.mode;
+    servo_reply_[joint_name].result.mode =
+        (command.mode == moteus::Mode::kPositionContinue)
+            ? moteus::Mode::kPosition
+            : command.mode;
 
     if (command.mode == moteus::Mode::kStopped) {
       motor_args.m_controlMode = CONTROL_MODE_VELOCITY;
@@ -179,14 +184,13 @@ void BulletInterface::send_commands(const moteus::Data& data) {
       continue;
     }
 
-    if (command.mode != moteus::Mode::kPosition) {
+    if (command.mode != moteus::Mode::kPosition &&
+        command.mode != moteus::Mode::kPositionContinue) {
       throw std::runtime_error(
           "Bullet interface does not support command mode " +
           std::to_string(static_cast<unsigned>(command.mode)));
     }
 
-    // TODO(scaron): introduce control_position/velocity intermediates
-    // See https://github.com/mjbots/moteus/blob/main/docs/reference.md
     const double target_position = command.position.position * (2.0 * M_PI);
     const double target_velocity = command.position.velocity * (2.0 * M_PI);
     if (params_.use_torque_control) {

vulp/actuation/Interface.h

@@ -130,6 +130,18 @@ class Interface {
     }
   }
 
+  /*! Keep going with current position commands.
+   *
+   * \param[out] commands Servo commands.
+   */
+  inline void continue_position_commands() noexcept {
+    for (auto& command : commands_) {
+      if (command.mode == actuation::moteus::Mode::kPosition) {
+        command.mode = actuation::moteus::Mode::kPositionContinue;
+      }
+    }
+  }
+
  private:
   //! Servo layout.
   ServoLayout servo_layout_;

vulp/actuation/Pi3HatInterface.cpp

@@ -125,6 +125,10 @@ moteus::Output Pi3HatInterface::cycle_can_thread() {
         moteus::EmitPositionCommand(&write_frame, cmd.position, cmd.resolution);
         break;
       }
+      case moteus::Mode::kPositionContinue: {
+        // keep going with last position command
+        break;
+      }
       default: {
         throw std::logic_error("unsupported mode");
       }

vulp/actuation/moteus/Mode.h

@@ -24,22 +24,126 @@
 
 namespace vulp::actuation::moteus {
 
-//! Control mode
+/*! Control mode
+ *
+ * This class mirrors the BldcServoMode in ``bldc_servo_structs.h``. See the
+ * mjbots/moteus repository for details.
+ */
 enum class Mode {
+  // In this mode, the entire motor driver will be disabled.
+  //
+  // When exiting this state, the current offset will be
+  // recalibrated.
   kStopped = 0,
+
+  // This stage cannot be commanded directly, but will be entered
+  // upon any fault. Here, the motor driver remains enabled, but
+  // the output stage power is removed. The only valid transition
+  // from this state is to kStopped.
   kFault = 1,
+
+  // This mode may not be commanded directly. It is used when
+  // transitioning from kStopped to another mode.
   kEnabling = 2,
+
+  // This mode may not be commanded directly, but is used when
+  // transitioning from kStopped to another mode.
   kCalibrating = 3,
+
+  // This mode may not be commanded directly, but is used when
+  // transitioning from kStopped to another mode.
   kCalibrationComplete = 4,
+
+  // Directly control the PWM of all 3 phases.
   kPwm = 5,
+
+  // Control the voltage of all three phases
   kVoltage = 6,
+
+  // Control the phase and voltage magnitude
   kVoltageFoc = 7,
+
+  // Control d and q voltage
   kVoltageDq = 8,
+
+  // Control d and q current
   kCurrent = 9,
+
+  // Control absolute position
   kPosition = 10,
+
+  // This state can be commanded directly, and will also be entered
+  // automatically upon a watchdog timeout from kPosition.  When in
+  // this state, the controller will apply the selected fallback
+  // control mode.
+  //
+  // The only way to exit this state is through a stop command.
   kPositionTimeout = 11,
+
+  // Control to zero velocity through a derivative only version of
+  // the position mode.
   kZeroVelocity = 12,
-  kNumModes,
+
+  // This applies the PID controller only to stay within a
+  // particular position region, and applies 0 torque when within
+  // that region.
+  kStayWithinBounds = 13,
+
+  // This mode applies a fixed voltage square waveform in the D axis
+  // in order to measure inductance assuming a motor with
+  // approximately equal D/Q axis inductances.
+  kMeasureInductance = 14,
+
+  // All phases are pulled to ground.
+  kBrake = 15,
+
+  kPositionContinue = 42,  // Vulp-specific, not from moteus lib
 };
 
+/*! Name of a mode.
+ *
+ * \param[in] mode Mode to name.
+ */
+constexpr const char* to_string(const Mode& mode) noexcept {
+  switch (mode) {
+    case Mode::kStopped:
+      return "Mode::kStopped";
+    case Mode::kFault:
+      return "Mode::kFault";
+    case Mode::kEnabling:
+      return "Mode::kEnabling";
+    case Mode::kCalibrating:
+      return "Mode::kCalibrating";
+    case Mode::kCalibrationComplete:
+      return "Mode::kCalibrationComplete";
+    case Mode::kPwm:
+      return "Mode::kPwm";
+    case Mode::kVoltage:
+      return "Mode::kVoltage";
+    case Mode::kVoltageFoc:
+      return "Mode::kVoltageFoc";
+    case Mode::kVoltageDq:
+      return "Mode::kVoltageDq";
+    case Mode::kCurrent:
+      return "Mode::kCurrent";
+    case Mode::kPosition:
+      return "Mode::kPosition";
+    case Mode::kPositionTimeout:
+      return "Mode::kPositionTimeout";
+    case Mode::kZeroVelocity:
+      return "Mode::kZeroVelocity";
+    case Mode::kStayWithinBounds:
+      return "Mode::kStayWithinBounds";
+    case Mode::kMeasureInductance:
+      return "Mode::kMeasureInductance";
+    case Mode::kBrake:
+      return "Mode::kBrake";
+    case Mode::kPositionContinue:
+      return "Mode::kPositionContinue";
+    default:
+      break;
+  }
+  return "?";
+}
+
 }  // namespace vulp::actuation::moteus

vulp/actuation/tests/InterfaceTest.cpp

@@ -162,4 +162,36 @@ TEST_F(InterfaceTest, ForwardVelocityCommands) {
   }
 }
 
+TEST_F(InterfaceTest, ContinuePositionAfterFault) {
+  for (auto& command : interface_->commands()) {
+    command.mode = actuation::moteus::Mode::kFault;
+  }
+  interface_->continue_position_commands();
+  for (const auto& command : interface_->commands()) {
+    ASSERT_EQ(command.mode, actuation::moteus::Mode::kFault);
+  }
+}
+
+TEST_F(InterfaceTest, ContinuePositionAfterStop) {
+  interface_->write_stop_commands();
+  interface_->continue_position_commands();
+  for (const auto& command : interface_->commands()) {
+    ASSERT_EQ(command.mode, actuation::moteus::Mode::kStopped);
+  }
+}
+
+TEST_F(InterfaceTest, ContinuePositionAfterPosition) {
+  Dictionary action;
+  for (auto servo_name :
+       {"left_pump", "left_grinder", "right_pump", "right_grinder"}) {
+    action("servo")(servo_name)("position") = 2 * M_PI;  // [rad]
+    action("servo")(servo_name)("velocity") = M_PI;      // [rad] / [s]
+  }
+  interface_->write_position_commands(action);
+  interface_->continue_position_commands();
+  for (const auto& command : interface_->commands()) {
+    ASSERT_EQ(command.mode, actuation::moteus::Mode::kPositionContinue);
+  }
+}
+
 }  // namespace vulp::actuation

vulp/spine/Spine.cpp

@@ -26,8 +26,11 @@
 
 #include <limits>
 
+#include "vulp/actuation/moteus/Mode.h"
 #include "vulp/observation/ObserverError.h"
 
+namespace moteus = vulp::actuation::moteus;
+
 namespace vulp::spine {
 
 using observation::ObserverError;
@@ -184,11 +187,8 @@ void Spine::cycle_actuation() {
     } else if (state_machine_.state() == State::kAct) {
       Dictionary& action = dict_("action");
       actuation_.write_position_commands(action);
-      // TODO(scaron): don't re-send actuation
-      // See https://github.com/tasts-robots/vulp/issues/2
-      // spdlog::info("[Spine] ok");
     } else {
-      // spdlog::warn("[Spine] re-sending previous commands");
+      actuation_.continue_position_commands();
     }
   } catch (const std::exception& e) {
     spdlog::error("[Spine] Caught an exception: {}", e.what());
@@ -213,6 +213,7 @@ void Spine::cycle_actuation() {
     std::copy(actuation_.replies().begin(),
               actuation_.replies().begin() + rx_count, latest_replies_.begin());
     latest_imu_data_ = actuation_.imu_data();
+    check_replies(latest_replies_);
   }
 
   // Now we are after the previous cycle (we called actuation_output_.get())
@@ -236,4 +237,42 @@ void Spine::cycle_actuation() {
   actuation_output_ = promise->get_future();
 }
 
+void Spine::check_replies(
+    const std::vector<actuation::moteus::ServoReply>& servo_replies) {
+  const auto& servo_joint_map = actuation_.servo_joint_map();
+  for (const auto& reply : servo_replies) {
+    const int servo_id = reply.id;
+    auto it = servo_joint_map.find(servo_id);
+    if (it == servo_joint_map.end()) {
+      spdlog::error("Unknown servo ID {} in CAN reply", servo_id);
+      continue;
+    }
+
+    const auto& joint = it->second;
+    switch (reply.result.mode) {
+      case moteus::Mode::kStopped:
+      case moteus::Mode::kPosition:
+      case moteus::Mode::kZeroVelocity:
+      case moteus::Mode::kPositionContinue: {
+        // expected mode, nothing to report
+        break;
+      }
+      case moteus::Mode::kEnabling: {
+        spdlog::info("{} servo is being enabled...", joint);
+        break;
+      }
+      case moteus::Mode::kCalibrating: {
+        spdlog::info("{} servo is calibrating...", joint);
+        break;
+      }
+      default: {
+        const unsigned mode = static_cast<unsigned>(reply.result.mode);
+        const std::string label = moteus::to_string(reply.result.mode);
+        spdlog::error("{} servo is in mode {} ({})", joint, mode, label);
+        break;
+      }
+    }
+  }
+}
+
 }  // namespace vulp::spine

vulp/spine/Spine.h

@@ -143,6 +143,13 @@ class Spine {
   //! Log internal dictionary
   void log_dict();
 
+  /*! Check servo replies.
+   *
+   * \param[in] servo_replies Sequence of servo replies.
+   */
+  void check_replies(
+      const std::vector<actuation::moteus::ServoReply>& servo_replies);
+
  protected:
   //! Frequency of the spine loop in [Hz].
   const unsigned frequency_;