速度制御機能とサンプルの追加 (#4)

* for-loopをconst 参照に書き換えて、不要なコピーと値の書き換えを防ぐ

* サーボの速度、電流、電圧、温度をsync_readする機能を実装

* READMEの更新

* S17のread_preset_valuesサンプル追加

* ライブラリ：関数名の誤記修正

* サンプル：ID、ジョイント名でパラメータを読み取るサンプルを追加

* Operating Mode切り替え機能を実装。
goal_velocityの書き込みに対応

* goal_velocityを書き込むサンプルを追加

* start_thread()を実行しなければ、目標速度をセットできない

* 角度リミットに達したらgoal_velocityを0にする。
goal_velocityを0にし忘れても良いように、安全な終了処理を実装

* 速度制御時の角度リミット値にマージンを加算できる

* 速度制御時は、positionのsync_readを必須にする

* connect状態のフラグを追加して、disconnect関数の連続実行を防ぐ

* x7_write_velocityを修正

* s17_write_velocityを追加

* READMEを更新

* READMEを更新

* velocity pi gain設定機能を追加

* s17_write_velocityに速度PIDゲインの設定コードを追加

* READMEにvelocity_pi_gainの使い方を追記

* 目次の更新

* 微修正

* constの付け忘れを修正

* READMEにサーボモータのposition limitを確認すること、という文を追記

* max/min_pos_limit_marginをpos_limit_marginに統合

* forearmをwristに変更

* READMEにライブラリとサンプルの変更点を反映

Author: Shota Aoki

rt_manipulators_lib/include/hardware.hpp

@@ -62,6 +62,9 @@ class Hardware {
   bool set_position(const uint8_t id, const double position);
   bool set_position(const std::string& joint_name, const double position);
   bool set_positions(const std::string& group_name, std::vector<double>& positions);
+  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 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);
@@ -71,6 +74,10 @@ class Hardware {
                                const uint16_t d);
   bool write_position_pid_gain_to_group(const std::string& group_name, const uint16_t p,
                                         const uint16_t i, const uint16_t d);
+  bool write_velocity_pi_gain(const uint8_t id, const uint16_t p, const uint16_t i);
+  bool write_velocity_pi_gain(const std::string& joint_name, const uint16_t p, const uint16_t i);
+  bool write_velocity_pi_gain_to_group(const std::string& group_name, const uint16_t p,
+                                       const uint16_t i);
 
  protected:
   bool write_byte_data(const uint8_t id, const uint16_t address, const uint8_t write_data);
@@ -82,12 +89,16 @@ class Hardware {
   bool write_double_word_data(const uint8_t id, const uint16_t address, const uint32_t write_data);
   bool write_double_word_data_to_group(const std::string& group_name, const uint16_t address,
                                        const uint32_t write_data);
+  bool read_byte_data(const uint8_t id, const uint16_t address, uint8_t& read_data);
+  bool read_double_word_data(const uint8_t id, const uint16_t address, uint32_t& read_data);
 
  private:
   bool parse_config_file(const std::string& config_yaml);
   bool joint_groups_contain(const std::string& group_name);
   bool all_joints_contain(const std::string& joint_name);
   bool all_joints_contain_id(const uint8_t id);
+  bool write_operating_mode(const std::string& group_name);
+  bool limit_goal_velocity_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,
@@ -102,8 +113,9 @@ class Hardware {
   double dxl_current_to_ampere(const int16_t current) const;
   double dxl_voltage_to_volt(const int16_t voltage) const;
   uint32_t radian_to_dxl_pos(const double position);
-  uint32_t to_dxl_acceleration(const double acceleration_rpss);
   uint32_t to_dxl_velocity(const double velocity_rps);
+  uint32_t to_dxl_acceleration(const double acceleration_rpss);
+  uint32_t to_dxl_profile_velocity(const double velocity_rps);
 
   std::shared_ptr<dynamixel::PortHandler> port_handler_;
   std::shared_ptr<dynamixel::PacketHandler> packet_handler_;
@@ -117,7 +129,7 @@ class Hardware {
   std::map<JointGroupName, uint16_t> addr_sync_read_current_;
   std::map<JointGroupName, uint16_t> addr_sync_read_voltage_;
   std::map<JointGroupName, uint16_t> addr_sync_read_temperature_;
-  std::map<JointGroupName, uint16_t> address_indirect_goal_position_;
+  bool is_connected_;
   bool thread_enable_;
   std::shared_ptr<std::thread> read_write_thread_;
 };

rt_manipulators_lib/include/joint.hpp

@@ -23,9 +23,12 @@ namespace joint {
 
 class Joint {
  public:
-  Joint(const uint8_t id, const uint8_t operating_mode);
+  Joint(const uint8_t id, const uint8_t operating_mode,
+        const double max_position_limit, const double min_position_limit);
   uint8_t id() const;
   uint8_t operating_mode() const;
+  double max_position_limit() const;
+  double min_position_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);
@@ -37,17 +40,22 @@ class Joint {
   double get_present_voltage() const;
   int8_t get_present_temperature() const;
   void set_goal_position(const double position_radian);
+  void set_goal_velocity(const double velocity_rps);
   double get_goal_position() const;
+  double get_goal_velocity() const;
 
  private:
   uint8_t id_;
   uint8_t operating_mode_;
+  double max_position_limit_;
+  double min_position_limit_;
   double present_position_;
   double present_velocity_;
   double present_current_;
   double present_voltage_;
   int8_t present_temperature_;
   double goal_position_;
+  double goal_velocity_;
 };
 
 class JointGroup {