Avoid re-sending old position commands

CHANGELOG.md

@@ -124,6 +124,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

@@ -3,6 +3,8 @@
 
 #include "vulp/actuation/BulletInterface.h"
 
+#include <spdlog/spdlog.h>
+
 #include <algorithm>
 #include <memory>
 #include <string>
@@ -213,7 +215,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;
@@ -223,14 +228,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);
  const double feedforward_torque = command.position.feedforward_torque;

vulp/actuation/Interface.h

@@ -117,6 +117,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

@@ -113,6 +113,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

@@ -12,22 +12,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

@@ -149,4 +149,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

@@ -14,8 +14,11 @@
 
 #include <limits>
 
+#include "vulp/actuation/moteus/Mode.h"
 #include "vulp/exceptions/ObserverError.h"
 
+namespace moteus = vulp::actuation::moteus;
+
 namespace vulp::spine {
 
 using palimpsest::Dictionary;
@@ -179,11 +182,8 @@ void Spine::cycle_actuation() {
  } else if (state_machine_.state() == State::kAct) {
  Dictionary& action = working_dict_("action");
  actuation_.write_position_commands(action);
- // TODO(scaron): don't re-send actuation
- // See https://github.com/upkie/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());
@@ -208,6 +208,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())
@@ -231,4 +232,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

@@ -130,6 +130,13 @@ class Spine {
  //! Log internal dictionary
  void log_working_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_;