Fix position controller bugs

vesc_hw_interface/README.md

@@ -27,11 +27,9 @@ All of following parameters are in `${VESC_HW_INTERFACE_NODE_NAME}/` namespace.
 #### Common
 - `port` (string, **required**): port name connecting to your VESC, *e.g.* `/dev/ttyUSB0`.
 - `command_mode` (string, **required**): control mode you want to use. Enter one of following parameters: `position`, `velocity`, `effort` and `effort_duty`.
-- `joint_name` (string, *default*: `joint_vesc`): corresponding joint name in your robot URDF.
 - `num_rotor_poles` (int, *default*: 2): the number of rotor poles.
 - `gear_ratio` (double, *default*: 1.0): ratio of reduction calculated by `joint velocity/motor velocity`.
 - `torque_const` (double, *default*: 1.0): motor torque constant (unit: Nm/A).
-- `robot_description_name` (string, *default*: /robot_description): name of the robot description parameters for loading joint limits
 
 **NOTE**: `gear_ratio` and `torque_const` are used to calculate joint states because VESC generally senses just motor position and motor current, neither joint position nor motor torque.
 If your motor unit has other structures, you should implement your own controller.

vesc_hw_interface/include/vesc_hw_interface/vesc_servo_controller.hpp

@@ -56,6 +56,7 @@ class VescServoController
   double getEffortSens();
   void executeCalibration();
   void updateSensor(const std::shared_ptr<VescPacket const>& packet);
+  bool calibrate();
 
 private:
   rclcpp::Node::SharedPtr node_;
@@ -104,7 +105,6 @@ class VescServoController
   double endstop_threshold_;
   double endstop_margin_;
 
-  bool calibrate();
   // void controlTimerCallback(const ros::TimerEvent& e);
   void endstopCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg);
 };

vesc_hw_interface/launch/position_test.ros2_control.xacro

@@ -9,8 +9,6 @@
       <param name="num_hall_sensors">3</param>
       <param name="screw_lead">1.0</param>
       <param name="num_rotor_poles">20</param>
-      <param name="upper_limit">0.42</param>
-      <param name="lower_limit">0.0</param>
       <param name="servo/Kp">0.005</param>
       <param name="servo/Ki">0.005</param>
       <param name="servo/Kd">0.0025</param>
@@ -33,7 +31,6 @@
       <param name="servo/endstop_threshold">0.8</param>
       <param name="servo/endstop_margin">0.02</param>
       <param name="command_mode">position</param>
-      <param name="joint_type">prismatic</param>
     </hardware>
     <joint name="vesc_joint">
       <command_interface name="position">

vesc_hw_interface/launch/velocity_duty_test.ros2_control.xacro

@@ -20,7 +20,6 @@
       <param name="motor/smooth_diff/max_sample_sec">0.2</param>
       <param name="motor/smooth_diff/max_smooth_step">10</param>
       <param name="command_mode">velocity_duty</param>
-      <param name="joint_type">continuous</param>
     </hardware>
     <joint name="vesc_joint">
       <command_interface name="position">

vesc_hw_interface/package.xml

@@ -23,6 +23,7 @@
   <depend>joint_limits</depend>
   <depend>pluginlib</depend>
   <depend>std_msgs</depend>
+  <depend>tinyxml2_vendor</depend>
   <depend>urdf</depend>
   <depend>vesc_driver</depend>
 

vesc_hw_interface/src/vesc_hw_interface.cpp

@@ -20,6 +20,8 @@
 #include <hardware_interface/types/hardware_interface_type_values.hpp>
 #include <rclcpp/clock.hpp>
 #include <rclcpp/duration.hpp>
+#include <rclcpp/utilities.hpp>
+#include <tinyxml2.h>
 
 namespace vesc_hw_interface
 {
@@ -42,11 +44,7 @@ CallbackReturn VescHwInterface::on_init(const hardware_interface::HardwareInfo&
   velocity_ = std::numeric_limits<double>::quiet_NaN();
   effort_ = std::numeric_limits<double>::quiet_NaN();
 
-  // initializes the joint name
-  joint_name_ = info_.hardware_parameters["joint_name"];
-
   // initializes commands and states
-  command_ = 0.0;
   position_ = 0.0;
   velocity_ = 0.0;
   effort_ = 0.0;
@@ -92,17 +90,39 @@ CallbackReturn VescHwInterface::on_init(const hardware_interface::HardwareInfo&
   command_mode_ = info_.hardware_parameters["command_mode"];
   RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "mode: %s", command_mode_.data());
 
-  // check joint type
-  joint_type_ = info_.hardware_parameters["joint_type"];
+  const hardware_interface::ComponentInfo& joint = info_.joints[0];
+  joint_name_ = joint.name;
+
+  // parse URDF for joint type
+  auto urdf = info_.original_xml;
+  if (!urdf.empty()) {
+    tinyxml2::XMLDocument doc;
+    if (doc.Parse(urdf.c_str()) != tinyxml2::XML_SUCCESS) {
+      RCLCPP_ERROR_STREAM(get_logger(), "Failed to parse URDF XML");
+      return hardware_interface::CallbackReturn::ERROR;
+    }
+    const tinyxml2::XMLElement * joint_it = doc.RootElement()->FirstChildElement("joint");
+    while (joint_it) {
+      const tinyxml2::XMLAttribute * name_attr = joint_it->FindAttribute("name");
+      const tinyxml2::XMLAttribute * type_attr = joint_it->FindAttribute("type");
+      if (name_attr && type_attr) {
+        std::string name = joint_it->Attribute("name");
+        std::string type = joint_it->Attribute("type");
+        if (name == joint_name_) {
+          joint_type_ = type;
+          break;
+        }
+      }
+      joint_it = joint_it->NextSiblingElement("joint");
+    }
+  }
   RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "joint type: %s", joint_type_.data());
   if ((joint_type_ != "revolute") && (joint_type_ != "continuous") && (joint_type_ != "prismatic"))
   {
     RCLCPP_FATAL(rclcpp::get_logger("VescHwInterface"), "Verify your joint type");
     return CallbackReturn::ERROR;
   }
 
-  const hardware_interface::ComponentInfo& joint = info_.joints[0];
-  joint_name_ = joint.name;
   if (joint.command_interfaces.size() != 3)
   {
     RCLCPP_FATAL(rclcpp::get_logger("VescHwInterface"), "Joint '%s' has %zu command interfaces found. 3 expected.",
@@ -165,20 +185,17 @@ CallbackReturn VescHwInterface::on_configure(const rclcpp_lifecycle::State& /*pr
   if (command_mode_ == hardware_interface::HW_IF_POSITION)
   {
     auto upper_limit = 0.0;
-    if (info_.hardware_parameters.find("upper_limit") != info_.hardware_parameters.end())
-    {
-      upper_limit = std::stod(info_.hardware_parameters["upper_limit"]);
-    }
     auto lower_limit = 0.0;
-    if (info_.hardware_parameters.find("lower_limit") != info_.hardware_parameters.end())
+    // parse URDF for limit parameters
+    auto joint_limit_itr = info_.limits.find(joint_name_);
+    if (joint_limit_itr != info_.limits.end())
     {
-      lower_limit = std::stod(info_.hardware_parameters["lower_limit"]);
+      upper_limit = joint_limit_itr->second.max_position;
+      lower_limit = joint_limit_itr->second.min_position;
+    } else {
+      RCLCPP_WARN(rclcpp::get_logger("VescHwInterface"), "No joint position limits found in URDF, using default limits");
     }
-    // if (joint_limits_.has_position_limits)
-    // {
-    //   upper_limit = joint_limits_.max_position;
-    //   lower_limit = joint_limits_.min_position;
-    // }
+
     // initializes the servo controller
     screw_lead_ = 1.0;
     if (info_.hardware_parameters.find("screw_lead") != info_.hardware_parameters.end())
@@ -190,7 +207,22 @@ CallbackReturn VescHwInterface::on_configure(const rclcpp_lifecycle::State& /*pr
                            joint_type_ == "continuous" ? 1 :
                                                          2,
                            screw_lead_, upper_limit, lower_limit);
-    servo_controller_.spinSensorData();
+    bool calibration = true;
+    if (info_.hardware_parameters.find("servo/calibration") != info_.hardware_parameters.end())
+    {
+      calibration = info_.hardware_parameters["servo/calibration"] == "true";
+    }
+    if (calibration)
+    {
+      while (rclcpp::ok())
+      {
+        vesc_interface_->requestState();
+        servo_controller_.spinSensorData();
+        if (servo_controller_.calibrate())
+          break;
+        rclcpp::sleep_for(std::chrono::milliseconds(10));
+      }
+    }
     position_ = servo_controller_.getPositionSens();
     velocity_ = servo_controller_.getVelocitySens();
     effort_ = servo_controller_.getEffortSens();
@@ -260,8 +292,6 @@ CallbackReturn VescHwInterface::on_activate(const rclcpp_lifecycle::State& /*pre
     velocity_ = 0;
   if (std::isnan(effort_))
     effort_ = 0;
-  if (std::isnan(command_))
-    command_ = 0;
 
   RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "System successfully activated!");
   return CallbackReturn::SUCCESS;
@@ -309,22 +339,27 @@ hardware_interface::return_type VescHwInterface::read(const rclcpp::Time& /*time
 hardware_interface::return_type VescHwInterface::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& period)
 {
   // sends commands
+
+  auto command = command_;
+  if (std::isnan(command) && command_mode_ != "position") {
+    command = 0.0;
+  }
   if (command_mode_ == "position")
   {
     // Limit the speed using the parameters listed in xacro
     // limit_position_interface_.enforceLimits(period);
     // limit_position_handle_.enforceLimits(period);
 
     // executes PID control
-    servo_controller_.setTargetPosition(command_);
+    servo_controller_.setTargetPosition(command);
     servo_controller_.control(1.0 / period.seconds());
   }
   else if (command_mode_ == "velocity")
   {
     // limit_velocity_interface_.enforceLimits(period);
 
     // converts the velocity unit: rad/s or m/s -> rpm -> erpm
-    const double command_rpm = command_ * 60.0 / 2.0 / M_PI / gear_ratio_;
+    const double command_rpm = command * 60.0 / 2.0 / M_PI / gear_ratio_;
     const double command_erpm = command_rpm * static_cast<double>(num_rotor_poles_) / 2;
 
     // sends a reference velocity command
@@ -335,26 +370,26 @@ hardware_interface::return_type VescHwInterface::write(const rclcpp::Time& /*tim
     // limit_velocity_interface_.enforceLimits(period);
 
     // executes PID control
-    wheel_controller_.setTargetVelocity(command_);
+    wheel_controller_.setTargetVelocity(command);
     wheel_controller_.control(1.0 / period.seconds());
   }
   else if (command_mode_ == "effort")
   {
     // limit_effort_interface_.enforceLimits(period);
 
     // converts the command unit: Nm or N -> A
-    const double command_current = command_ * gear_ratio_ / torque_const_;
+    const double command_current = command * gear_ratio_ / torque_const_;
 
     // sends a reference current command
     vesc_interface_->setCurrent(command_current);
   }
   else if (command_mode_ == "effort_duty")
   {
-    command_ = std::max(-1.0, command_);
-    command_ = std::min(1.0, command_);
+    command = std::max(-1.0, command);
+    command = std::min(1.0, command);
 
     // sends a  duty command
-    vesc_interface_->setDutyCycle(command_);
+    vesc_interface_->setDutyCycle(command);
   }
   return hardware_interface::return_type::OK;
 }

vesc_hw_interface/src/vesc_servo_controller.cpp

@@ -21,6 +21,7 @@
 #include <numeric>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/utilities.hpp>
+#include <rclcpp/wait_for_message.hpp>
 
 namespace vesc_hw_interface
 {
@@ -181,6 +182,8 @@ void VescServoController::init(hardware_interface::HardwareInfo& info,
     endstop_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
         "endstop", rclcpp::SensorDataQoS(),
         std::bind(&VescServoController::endstopCallback, this, std::placeholders::_1));
+    std_msgs::msg::Bool endstop_msg;
+    rclcpp::wait_for_message(endstop_msg, node_, "endstop");
     while (endstop_sub_->get_publisher_count() == 0)
     {
       RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Servo Control] Waiting for endstop sensor publisher...");
@@ -256,23 +259,20 @@ void VescServoController::init(hardware_interface::HardwareInfo& info,
   // Create timer callback for PID servo control
   // control_timer_ = nh.createTimer(ros::Duration(1.0 / control_rate_), &VescServoController::controlTimerCallback,
   // this);
+
   return;
 }
 
 void VescServoController::control(const double control_rate)
 {
   if (sensor_initialize_)
     return;
-  // executes calibration
-  if (calibration_flag_)
+
+  if (std::isnan(target_position_))
   {
-    calibrate();
-    // initializes/resets control variables
-    target_position_previous_ = calibration_position_;
+    target_position_ = calibration_position_;
     vesc_step_difference_.resetStepDifference(position_steps_);
-    return;
   }
-
   double error = target_position_ - sens_position_;
   // PID control
   double step_diff = vesc_step_difference_.getStepDifference(position_steps_);
@@ -351,7 +351,7 @@ void VescServoController::control(const double control_rate)
 
 void VescServoController::setTargetPosition(const double position)
 {
-  target_position_ = position;
+  target_position_ = std::clamp(position, lower_endstop_position_, upper_endstop_position_);
 }
 
 void VescServoController::setGearRatio(const double gear_ratio)
@@ -407,10 +407,6 @@ void VescServoController::spinSensorData()
 
 double VescServoController::getPositionSens()
 {
-  if (calibration_flag_)
-  {
-    return calibration_position_;
-  }
   return sens_position_;
 }
 
@@ -432,6 +428,10 @@ void VescServoController::executeCalibration()
 
 bool VescServoController::calibrate()
 {
+  if (!calibration_flag_)
+  {
+    return true;
+  }
   // sends a command for calibration
   if (calibration_mode_ == CURRENT_)
   {
@@ -469,6 +469,7 @@ bool VescServoController::calibrate()
          std::fabs(calibration_duty_ - calibration_strict_duty_) < std::numeric_limits<double>::epsilon()))
     {
       target_position_ = calibration_position_;
+      vesc_step_difference_.resetStepDifference(position_steps_);
       RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "Calibration Finished");
       calibration_flag_ = false;
       return true;
@@ -489,8 +490,9 @@ bool VescServoController::calibrate()
     {
       // finishes calibrating
       calibration_steps_ = 0;
-      zero_position_ = sens_position_ - calibration_position_;
+      zero_position_ = sens_position_ + zero_position_ - calibration_position_;
       target_position_ = calibration_position_;
+      vesc_step_difference_.resetStepDifference(position_steps_);
       RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "Calibration Finished");
       calibration_flag_ = false;
       return true;