Added the configuration of the Json file to the odrive object.

also updated the xacro with the right limits.

Author: JorisWeeda

march_hardware/config/march_odrive.json

@@ -9,6 +9,11 @@
         "type":"uint32",
         "value":4096
     },
+    {
+        "name":"axis0.encoder.config.use_index",
+        "type":"bool",
+        "value":1
+    },
     {
         "name":"config.brake_resistance",
         "type":"float",

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

@@ -7,7 +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_DESIRED_MOTOR_CURRENT ".controller.current_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

@@ -18,7 +18,9 @@ namespace march
 class OdriveMotor : public Odrive
 {
 public:
-  OdriveMotor(const std::string& axisNumber, std::shared_ptr<OdriveEndpoint> odriveEndpoint, ActuationMode mode);
+  OdriveMotor(const std::string& axisNumber, std::shared_ptr<OdriveEndpoint> odriveEndpoint, ActuationMode mode,
+              std::string json_config_file_path);
+  ~OdriveMotor();
 
   bool initialize(int cycle_time) override;
   void prepareActuation() override;
@@ -66,6 +68,7 @@ class OdriveMotor : public Odrive
   int getAngleCountsIncremental();
 
   ActuationMode mode_;
+  std::string json_config_file_path_;
 };
 
 }  // namespace march

march_hardware/src/odrive/odrive.cpp

@@ -296,7 +296,7 @@ int Odrive::json_string_read(const Json::Value& json_parameter_object)
   else
   {
     ROS_ERROR("Error converting string for reading, invalid type %s", parameter_name.c_str());
-    return ODRIVE_ERROR;
+    return ODRIVE_ERROR
   }
 }
 
@@ -349,7 +349,7 @@ int Odrive::json_string_write(const Json::Value& json_parameter_object)
   else
   {
     ROS_ERROR("Error converting string for writing, invalid type %s", parameter_name.c_str());
-    return ODRIVE_ERROR;
+    return ODRIVE_ERROR
   }
 }
 
@@ -374,23 +374,21 @@ int Odrive::setConfigurations(const std::string& configuration_json_path)
   if (!res)
   {
     ROS_INFO("Error parsing odrive configuration, error");
-    return ODRIVE_ERROR;
+    return ODRIVE_ERROR
   }
 
   for (auto& parameter : this->odrive_configuration_json_)
   {
-    ROS_INFO("Setting %s to %s", parameter["name"].asString().c_str(), parameter["value"].asString().c_str());
+    ROS_DEBUG("Setting %s to %s", parameter["name"].asString().c_str(), parameter["value"].asString().c_str());
     int result = this->json_string_write(parameter);
 
     if (result != LIBUSB_SUCCESS)
     {
-      ROS_INFO("Setting %s to %s failed", parameter["name"].asString().c_str(), parameter["value"].asString().c_str());
+      ROS_WARN("Setting %s to %s failed", parameter["name"].asString().c_str(), parameter["value"].asString().c_str());
       continue;
     }
-
-    ROS_INFO("Setting succeeded %s", parameter["name"].asString().c_str());
   }
-  return ODRIVE_OK;
+  return ODRIVE_OK
 }
 
 template int Odrive::validateType(const odrive_json_object& json_object, int8_t&);

march_hardware/src/odrive/odrive_motor.cpp

@@ -6,11 +6,20 @@
 namespace march
 {
 OdriveMotor::OdriveMotor(const std::string& axisNumber, std::shared_ptr<OdriveEndpoint> odriveEndpoint,
-                         ActuationMode mode)
-  : Odrive(axisNumber, std::move(odriveEndpoint), false), mode_(mode)
+                         ActuationMode mode, std::string json_config_file_path)
+  : Odrive(axisNumber, std::move(odriveEndpoint), false), mode_(mode), json_config_file_path_(json_config_file_path)
 {
 }
 
+OdriveMotor::~OdriveMotor()
+{
+  if (this->setState(States::AXIS_STATE_IDLE) == 1)
+  {
+    ROS_FATAL("Not set to idle when closed");
+    return;
+  }
+}
+
 bool OdriveMotor::initialize(int cycle_time)
 {
   return cycle_time;
@@ -20,6 +29,13 @@ void OdriveMotor::prepareActuation()
 {
   this->importOdriveJson();
 
+  if (this->setConfigurations(this->json_config_file_path_) == 1)
+  {
+    ROS_FATAL("Setting configurations was not finished successfully");
+  }
+
+  this->reset();
+
   if (this->setState(States::AXIS_STATE_FULL_CALIBRATION_SEQUENCE) == 1)
   {
     ROS_FATAL("Calibration sequence was not finished successfully");
@@ -30,7 +46,7 @@ void OdriveMotor::prepareActuation()
 
   if (this->setState(States::AXIS_STATE_CLOSED_LOOP_CONTROL) == 1)
   {
-    ROS_FATAL("Calibration sequence was not finished successfully");
+    ROS_FATAL("Setting closed loop control was not finished successfully");
     return;
   }
 }
@@ -61,6 +77,27 @@ void OdriveMotor::reset()
   {
     ROS_ERROR("Could not reset axis");
   }
+
+  uint16_t axis_motor_error = 0;
+  command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR);
+  if (this->write(command_name_, axis_motor_error) == 1)
+  {
+    ROS_ERROR("Could not reset motor axis");
+  }
+
+  uint8_t axis_encoder_error = 0;
+  command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR);
+  if (this->write(command_name_, axis_encoder_error) == 1)
+  {
+    ROS_ERROR("Could not reset encoder axis");
+  }
+
+  uint8_t axis_controller_error = 0;
+  command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR);
+  if (this->write(command_name_, axis_controller_error) == 1)
+  {
+    ROS_ERROR("Could not reset controller axis");
+  }
 }
 
 void OdriveMotor::actuateRad(double target_rad)
@@ -72,6 +109,12 @@ void OdriveMotor::actuateRad(double target_rad)
 void OdriveMotor::actuateTorque(double target_torque_ampere)
 {
   float target_torque_ampere_float = (float)target_torque_ampere;
+  ROS_INFO("target torque: %f", target_torque_ampere_float);
+
+  if (target_torque_ampere_float < 3.0)
+  {
+    target_torque_ampere_float = 3.0;
+  }
   std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT);
   if (this->write(command_name_, target_torque_ampere_float) == 1)
   {
@@ -207,8 +250,7 @@ int OdriveMotor::getAngleCountsAbsolute()
 
 double OdriveMotor::getAngleRadAbsolute()
 {
-  double angle_rad = this->getAngleCountsAbsolute() * PI_2 / std::pow(2, 17);
-  return angle_rad;
+  return 0;
 }
 
 double OdriveMotor::getVelocityRadAbsolute()
@@ -230,6 +272,7 @@ int OdriveMotor::getAngleCountsIncremental()
     ROS_ERROR("Could not retrieve incremental position of the encoder");
     return ODRIVE_ERROR;
   }
+  ROS_WARN("encoder position: %f", iu_position);
   return iu_position;
 }
 

march_hardware_builder/src/hardware_builder.cpp

@@ -165,9 +165,11 @@ std::unique_ptr<march::OdriveMotor> HardwareBuilder::createOdrive(const YAML::No
   YAML::Node absolute_encoder_config = odrive_config["absoluteEncoder"];
   std::string axis = odrive_config["axis"].as<std::string>();
   std::string serial_number = odrive_config["serial_number"].as<std::string>();
+  std::string json_configuration_path = ros::package::getPath("march_hardware").append("/config/march_odrive.json");
+
   auto odrive_endpoint = usb_master.getSerialConnection(serial_number);
 
-  return std::make_unique<march::OdriveMotor>(axis, odrive_endpoint, mode);
+  return std::make_unique<march::OdriveMotor>(axis, odrive_endpoint, mode, json_configuration_path);
 }
 
 std::unique_ptr<march::IMotionCube> HardwareBuilder::createIMotionCube(const YAML::Node& imc_config,