Refactor spine-actuation functions

actuation/BUILD

@@ -41,7 +41,11 @@ cc_library(
     hdrs = [
         "Interface.h",
     ],
+    srcs = [
+        "Interface.cpp",
+    ],
     deps = [
+        "//actuation/moteus",
         ":imu_data",
         ":resolution",
         ":servo_layout",

actuation/Interface.cpp

@@ -0,0 +1,119 @@
+/*
+ * Copyright 2022 Stéphane Caron
+ * Copyright 2023 Inria
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * This file incorporates work covered by the following copyright and
+ * permission notice:
+ *
+ *     Copyright 2020 Josh Pieper, jjp@pobox.com.
+ *     License: Apache-2.0
+ */
+
+#include "vulp/actuation/Interface.h"
+
+#include <palimpsest/Dictionary.h>
+
+#include <limits>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "vulp/actuation/moteus/Mode.h"
+#include "vulp/actuation/moteus/ServoCommand.h"
+
+namespace vulp::actuation {
+
+namespace defaults {
+
+constexpr double kKdScale = 1.0;
+
+constexpr double kKpScale = 1.0;
+
+// TODO(scaron): this is a sane default for a qdd100, not for weaker servos
+constexpr double kMaximumTorque = 8.42;  // N.m
+
+constexpr double kVelocity = 0.0;  // rad/s
+
+}  // namespace defaults
+
+void Interface::initialize_action(palimpsest::Dictionary& action) {
+  for (const auto& id_joint : servo_layout_.servo_joint_map()) {
+    const std::string& joint_name = id_joint.second;
+    auto& servo_action = action("servo")(joint_name);
+    servo_action("position") = std::numeric_limits<double>::quiet_NaN();
+    servo_action("velocity") = 0.0;
+    servo_action("kp_scale") = defaults::kKpScale;
+    servo_action("kd_scale") = defaults::kKdScale;
+    servo_action("maximum_torque") = defaults::kMaximumTorque;
+  }
+}
+
+void Interface::write_position_commands(const palimpsest::Dictionary& action) {
+  using Mode = actuation::moteus::Mode;
+  if (!action.has("servo")) {
+    spdlog::warn("No position command at key \"servo\" of action");
+    return;
+  }
+
+  const auto& servo = action("servo");
+  const auto& servo_joint_map = servo_layout_.servo_joint_map();
+  for (auto& command : commands_) {
+    const int servo_id = command.id;
+    auto it = servo_joint_map.find(servo_id);
+    if (it == servo_joint_map.end()) {
+      spdlog::error("Unknown servo ID {} in CAN command", servo_id);
+      command.mode = Mode::kStopped;
+      continue;
+    }
+    const auto& joint = it->second;
+    if (!servo.has(joint)) {
+      spdlog::error("No action for joint {} (servo ID={})", joint, servo_id);
+      command.mode = Mode::kStopped;
+      continue;
+    }
+    const auto& servo_action = servo(joint);
+    if (!servo_action.has("position")) {
+      spdlog::error("No position command for joint {} (servo ID={})", joint,
+                    servo_id);
+      command.mode = Mode::kStopped;
+      continue;
+    }
+
+    const double position_rad = servo_action("position");
+    const double velocity_rad_s =
+        servo_action.get<double>("velocity", defaults::kVelocity);
+    const double kp_scale =
+        servo_action.get<double>("kp_scale", defaults::kKpScale);
+    const double kd_scale =
+        servo_action.get<double>("kd_scale", defaults::kKdScale);
+    const double maximum_torque =
+        servo_action.get<double>("maximum_torque", defaults::kMaximumTorque);
+
+    // The moteus convention is that positive angles correspond to clockwise
+    // rotations when looking at the rotor / back of the moteus board. See:
+    // https://jpieper.com/2021/04/30/moteus-direction-configuration/
+    const double position_rev = position_rad / (2.0 * M_PI);
+    const double velocity_rev_s = velocity_rad_s / (2.0 * M_PI);
+
+    command.mode = Mode::kPosition;
+    command.position.position = position_rev;
+    command.position.velocity = velocity_rev_s;
+    command.position.kp_scale = kp_scale;
+    command.position.kd_scale = kd_scale;
+    command.position.maximum_torque = maximum_torque;
+  }
+}
+
+}  // namespace vulp::actuation

actuation/Interface.h

@@ -102,6 +102,31 @@ class Interface {
    */
   moteus::Data& data() { return data_; }
 
+  /*! Initialize action dictionary with keys corresponding to the servo layout.
+   *
+   * \param[out] action Action dictionary.
+   */
+  void initialize_action(palimpsest::Dictionary& action);
+
+  /*! Write position commands from an action dictionary.
+   *
+   * \param[in] action Action to read commands from.
+   */
+  void write_position_commands(const palimpsest::Dictionary& action);
+
+  /*! Stop all servos.
+   *
+   * \param[out] commands Servo commands to set to stop.
+   *
+   * This function does not and should not throw, as it will be called by
+   * default if any exception is caught from the spine control loop.
+   */
+  inline void write_stop_commands() noexcept {
+    for (auto& command : commands_) {
+      command.mode = actuation::moteus::Mode::kStopped;
+    }
+  }
+
  private:
   //! Servo layout.
   ServoLayout servo_layout_;

actuation/tests/InterfaceTest.cpp

@@ -25,11 +25,13 @@
 
 namespace vulp::actuation {
 
+using palimpsest::Dictionary;
+
 class SmallInterface : public Interface {
  public:
   using Interface::Interface;
 
-  void reset(const palimpsest::Dictionary& config) override {}
+  void reset(const Dictionary& config) override {}
 
   void cycle(const moteus::Data& data,
              std::function<void(const moteus::Output&)> callback) final {}
@@ -72,4 +74,92 @@ TEST_F(InterfaceTest, DataIsInitialized) {
   ASSERT_EQ(interface_->data().replies.data(), interface_->replies().data());
 }
 
+TEST_F(InterfaceTest, WriteStopCommands) {
+  for (auto& command : interface_->commands()) {
+    command.mode = actuation::moteus::Mode::kFault;
+  }
+  interface_->write_stop_commands();
+  for (const auto& command : interface_->commands()) {
+    ASSERT_EQ(command.mode, actuation::moteus::Mode::kStopped);
+  }
+}
+
+TEST_F(InterfaceTest, ExpectFillsDictionaryKeys) {
+  Dictionary action;
+  interface_->initialize_action(action);
+  ASSERT_TRUE(action.has("servo"));
+  const Dictionary& servo = action("servo");
+  for (const auto joint_name :
+       {"left_grinder", "left_pump", "right_grinder", "right_pump"}) {
+    ASSERT_TRUE(servo.has(joint_name));
+    ASSERT_TRUE(servo(joint_name).has("position"));
+    ASSERT_TRUE(servo(joint_name).has("velocity"));
+    ASSERT_TRUE(servo(joint_name).has("kp_scale"));
+    ASSERT_TRUE(servo(joint_name).has("kd_scale"));
+    ASSERT_TRUE(servo(joint_name).has("maximum_torque"));
+  }
+}
+
+TEST_F(InterfaceTest, DontThrowIfNoServoAction) {
+  Dictionary action;
+  ASSERT_NO_THROW(interface_->write_position_commands(action));
+}
+
+TEST_F(InterfaceTest, StopUnknownServos) {
+  Dictionary action;
+  action("servo")("left_pump")("position") = 0.0;
+  action("servo")("left_grinder")("position") = 0.0;
+  interface_->write_position_commands(action);
+  ASSERT_EQ(interface_->commands()[0].mode, actuation::moteus::Mode::kPosition);
+  ASSERT_EQ(interface_->commands()[1].mode, actuation::moteus::Mode::kPosition);
+  ASSERT_EQ(interface_->commands()[2].mode, actuation::moteus::Mode::kStopped);
+  ASSERT_EQ(interface_->commands()[3].mode, actuation::moteus::Mode::kStopped);
+}
+
+TEST_F(InterfaceTest, ForwardPositionCommands) {
+  Dictionary action;
+  action("servo")("left_grinder")("position") = 2 * M_PI;   // [rad]
+  action("servo")("left_pump")("position") = M_PI;          // [rad]
+  action("servo")("right_grinder")("position") = 2 * M_PI;  // [rad]
+  action("servo")("right_pump")("position") = M_PI;         // [rad]
+
+  interface_->write_position_commands(action);
+  for (const auto& command : interface_->commands()) {
+    ASSERT_EQ(command.mode, actuation::moteus::Mode::kPosition);
+  }
+  const auto& commands = interface_->commands();
+  ASSERT_DOUBLE_EQ(commands[0].position.position, 1.0);  // [rev]
+  ASSERT_DOUBLE_EQ(commands[1].position.position, 0.5);  // [rev]
+  ASSERT_DOUBLE_EQ(commands[2].position.position, 1.0);  // [rev]
+  ASSERT_DOUBLE_EQ(commands[3].position.position, 0.5);  // [rev]
+}
+
+TEST_F(InterfaceTest, StopServoIfNoAction) {
+  Dictionary action;
+  interface_->write_position_commands(action);
+  ASSERT_EQ(interface_->commands()[0].mode, actuation::moteus::Mode::kStopped);
+}
+
+TEST_F(InterfaceTest, StopServoIfNoPosition) {
+  Dictionary action;
+  action("servo")("left_pump")("velocity") = 1.5;  // velocity but not position
+  interface_->write_position_commands(action);
+  ASSERT_EQ(interface_->commands()[0].mode, actuation::moteus::Mode::kStopped);
+}
+
+TEST_F(InterfaceTest, ForwardVelocityCommands) {
+  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);
+  for (const auto& command : interface_->commands()) {
+    ASSERT_EQ(command.mode, actuation::moteus::Mode::kPosition);
+    ASSERT_EQ(command.position.position, 1.0);
+    ASSERT_EQ(command.position.velocity, 0.5);
+  }
+}
+
 }  // namespace vulp::actuation

spine/BUILD

@@ -7,38 +7,11 @@ load("@pip_vulp//:requirements.bzl", "requirement")
 
 package(default_visibility = ["//visibility:public"])
 
-cc_library(
-    name = "stop_commands",
-    hdrs = [
-        "stop_commands.h",
-    ],
-    deps = [
-        "//actuation/moteus",
-    ],
-    include_prefix = "vulp/spine",
-)
-
-cc_library(
-    name = "position_commands",
-    hdrs = [
-        "position_commands.h",
-    ],
-    deps = [
-        "@palimpsest",
-        "//actuation/moteus",
-    ],
-    include_prefix = "vulp/spine",
-)
-
 cc_library(
     name = "request",
     hdrs = [
         "Request.h",
     ],
-    srcs = [
-    ],
-    deps = [
-    ],
     include_prefix = "vulp/spine",
 )
 
@@ -93,9 +66,7 @@ cc_library(
         "//observation:observer_pipeline",
         "//utils:realtime",
         "//utils:synchronous_clock",
-        ":position_commands",
         ":state_machine",
-        ":stop_commands",
         "@mpacklog",
     ],
     include_prefix = "vulp/spine",

spine/Spine.cpp

@@ -27,8 +27,6 @@
 #include <limits>
 
 #include "vulp/observation/ObserverError.h"
-#include "vulp/spine/position_commands.h"
-#include "vulp/spine/stop_commands.h"
 
 namespace vulp::spine {
 
@@ -79,7 +77,7 @@ void Spine::reset(const Dictionary& config) {
   actuation_.reset(config);
   dict_("action").clear();
   expect_timestamp(dict_("action"));
-  expect_position_commands(dict_("action"), actuation_.servo_joint_map());
+  actuation_.initialize_action(dict_("action"));
   observer_pipeline_.reset(config);
 }
 
@@ -189,25 +187,26 @@ void Spine::cycle_actuation() {
     // 2. Action
     if (state_machine_.state() == State::kSendStops ||
         state_machine_.state() == State::kShutdown) {
-      write_stop_commands(actuation_.commands());
+      actuation_.write_stop_commands();
     } else if (state_machine_.state() == State::kAct) {
       Dictionary& action = dict_("action");
-      write_position_commands(actuation_.commands(),
-                              actuation_.servo_joint_map(), 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::cycle_actuation] ok");
     } else {
-      // TODO(scaron): clear commands, otherwise the previous ones will be
-      // sent again!
+      // spdlog::warn("[Spine::cycle_actuation] re-sending previous commands");
     }
   } catch (const std::exception& e) {
     spdlog::error("[Spine::cycle_actuation] Caught an exception: {}", e.what());
     spdlog::error("[Spine::cycle_actuation] Sending stop commands...");
     state_machine_.process_event(Event::kInterrupt);
-    write_stop_commands(actuation_.commands());
+    actuation_.write_stop_commands();
   } catch (...) {
     spdlog::error("[Spine::cycle_actuation] Caught an unknown exception!");
     spdlog::error("[Spine::cycle_actuation] Sending stop commands...");
     state_machine_.process_event(Event::kInterrupt);
-    write_stop_commands(actuation_.commands());
+    actuation_.write_stop_commands();
   }
 
   // Whatever exceptions were thrown around, we caught them and at this