電流制御機能とサンプルの追加 (#7)

* 電流制御機能の実装。X7の電流制御サンプルの実装

* 電流制限機能を実装

* set_currentsの修正。current_limitが0以上の値になるように修正

* load_config_file内で変更後のposition_limitとcurrent_limitを表示する

* コンフィグファイルの調整

* コンフィグファイル読み込み時に、current writeとposition readの設定をチェックする

* S17のwrite_currentサンプル追加

* READMEに電流制御の説明を追記

* README修正

* READMEの微修正

* Jointのcurrent_limitをcurrent_limit_when_position_exceeds_limitに変更

* current limitに到達したら制限電流値を表示する

* READMEを微修正

Author: Shota Aoki

rt_manipulators_lib/include/hardware.hpp

@@ -66,6 +66,9 @@ class Hardware {
   bool set_velocity(const uint8_t id, const double velocity);
   bool set_velocity(const std::string& joint_name, const double velocity);
   bool set_velocities(const std::string& group_name, std::vector<double>& velocities);
+  bool set_current(const uint8_t id, const double current);
+  bool set_current(const std::string& joint_name, const double current);
+  bool set_currents(const std::string& group_name, std::vector<double>& currents);
   bool write_max_acceleration_to_group(const std::string& group_name,
                                        const double acceleration_rpss);
   bool write_max_velocity_to_group(const std::string& group_name, const double velocity_rps);
@@ -94,6 +97,7 @@ class Hardware {
   bool parse_config_file(const std::string& config_yaml);
   bool write_operating_mode(const std::string& group_name);
   bool limit_goal_velocity_by_present_position(const std::string& group_name);
+  bool limit_goal_current_by_present_position(const std::string& group_name);
   bool create_sync_read_group(const std::string& group_name);
   bool create_sync_write_group(const std::string& group_name);
   bool set_indirect_address(const std::string& group_name, const uint16_t addr_indirect_start,
@@ -106,6 +110,7 @@ class Hardware {
   double dxl_voltage_to_volt(const int16_t voltage) const;
   uint32_t radian_to_dxl_pos(const double position);
   uint32_t to_dxl_velocity(const double velocity_rps);
+  uint16_t to_dxl_current(const double current_ampere);
   uint32_t to_dxl_acceleration(const double acceleration_rpss);
   uint32_t to_dxl_profile_velocity(const double velocity_rps);
 

rt_manipulators_lib/include/hardware_communicator.hpp

@@ -64,6 +64,7 @@ class Communicator{
   bool write_double_word_data(const dxl_id_t & id, const dxl_address_t & address,
                               const dxl_double_word_t & write_data);
   bool read_byte_data(const dxl_id_t & id, const dxl_address_t & address, dxl_byte_t & read_data);
+  bool read_word_data(const dxl_id_t & id, const dxl_address_t & address, dxl_word_t & read_data);
   bool read_double_word_data(const dxl_id_t & id, const dxl_address_t & address,
                              dxl_double_word_t & read_data);
 

rt_manipulators_lib/include/hardware_joints.hpp

@@ -70,6 +70,9 @@ class Joints{
   bool set_velocity(const dxl_id_t & id, const velocity_t & velocity);
   bool set_velocity(const joint_name_t & joint_name, const velocity_t & velocity);
   bool set_velocities(const group_name_t & group_name, const std::vector<velocity_t>& velocities);
+  bool set_current(const dxl_id_t & id, const current_t & current);
+  bool set_current(const joint_name_t & joint_name, const current_t & current);
+  bool set_currents(const group_name_t & group_name, const std::vector<current_t>& currents);
 
  private:
   group_map_t joint_groups_;

rt_manipulators_lib/include/joint.hpp

@@ -24,11 +24,13 @@ namespace joint {
 class Joint {
  public:
   Joint(const uint8_t id, const uint8_t operating_mode,
-        const double max_position_limit, const double min_position_limit);
+        const double max_position_limit, const double min_position_limit,
+        const double current_limit_when_position_exceeds_limit);
   uint8_t id() const;
   uint8_t operating_mode() const;
   double max_position_limit() const;
   double min_position_limit() const;
+  double current_limit_when_position_exceeds_limit() const;
   void set_present_position(const double position_radian);
   void set_present_velocity(const double velocity_rps);
   void set_present_current(const double current_ampere);
@@ -41,21 +43,25 @@ class Joint {
   int8_t get_present_temperature() const;
   void set_goal_position(const double position_radian);
   void set_goal_velocity(const double velocity_rps);
+  void set_goal_current(const double current_ampere);
   double get_goal_position() const;
   double get_goal_velocity() const;
+  double get_goal_current() const;
 
  private:
   uint8_t id_;
   uint8_t operating_mode_;
   double max_position_limit_;
   double min_position_limit_;
+  double current_limit_when_position_exceeds_limit_;
   double present_position_;
   double present_velocity_;
   double present_current_;
   double present_voltage_;
   int8_t present_temperature_;
   double goal_position_;
   double goal_velocity_;
+  double goal_current_;
 };
 
 class JointGroup {

rt_manipulators_lib/src/hardware.cpp

@@ -39,10 +39,12 @@ const double TO_ACCELERATION_TO_RAD_PER_SS = TO_ACCELERATION_TO_RAD_PER_MM / 360
 const double DXL_ACCELERATION_FROM_RAD_PER_SS = 1.0 / TO_ACCELERATION_TO_RAD_PER_SS;
 const int DXL_MAX_ACCELERATION = 32767;
 const double TO_CURRENT_AMPERE = 0.00269;
+const double TO_DXL_CURRENT = 1.0 / TO_CURRENT_AMPERE;
 const double TO_VOLTAGE_VOLT = 0.1;
 
 // Dynamixel XM Series address table
 const uint16_t ADDR_OPERATING_MODE = 11;
+const uint16_t ADDR_CURRENT_LIMIT = 38;
 const uint16_t ADDR_MAX_POSITION_LIMIT = 48;
 const uint16_t ADDR_MIN_POSITION_LIMIT = 52;
 const uint16_t ADDR_TORQUE_ENABLE = 64;
@@ -100,6 +102,12 @@ bool Hardware::load_config_file(const std::string& config_yaml) {
       std::cerr << "positionもsync_readするようにコンフィグファイルを修正して下さい." << std::endl;
       return false;
     }
+    if (group->sync_write_current_enabled() && !group->sync_read_position_enabled()) {
+      std::cerr << group_name << "グループはcurrentをsync_writeしますが, ";
+      std::cerr << "positionをsync_readしません." << std::endl;
+      std::cerr << "positionもsync_readするようにコンフィグファイルを修正して下さい." << std::endl;
+      return false;
+    }
   }
 
   std::cout << "Config file '" << config_yaml << "' loaded." << std::endl;
@@ -109,6 +117,12 @@ bool Hardware::load_config_file(const std::string& config_yaml) {
       std::cout << "\t" << joint_name;
       std::cout << ", id:" << std::to_string(joints_.joint(joint_name)->id());
       std::cout << ", mode:" << std::to_string(joints_.joint(joint_name)->operating_mode());
+      std::cout << ", modified max_position_limit:" << std::to_string(
+        joints_.joint(joint_name)->max_position_limit());
+      std::cout << ", modified min_position_limit:" << std::to_string(
+        joints_.joint(joint_name)->min_position_limit());
+      std::cout << ", current_limit_when_position_exceeds_limit:" << std::to_string(
+        joints_.joint(joint_name)->current_limit_when_position_exceeds_limit());
       std::cout << std::endl;
     }
   }
@@ -129,8 +143,8 @@ void Hardware::disconnect() {
     return;
   }
 
-  // 速度指示モードの場合は、goal_velocityを0にする
   for (const auto& [group_name, group] : joints_.groups()) {
+    // 速度指示モードの場合は、goal_velocityを0にする
     if (group->sync_write_velocity_enabled()) {
       std::cout << group_name << "グループにはvelocityのsync_writeが設定されています." << std::endl;
       std::cout << "安全のため, disconnect()関数内で目標速度 0 rad/sを書き込みます." << std::endl;
@@ -139,6 +153,15 @@ void Hardware::disconnect() {
       }
       sync_write(group_name);
     }
+    // 電流指示モードの場合は、goal_currentを0にする
+    if (group->sync_write_current_enabled()) {
+      std::cout << group_name << "グループにはcurrentのsync_writeが設定されています." << std::endl;
+      std::cout << "安全のため, disconnect()関数内で目標速度 0 Aを書き込みます." << std::endl;
+      for (const auto & joint_name : group->joint_names()) {
+          joints_.joint(joint_name)->set_goal_current(0);
+      }
+      sync_write(group_name);
+    }
   }
 
   comm_->disconnect();
@@ -282,6 +305,12 @@ bool Hardware::sync_write(const std::string& group_name) {
       write_data.push_back(DXL_HIBYTE(DXL_HIWORD(goal_velocity)));
     }
 
+    if (joints_.group(group_name)->sync_write_current_enabled()) {
+      uint16_t goal_current = to_dxl_current(joints_.joint(joint_name)->get_goal_current());
+      write_data.push_back(DXL_LOBYTE(goal_current));
+      write_data.push_back(DXL_HIBYTE(goal_current));
+    }
+
     auto id = joints_.joint(joint_name)->id();
     if (!comm_->set_sync_write_data(group_name, id, write_data)) {
       return false;
@@ -335,8 +364,8 @@ bool Hardware::stop_thread() {
   // リソースを解放
   read_write_thread_.reset();
 
-  // 速度指示モードの場合は、goal_velocityを0にする
   for (const auto& [group_name, group] : joints_.groups()) {
+    // 速度指示モードの場合は、goal_velocityを0にする
     if (group->sync_write_velocity_enabled()) {
       std::cout << group_name << "グループにはvelocityのsync_writeが設定されています." << std::endl;
       std::cout << "安全のため, stop_thread()関数内で目標速度 0 rad/sを書き込みます." << std::endl;
@@ -345,6 +374,15 @@ bool Hardware::stop_thread() {
       }
       sync_write(group_name);
     }
+    // 電流指示モードの場合は、goal_currentを0にする
+    if (group->sync_write_current_enabled()) {
+      std::cout << group_name << "グループにはcurrentのsync_writeが設定されています." << std::endl;
+      std::cout << "安全のため, stop_thread()関数内で目標電流 0 Aを書き込みます." << std::endl;
+      for (const auto & joint_name : group->joint_names()) {
+          joints_.joint(joint_name)->set_goal_current(0);
+      }
+      sync_write(group_name);
+    }
   }
 
   return true;
@@ -452,6 +490,36 @@ bool Hardware::set_velocities(const std::string& group_name, std::vector<double>
   return joints_.set_velocities(group_name, velocities);
 }
 
+bool Hardware::set_current(const uint8_t id, const double current) {
+  if (!thread_enable_) {
+    std::cerr << "目標電流を書き込む場合は、";
+    std::cerr << "安全のためstart_thread()を実行してください." << std::endl;
+    return false;
+  }
+
+  return joints_.set_current(id, current);
+}
+
+bool Hardware::set_current(const std::string& joint_name, const double current) {
+  if (!thread_enable_) {
+    std::cerr << "目標電流を書き込む場合は、";
+    std::cerr << "安全のためstart_thread()を実行してください." << std::endl;
+    return false;
+  }
+
+  return joints_.set_current(joint_name, current);
+}
+
+bool Hardware::set_currents(const std::string& group_name, std::vector<double>& currents) {
+  if (!thread_enable_) {
+    std::cerr << "目標電流を書き込む場合は、";
+    std::cerr << "安全のためstart_thread()を実行してください." << std::endl;
+    return false;
+  }
+
+  return joints_.set_currents(group_name, currents);
+}
+
 bool Hardware::write_max_acceleration_to_group(const std::string& group_name,
                                                const double acceleration_rpss) {
   // 指定されたグループ内のサーボモータの最大動作加速度（radian / s^2）を設定する
@@ -703,6 +771,7 @@ bool Hardware::parse_config_file(const std::string& config_yaml) {
 
       uint32_t dxl_max_pos_limit = 0;
       uint32_t dxl_min_pos_limit = 0;
+      uint16_t dxl_current_limit = 0;
       if (!comm_->read_double_word_data(joint_id, ADDR_MAX_POSITION_LIMIT, dxl_max_pos_limit)) {
         std::cerr << joint_name << "のMax Position Limitの読み取りに失敗しました." << std::endl;
         return false;
@@ -711,6 +780,10 @@ bool Hardware::parse_config_file(const std::string& config_yaml) {
         std::cerr << joint_name << "のMin Position Limitの読み取りに失敗しました." << std::endl;
         return false;
       }
+      if (!comm_->read_word_data(joint_id, ADDR_CURRENT_LIMIT, dxl_current_limit)) {
+        std::cerr << joint_name << "のCurrent Limitの読み取りに失敗しました." << std::endl;
+        return false;
+      }
 
       // 角度リミット値をラジアンに変換
       double max_position_limit = dxl_pos_to_radian(static_cast<int32_t>(dxl_max_pos_limit));
@@ -722,7 +795,18 @@ bool Hardware::parse_config_file(const std::string& config_yaml) {
         min_position_limit += config[joint_name]["pos_limit_margin"].as<double>();
       }
 
-      auto joint = joint::Joint(joint_id, ope_mode, max_position_limit, min_position_limit);
+      // 電流リミット値をアンペアに変換
+      double current_limit = dxl_current_to_ampere(dxl_current_limit);
+      // 電流リミット値にマージンを加算する
+      if (config[joint_name]["current_limit_margin"]) {
+        // current_limitは0以上の値にする
+        current_limit = std::max(
+          current_limit - config[joint_name]["current_limit_margin"].as<double>(),
+          0.0);
+      }
+
+      auto joint = joint::Joint(
+        joint_id, ope_mode, max_position_limit, min_position_limit, current_limit);
       joints_.append_joint(joint_name, joint);
     }
     std::vector<std::string> sync_read_targets;
@@ -815,6 +899,33 @@ bool Hardware::limit_goal_velocity_by_present_position(const std::string& group_
   return retval;
 }
 
+bool Hardware::limit_goal_current_by_present_position(const std::string& group_name) {
+  // ジョイントの現在角度がmax/min position limit を超えた場合、
+  // goal_currentをcurrent limitに制限する
+  bool retval = true;
+  for (const auto & joint_name : joints_.group(group_name)->joint_names()) {
+    auto max_position_limit = joints_.joint(joint_name)->max_position_limit();
+    auto min_position_limit = joints_.joint(joint_name)->min_position_limit();
+    auto current_limit = joints_.joint(joint_name)->current_limit_when_position_exceeds_limit();
+    auto present_position = joints_.joint(joint_name)->get_present_position();
+    auto goal_current = joints_.joint(joint_name)->get_goal_current();
+    bool has_exceeded_max_pos_limit = present_position > max_position_limit &&
+      goal_current > current_limit;
+    bool has_exceeded_min_pos_limit = present_position < min_position_limit &&
+      goal_current < -current_limit;
+
+    if (has_exceeded_max_pos_limit || has_exceeded_min_pos_limit) {
+      std::cout << joint_name << "ジョイントの現在角度が限界角度に到達しました、";
+      std::cout << "goal_currentを" << current_limit << " Aに制限します." << std::endl;
+      auto limited_current = std::clamp(goal_current, -current_limit, current_limit);
+      joints_.joint(joint_name)->set_goal_current(limited_current);
+      retval = false;
+    }
+  }
+
+  return retval;
+}
+
 bool Hardware::create_sync_read_group(const std::string& group_name) {
   // HardwareCommunicatorに、指定されたデータを読むSyncReadGroupを追加する
   // できるだけ多くのデータをSyncReadで読み取るため、インダイレクトアドレスを活用する
@@ -909,6 +1020,12 @@ bool Hardware::create_sync_write_group(const std::string& group_name) {
     }
   }
 
+  if (joints_.group(group_name)->sync_write_current_enabled()) {
+    if (!append(ADDR_GOAL_CURRENT, LEN_GOAL_CURRENT, "goal_current")) {
+      return false;
+    }
+  }
+
   comm_->make_sync_write_group(group_name, ADDR_INDIRECT_DATA_29, total_length);
 
   std::vector<uint8_t> init_data(total_length, 0);
@@ -957,6 +1074,9 @@ void Hardware::read_write_thread(const std::vector<std::string>& group_names,
       if (joints_.group(group_name)->sync_write_velocity_enabled()) {
         limit_goal_velocity_by_present_position(group_name);
       }
+      if (joints_.group(group_name)->sync_write_current_enabled()) {
+        limit_goal_current_by_present_position(group_name);
+      }
       sync_write(group_name);
     }
 
@@ -988,6 +1108,10 @@ uint32_t Hardware::to_dxl_velocity(const double velocity_rps) {
   return velocity_rps * DXL_VELOCITY_FROM_RAD_PER_SEC;
 }
 
+uint16_t Hardware::to_dxl_current(const double current_ampere) {
+  return current_ampere * TO_DXL_CURRENT;
+}
+
 uint32_t Hardware::to_dxl_acceleration(const double acceleration_rpss) {
   // 加速度 rad/s^2をDYNAMIXELのデータに変換する
 

rt_manipulators_lib/src/hardware_communicator.cpp

@@ -194,6 +194,20 @@ bool Communicator::read_byte_data(
   return true;
 }
 
+bool Communicator::read_word_data(
+  const dxl_id_t & id, const dxl_address_t & address, dxl_word_t & read_data) {
+  dxl_error_t dxl_error = 0;
+  dxl_word_t data = 0;
+  dxl_result_t dxl_result =
+      packet_handler_->read2ByteTxRx(port_handler_.get(), id, address, &data, &dxl_error);
+
+  if (!parse_dxl_error(std::string(__func__), id, address, dxl_result, dxl_error)) {
+    return false;
+  }
+  read_data = data;
+  return true;
+}
+
 bool Communicator::read_double_word_data(
   const dxl_id_t & id, const dxl_address_t & address, dxl_double_word_t & read_data) {
   dxl_error_t dxl_error = 0;

rt_manipulators_lib/src/hardware_joints.cpp

@@ -291,4 +291,44 @@ bool Joints::set_velocities(
   return true;
 }
 
+bool Joints::set_current(const dxl_id_t & id, const current_t & current) {
+  if (!has_joint(id)) {
+    std::cerr << "ID:" << std::to_string(id) << "のジョイントは存在しません." << std::endl;
+    return false;
+  }
+
+  joint(id)->set_goal_current(current);
+  return true;
+}
+
+bool Joints::set_current(const joint_name_t & joint_name, const current_t & current) {
+  if (!has_joint(joint_name)) {
+    std::cerr << joint_name << "ジョイントは存在しません." << std::endl;
+    return false;
+  }
+
+  joint(joint_name)->set_goal_current(current);
+  return true;
+}
+
+bool Joints::set_currents(const group_name_t & group_name, const std::vector<current_t>& currents) {
+  if (!has_group(group_name)) {
+    std::cerr << group_name << "はjoint_groupsに存在しません." << std::endl;
+    return false;
+  }
+
+  if (joint_groups_.at(group_name)->joint_names().size() != currents.size()) {
+    std::cerr << "目標値のサイズ:" << currents.size();
+    std::cerr << "がジョイント数:" << joint_groups_.at(group_name)->joint_names().size();
+    std::cerr << "と一致しません." << std::endl;
+    return false;
+  }
+
+  for (size_t i = 0; i < currents.size(); i++) {
+    auto joint_name = joint_groups_.at(group_name)->joint_names()[i];
+    joint(joint_name)->set_goal_current(currents[i]);
+  }
+  return true;
+}
+
 }  // namespace hardware_joints