Merge branch 'feature/add-odrive' of https://github.com/project-march/hardware-interface into feature/add-odrive

Author: JorisWeeda

march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h

@@ -71,7 +71,7 @@ class IMotionCube : public MotorController, public Slave
 
   void setControlWord(uint16_t control_word);
   virtual void actuateRad(double target_rad) override;
-  virtual void actuateTorque(int16_t target_torque) override;
+  virtual void actuateTorque(double target_torque_ampere) override;
 
   bool initialize(int cycle_time) override;
   void goToTargetState(IMotionCubeTargetState target_state);
@@ -97,7 +97,9 @@ class IMotionCube : public MotorController, public Slave
   constexpr static double MAX_TARGET_DIFFERENCE = 0.393;
   // This value is slightly larger than the current limit of the
   // linear joints defined in the URDF.
-  const static int16_t MAX_TARGET_TORQUE = 23500;
+  constexpr static double IPEAK = 40;
+  // See CoE manual page 222
+  constexpr static double AMPERE_TO_IU_FACTOR = 65520;
 
   // Watchdog base time = 1 / 25 MHz * (2498 + 2) = 0.0001 seconds=100 µs
   static const uint16_t WATCHDOG_DIVIDER = 2498;
@@ -111,6 +113,7 @@ class IMotionCube : public MotorController, public Slave
 
 private:
   void actuateIU(int32_t target_iu);
+  int16_t ampereToTorqueIU(double ampere);
 
   void mapMisoPDOs(SdoSlaveInterface& sdo);
   void mapMosiPDOs(SdoSlaveInterface& sdo);

march_hardware/include/march_hardware/motor_controller/motor_controller.h

@@ -36,7 +36,7 @@ class MotorController
   virtual bool getIncrementalMorePrecise() const = 0;
 
   virtual void actuateRad(double target_rad) = 0;
-  virtual void actuateTorque(int16_t target_torque) = 0;
+  virtual void actuateTorque(double target_torque_ampere) = 0;
 
   virtual void prepareActuation() = 0;
   virtual bool initialize(int cycle_time) = 0;

march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h

@@ -7,6 +7,7 @@
 #define O_PM_ENCODER_POSITION_UI ".encoder.pos_estimate"
 #define O_PM_ENCODER_VELOCITY_UI ".encoder.vel_estimate"
 #define O_PM_ODRIVE_INPUT_VOLTAGE "vbus_voltage"
+#define O_PM_DESIRED_MOTOR_CURRENT ".motor.current_control.Iq_setpoint"
 #define O_PM_ACTUAL_MOTOR_CURRENT ".motor.current_control.Iq_measured"
 #define O_PM_ACTUAL_MOTOR_VOLTAGE_D ".motor.current_control.v_current_control_integral_d"
 #define O_PM_ACTUAL_MOTOR_VOLTAGE_Q ".motor.current_control.v_current_control_integral_q"

march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h

@@ -27,7 +27,7 @@ class OdriveMotor : public Odrive
   void reset() override;
 
   void actuateRad(double target_rad) override;
-  void actuateTorque(int16_t target_torque) override;
+  void actuateTorque(double target_torque_ampere) override;
 
   MotorControllerStates& getStates() override;
 

march_hardware/src/imotioncube/imotioncube.cpp

@@ -184,21 +184,22 @@ void IMotionCube::actuateIU(int32_t target_iu)
   this->write32(target_position_location, target_position);
 }
 
-void IMotionCube::actuateTorque(int16_t target_torque)
+void IMotionCube::actuateTorque(double target_torque_ampere)
 {
+  if (target_torque_ampere >= IPEAK)
+  {
+    throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE,
+                                   "Target torque of %dA exceeds max torque of %dA", target_torque_ampere, IPEAK);
+  }
+  int16_t target_torque = ampereToTorqueIU(target_torque_ampere);
+
   if (this->actuation_mode_ != ActuationMode::torque)
   {
     throw error::HardwareException(error::ErrorType::INVALID_ACTUATION_MODE,
                                    "trying to actuate torque, while actuation mode is %s",
                                    this->actuation_mode_.toString().c_str());
   }
 
-  if (target_torque >= MAX_TARGET_TORQUE)
-  {
-    throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE,
-                                   "Target torque of %d exceeds max torque of %d", target_torque, MAX_TARGET_TORQUE);
-  }
-
   bit16 target_torque_struct = { .i = target_torque };
 
   uint8_t target_torque_location = this->mosi_byte_offsets_.at(IMCObjectName::TargetTorque);
@@ -554,4 +555,10 @@ ActuationMode IMotionCube::getActuationMode() const
 {
   return this->actuation_mode_;
 }
+
+int16_t IMotionCube::ampereToTorqueIU(double ampere)
+{
+  // See CoE manual page 222
+  return AMPERE_TO_IU_FACTOR * ampere / (2 * IPEAK);
+}
 }  // namespace march

march_hardware/src/odrive/odrive_motor.cpp

@@ -69,17 +69,20 @@ void OdriveMotor::actuateRad(double target_rad)
   return;
 }
 
-void OdriveMotor::actuateTorque(int16_t target_torque)
+void OdriveMotor::actuateTorque(double target_torque_ampere)
 {
-  ROS_INFO("Actuating torque %d", target_torque);
-  return;
+  float target_torque_ampere_float = (float)target_torque_ampere;
+  std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT);
+  if (this->write(command_name_, target_torque_ampere_float) == 1)
+  {
+    ROS_ERROR("Could net set target torque; %f to the axis", target_torque_ampere);
+  }
 }
 
 MotorControllerStates& OdriveMotor::getStates()
 {
   static OdriveStates states;
 
-  // Common states
   states.motorCurrent = this->getMotorCurrent();
   states.controllerVoltage = this->getMotorControllerVoltage();
   states.motorVoltage = this->getMotorVoltage();

march_hardware/test/mocks/mock_motor_controller.h

@@ -33,7 +33,7 @@ class MockMotorController : public march::MotorController
   MOCK_METHOD0(getTorque, double());
 
   MOCK_METHOD1(actuateRad, void(double));
-  MOCK_METHOD1(actuateTorque, void(int16_t));
+  MOCK_METHOD1(actuateTorque, void(double));
 
   MOCK_METHOD1(initialize, bool(int cycle_time));
   MOCK_METHOD0(reset, void());

march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h

@@ -90,8 +90,8 @@ class MarchHardwareInterface : public hardware_interface::RobotHW
   static void getSoftJointLimitsError(const std::string& name, const urdf::JointConstSharedPtr& urdf_joint,
                                       joint_limits_interface::SoftJointLimits& error_soft_limits);
 
-  /* Limit of the change in effort command over one cycle, can be overridden by safety controller */
-  static constexpr double MAX_EFFORT_CHANGE = 5000;
+  /* Limit of the change (ampere) in effort command over one cycle, can be overridden by safety controller */
+  static constexpr double MAX_EFFORT_CHANGE = 6;
 
   /* March hardware */
   std::unique_ptr<march::MarchRobot> march_robot_;

march_hardware_interface/src/march_hardware_interface.cpp

@@ -231,7 +231,7 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat
   for (size_t i = 0; i < num_joints_; i++)
   {
     // Enlarge joint_effort_command because ROS control limits the pid values to a certain maximum
-    joint_effort_command_[i] = joint_effort_command_[i] * 1000.0;
+    joint_effort_command_[i] = joint_effort_command_[i];
     if (std::abs(joint_last_effort_command_[i] - joint_effort_command_[i]) > MAX_EFFORT_CHANGE)
     {
       joint_effort_command_[i] =