Merge 93c39ac into 30c42eb

CHANGELOG.md

@@ -4,6 +4,10 @@ All notable changes to this project will be documented in this file.
 
 ## [Unreleased]
 
+### Changed
+
+- BulletInterface: Take feedforward torque commands into account
+
 ## [2.0.0] - 2023-11-15
 
 ### Added

vulp/actuation/BulletInterface.cpp

@@ -213,25 +213,24 @@ void BulletInterface::send_commands(const moteus::Data& data) {
     // 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;
     const double kp_scale = command.position.kp_scale;
     const double kd_scale = command.position.kd_scale;
     const double maximum_torque = command.position.maximum_torque;
-    const double joint_torque =
-        compute_joint_torque(joint_name, target_position, target_velocity,
-                             kp_scale, kd_scale, maximum_torque);
+    const double joint_torque = compute_joint_torque(
+        joint_name, feedforward_torque, target_position, target_velocity,
+        kp_scale, kd_scale, maximum_torque);
     motor_args.m_controlMode = CONTROL_MODE_TORQUE;
     motor_args.m_maxTorqueValue = joint_torque;
     servo_reply_[joint_name].result.torque = joint_torque;
     bullet_.setJointMotorControl(robot_, joint_index, motor_args);
   }
 }
 
-double BulletInterface::compute_joint_torque(const std::string& joint_name,
-                                             const double target_position,
-                                             const double target_velocity,
-                                             const double kp_scale,
-                                             const double kd_scale,
-                                             const double maximum_torque) {
+double BulletInterface::compute_joint_torque(
+    const std::string& joint_name, const double feedforward_torque,
+    const double target_position, const double target_velocity,
+    const double kp_scale, const double kd_scale, const double maximum_torque) {
   assert(!std::isnan(target_velocity));
   const BulletJointProperties& joint_props = joint_properties_[joint_name];
   const auto& measurements = servo_reply_[joint_name].result;
@@ -240,7 +239,8 @@ double BulletInterface::compute_joint_torque(const std::string& joint_name,
   const double kp = kp_scale * params_.torque_control_kp;
   const double kd = kd_scale * params_.torque_control_kd;
   const double tau_max = std::min(maximum_torque, joint_props.maximum_torque);
-  double torque = kd * (target_velocity - measured_velocity);
+  double torque = feedforward_torque;
+  torque += kd * (target_velocity - measured_velocity);
   if (!std::isnan(target_position)) {
     torque += kp * (target_position - measured_position);
   }

vulp/actuation/BulletInterface.h

@@ -228,6 +228,7 @@ class BulletInterface : public Interface {
   /*! Reproduce the moteus position controller in Bullet.
    *
    * \param[in] joint_name Name of the joint.
+   * \param[in] feedforward_torque Feedforward torque command in [N] * [m].
    * \param[in] target_position Target angular position in [rad].
    * \param[in] target_velocity Target angular velocity in [rad] / [s].
    * \param[in] kp_scale Multiplicative factor applied to the proportional gain
@@ -237,6 +238,7 @@ class BulletInterface : public Interface {
    * \param[in] maximum_torque Maximum torque in [N] * [m] from the command.
    */
   double compute_joint_torque(const std::string& joint_name,
+                              const double feedforward_torque,
                               const double target_position,
                               const double target_velocity,
                               const double kp_scale, const double kd_scale,

vulp/actuation/Interface.cpp

@@ -23,28 +23,18 @@
 
 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
+constexpr double kMaximumTorque = 1.0;  // N.m
 
 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("feedforward_torque") = 0.0;
     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;
+    servo_action("kp_scale") = 1.0;
+    servo_action("kd_scale") = 1.0;
+    servo_action("maximum_torque") = kMaximumTorque;
   }
 }
 
@@ -80,14 +70,13 @@ void Interface::write_position_commands(const palimpsest::Dictionary& action) {
     }
 
     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 velocity_rad_s = servo_action.get<double>("velocity", 0.0);
+    const double kp_scale = servo_action.get<double>("kp_scale", 1.0);
+    const double kd_scale = servo_action.get<double>("kd_scale", 1.0);
+    const double feedforward_torque =
+        servo_action.get<double>("feedforward_torque", 0.0);
     const double maximum_torque =
-        servo_action.get<double>("maximum_torque", defaults::kMaximumTorque);
+        servo_action.get<double>("maximum_torque", kMaximumTorque);
 
     // The moteus convention is that positive angles correspond to clockwise
     // rotations when looking at the rotor / back of the moteus board. See:
@@ -100,6 +89,7 @@ void Interface::write_position_commands(const palimpsest::Dictionary& action) {
     command.position.velocity = velocity_rev_s;
     command.position.kp_scale = kp_scale;
     command.position.kd_scale = kd_scale;
+    command.position.feedforward_torque = feedforward_torque;
     command.position.maximum_torque = maximum_torque;
   }
 }

vulp/actuation/tests/BulletInterfaceTest.cpp

@@ -151,15 +151,18 @@ TEST_F(BulletInterfaceTest, ComputeJointTorquesStopped) {
 
   // Stopped joint and no target => no velocity and no torque
   const auto& measurements = interface_->servo_reply().at("left_wheel").result;
-  const double target_position = std::numeric_limits<double>::quiet_NaN();
+  const double no_feedforward_torque = 0.0;
+  const double no_position = std::numeric_limits<double>::quiet_NaN();
   const double target_velocity = measurements.velocity * (2.0 * M_PI);
-  double tau = interface_->compute_joint_torque("left_wheel", target_position,
-                                                target_velocity, 1.0, 1.0, 1.0);
+  double tau = interface_->compute_joint_torque(
+      "left_wheel", no_feedforward_torque, no_position, target_velocity, 1.0,
+      1.0, 1.0);
   ASSERT_NEAR(measurements.velocity, 0.0, 1e-3);  // should be zero here
   ASSERT_NEAR(tau, 0.0, 1e-3);
 }
 
 TEST_F(BulletInterfaceTest, ComputeJointTorquesWhileMoving) {
+  const double no_feedforward_torque = 0.0;
   const double no_position = std::numeric_limits<double>::quiet_NaN();
   for (auto& command : data_.commands) {
     command.mode = moteus::Mode::kPosition;
@@ -179,17 +182,41 @@ TEST_F(BulletInterfaceTest, ComputeJointTorquesWhileMoving) {
   const auto& right_wheel = interface_->servo_reply().at("right_wheel").result;
   const double right_target_velocity = right_wheel.velocity * (2.0 * M_PI);
   const double right_torque = interface_->compute_joint_torque(
-      "right_wheel", no_position, right_target_velocity, 1.0, 1.0, 1.0);
+      "right_wheel", no_feedforward_torque, no_position, right_target_velocity,
+      1.0, 1.0, 1.0);
   ASSERT_GT(right_wheel.velocity, 0.1);
   ASSERT_NEAR(right_torque, 0.0, 1e-3);
 
   // Left wheel has kinetic friction
   const auto& left_wheel = interface_->servo_reply().at("left_wheel").result;
   const double left_target_velocity = left_wheel.velocity * (2.0 * M_PI);
   const double left_torque = interface_->compute_joint_torque(
-      "left_wheel", no_position, left_target_velocity, 1.0, 1.0, 1.0);
+      "left_wheel", no_feedforward_torque, no_position, left_target_velocity,
+      1.0, 1.0, 1.0);
   ASSERT_GT(left_wheel.velocity, 0.1);                  // positive velocity
   ASSERT_NEAR(left_torque, -kLeftWheelFriction, 1e-3);  // negative torque
 }
 
+TEST_F(BulletInterfaceTest, ComputeJointFeedforwardTorque) {
+  const double no_position = std::numeric_limits<double>::quiet_NaN();
+  for (auto& command : data_.commands) {
+    command.mode = moteus::Mode::kPosition;
+    command.position.position = no_position;
+    command.position.velocity = 0.0;  // rev/s
+    command.position.kp_scale = 0.0;
+    command.position.kd_scale = 0.0;
+    command.position.feedforward_torque = 0.42;  // N.m
+    command.position.maximum_torque = 1.0;       // N.m
+  }
+
+  // Cycle a couple of times so that both wheels spin up
+  interface_->cycle(data_, [](const moteus::Output& output) {});
+  interface_->cycle(data_, [](const moteus::Output& output) {});
+  interface_->cycle(data_, [](const moteus::Output& output) {});
+
+  // Right wheel has no kinetic friction
+  const auto& right_wheel_reply = interface_->servo_reply().at("right_wheel");
+  ASSERT_NEAR(right_wheel_reply.result.torque, 0.42, 1e-3);
+}
+
 }  // namespace vulp::actuation