sync_read,write以外のDynamixel関係の関数をDynamixelクラスへ移植する (#18)

* position PIDゲインの書き込みをdynamixelクラスへ移植

* 速度PIゲインの書き込みをDynamixelクラスへ移植

* lintエラーの修正

* profile acceleration velocityの書き込みをdynamixelクラスへ移植

* Dynamixelのアドレステーブルに合わせて関数の順番を並び替える

* テストの実行順を変更

* Operating modeの読み書きをDynamixelクラスに移植

* リミット値の読み書きをDynamixelクラスへ移植

* 単位変換関数をdynamixelクラスに移植

Author: Shota Aoki

rt_manipulators_lib/include/dynamixel_base.hpp

@@ -31,9 +31,47 @@ class DynamixelBase {
 
   uint8_t get_id() const { return id_; }
   std::string get_name() const { return name_; }
+
+  virtual bool read_operating_mode(
+    const dynamixel_base::comm_t & comm, uint8_t & mode) { return false; }
+  virtual bool write_operating_mode(
+    const dynamixel_base::comm_t & comm, const uint8_t mode) { return false; }
+  virtual bool read_current_limit(
+    const dynamixel_base::comm_t & comm, double & limit_ampere) { return false; }
+  virtual bool read_max_position_limit(
+    const dynamixel_base::comm_t & comm, double & limit_radian) { return false; }
+  virtual bool read_min_position_limit(
+    const dynamixel_base::comm_t & comm, double & limit_radian) { return false; }
+
   virtual bool write_torque_enable(
     const dynamixel_base::comm_t & comm, const bool enable) { return false; }
 
+  virtual bool write_velocity_i_gain(
+    const dynamixel_base::comm_t & comm, const unsigned int gain) { return false; }
+  virtual bool write_velocity_p_gain(
+    const dynamixel_base::comm_t & comm, const unsigned int gain) { return false; }
+  virtual bool write_position_d_gain(
+    const dynamixel_base::comm_t & comm, const unsigned int gain) { return false; }
+  virtual bool write_position_i_gain(
+    const dynamixel_base::comm_t & comm, const unsigned int gain) { return false; }
+  virtual bool write_position_p_gain(
+    const dynamixel_base::comm_t & comm, const unsigned int gain) { return false; }
+
+  virtual bool write_profile_acceleration(
+    const dynamixel_base::comm_t & comm, const double acceleration_rpss) { return false; }
+  virtual bool write_profile_velocity(
+    const dynamixel_base::comm_t & comm, const double velocity_rps) { return false; }
+
+  virtual unsigned int to_profile_acceleration(const double acceleration_rpss) { return 1; }
+  virtual unsigned int to_profile_velocity(const double velocity_rps) { return 1; }
+  virtual double to_position_radian(const int position) { return 0.0; }
+  virtual double to_velocity_rps(const int velocity) { return 0.0; }
+  virtual double to_current_ampere(const int current) { return 0.0; }
+  virtual double to_voltage_volt(const int voltage) { return 0.0; }
+  virtual unsigned int from_position_radian(const double position_rad) { return 0; }
+  virtual unsigned int from_velocity_rps(const double velocity_rps) { return 0; }
+  virtual unsigned int from_current_ampere(const double current_ampere) { return 0; }
+
  protected:
   uint8_t id_;
   std::string name_;

rt_manipulators_lib/include/dynamixel_xm.hpp

@@ -21,8 +21,39 @@ namespace dynamixel_xm {
 
 class DynamixelXM : public dynamixel_base::DynamixelBase  {
  public:
-  explicit DynamixelXM(const uint8_t id);
+  explicit DynamixelXM(const uint8_t id, const int home_position = 2048);
+
+  bool read_operating_mode(const dynamixel_base::comm_t & comm, uint8_t & mode);
+  bool write_operating_mode(const dynamixel_base::comm_t & comm, const uint8_t mode);
+  bool read_current_limit(const dynamixel_base::comm_t & comm, double & limit_ampere);
+  bool read_max_position_limit(const dynamixel_base::comm_t & comm, double & limit_radian);
+  bool read_min_position_limit(const dynamixel_base::comm_t & comm, double & limit_radian);
+
   bool write_torque_enable(const dynamixel_base::comm_t & comm, const bool enable);
+
+  bool write_velocity_i_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+  bool write_velocity_p_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+  bool write_position_d_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+  bool write_position_i_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+  bool write_position_p_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+
+  bool write_profile_acceleration(
+    const dynamixel_base::comm_t & comm, const double acceleration_rpss);
+  bool write_profile_velocity(
+    const dynamixel_base::comm_t & comm, const double velocity_rps);
+
+  unsigned int to_profile_acceleration(const double acceleration_rpss);
+  unsigned int to_profile_velocity(const double velocity_rps);
+  double to_position_radian(const int position);
+  double to_velocity_rps(const int velocity);
+  double to_current_ampere(const int current);
+  double to_voltage_volt(const int voltage);
+  unsigned int from_position_radian(const double position_rad);
+  unsigned int from_velocity_rps(const double velocity_rps);
+  unsigned int from_current_ampere(const double current_ampere);
+
+ private:
+  int HOME_POSITION_;
 };
 
 }  // namespace dynamixel_xm

rt_manipulators_lib/include/hardware.hpp

@@ -88,8 +88,6 @@ class Hardware {
  protected:
   bool write_word_data_to_group(const std::string& group_name, const uint16_t address,
                                 const uint16_t write_data);
-  bool write_double_word_data_to_group(const std::string& group_name, const uint16_t address,
-                                       const uint32_t write_data);
 
   std::shared_ptr<hardware_communicator::Communicator> comm_;
 
@@ -103,15 +101,6 @@ class Hardware {
                             const uint16_t addr_target, const uint16_t len_target);
   void read_write_thread(const std::vector<std::string>& group_names,
                          const std::chrono::milliseconds& update_cycle_ms);
-  double dxl_pos_to_radian(const int32_t position);
-  double dxl_velocity_to_rps(const int32_t velocity) const;
-  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_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);
 
   hardware_joints::Joints joints_;
   std::map<JointGroupName, uint16_t> addr_sync_read_position_;

rt_manipulators_lib/src/dynamixel_xm.cpp

@@ -12,21 +12,176 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <cmath>
 
 #include "dynamixel_xm.hpp"
 
 
 namespace dynamixel_xm {
 
+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;
+const uint16_t ADDR_VELOCITY_I_GAIN = 76;
+const uint16_t ADDR_VELOCITY_P_GAIN = 78;
+const uint16_t ADDR_POSITION_D_GAIN = 80;
+const uint16_t ADDR_POSITION_I_GAIN = 82;
+const uint16_t ADDR_POSITION_P_GAIN = 84;
+const uint16_t ADDR_PROFILE_ACCELERATION = 108;
+const uint16_t ADDR_PROFILE_VELOCITY = 112;
 
-DynamixelXM::DynamixelXM(const uint8_t id)
-  : dynamixel_base::DynamixelBase(id) {
+const double TO_ACCELERATION_REV_PER_MM = 214.577;
+const double TO_ACCELERATION_TO_RAD_PER_MM = TO_ACCELERATION_REV_PER_MM * 2.0 * M_PI;
+const double TO_ACCELERATION_TO_RAD_PER_SS = TO_ACCELERATION_TO_RAD_PER_MM / 3600.0;
+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_VELOCITY_REV_PER_MIN = 0.229;
+const double TO_VELOCITY_RAD_PER_MIN = TO_VELOCITY_REV_PER_MIN * 2.0 * M_PI;
+const double TO_VELOCITY_RAD_PER_SEC = TO_VELOCITY_RAD_PER_MIN / 60.0;
+const double DXL_VELOCITY_FROM_RAD_PER_SEC = 1.0 / TO_VELOCITY_RAD_PER_SEC;
+const int DXL_MAX_VELOCITY = 32767;
+const double TO_RADIANS = (180.0 / 2048.0) * M_PI / 180.0;
+const double TO_CURRENT_AMPERE = 0.00269;
+const double TO_VOLTAGE_VOLT = 0.1;
+const double TO_DXL_POS = 1.0 / TO_RADIANS;
+const double TO_DXL_CURRENT = 1.0 / TO_CURRENT_AMPERE;
+
+DynamixelXM::DynamixelXM(const uint8_t id, const int home_position)
+  : dynamixel_base::DynamixelBase(id), HOME_POSITION_(home_position) {
   name_ = "XM";
 }
 
+bool DynamixelXM::read_operating_mode(const dynamixel_base::comm_t & comm, uint8_t & mode) {
+  return comm->read_byte_data(id_, ADDR_OPERATING_MODE, mode);
+}
+
+bool DynamixelXM::write_operating_mode(const dynamixel_base::comm_t & comm, const uint8_t mode) {
+  return comm->write_byte_data(id_, ADDR_OPERATING_MODE, mode);
+}
+
+bool DynamixelXM::read_current_limit(
+  const dynamixel_base::comm_t & comm, double & limit_ampere) {
+  uint16_t dxl_current_limit = 0;
+  bool retval = comm->read_word_data(id_, ADDR_CURRENT_LIMIT, dxl_current_limit);
+  limit_ampere = to_current_ampere(dxl_current_limit);
+  return retval;
+}
+
+bool DynamixelXM::read_max_position_limit(
+  const dynamixel_base::comm_t & comm, double & limit_radian) {
+  uint16_t dxl_position_limit = 0;
+  bool retval = comm->read_word_data(id_, ADDR_MAX_POSITION_LIMIT, dxl_position_limit);
+  limit_radian = to_position_radian(dxl_position_limit);
+  return retval;
+}
+
+bool DynamixelXM::read_min_position_limit(
+  const dynamixel_base::comm_t & comm, double & limit_radian) {
+  uint16_t dxl_position_limit = 0;
+  bool retval = comm->read_word_data(id_, ADDR_MIN_POSITION_LIMIT, dxl_position_limit);
+  limit_radian = to_position_radian(dxl_position_limit);
+  return retval;
+}
+
 bool DynamixelXM::write_torque_enable(const dynamixel_base::comm_t & comm, const bool enable) {
   return comm->write_byte_data(id_, ADDR_TORQUE_ENABLE, enable);
 }
 
+bool DynamixelXM::write_velocity_i_gain(
+  const dynamixel_base::comm_t & comm, const unsigned int gain) {
+  return comm->write_word_data(id_, ADDR_VELOCITY_I_GAIN, static_cast<uint16_t>(gain));
+}
+
+bool DynamixelXM::write_velocity_p_gain(
+  const dynamixel_base::comm_t & comm, const unsigned int gain) {
+  return comm->write_word_data(id_, ADDR_VELOCITY_P_GAIN, static_cast<uint16_t>(gain));
+}
+
+bool DynamixelXM::write_position_d_gain(
+  const dynamixel_base::comm_t & comm, const unsigned int gain) {
+  return comm->write_word_data(id_, ADDR_POSITION_D_GAIN, static_cast<uint16_t>(gain));
+}
+
+bool DynamixelXM::write_position_i_gain(
+  const dynamixel_base::comm_t & comm, const unsigned int gain) {
+  return comm->write_word_data(id_, ADDR_POSITION_I_GAIN, static_cast<uint16_t>(gain));
+}
+
+bool DynamixelXM::write_position_p_gain(
+  const dynamixel_base::comm_t & comm, const unsigned int gain) {
+  return comm->write_word_data(id_, ADDR_POSITION_P_GAIN, static_cast<uint16_t>(gain));
+}
+
+bool DynamixelXM::write_profile_acceleration(
+  const dynamixel_base::comm_t & comm, const double acceleration_rpss) {
+  return comm->write_double_word_data(
+    id_,
+    ADDR_PROFILE_ACCELERATION,
+    static_cast<uint32_t>(to_profile_acceleration(acceleration_rpss)));
+}
+
+bool DynamixelXM::write_profile_velocity(
+  const dynamixel_base::comm_t & comm, const double velocity_rps) {
+  return comm->write_double_word_data(
+    id_,
+    ADDR_PROFILE_VELOCITY,
+    static_cast<uint32_t>(to_profile_velocity(velocity_rps)));
+}
+
+unsigned int DynamixelXM::to_profile_acceleration(const double acceleration_rpss) {
+  int dxl_acceleration = DXL_ACCELERATION_FROM_RAD_PER_SS * acceleration_rpss;
+  if (dxl_acceleration > DXL_MAX_ACCELERATION) {
+    dxl_acceleration = DXL_MAX_ACCELERATION;
+  } else if (dxl_acceleration <= 0) {
+    // XMシリーズでは、'0'が最大加速度を意味する
+    // よって、加速度の最小値は'1'である
+    dxl_acceleration = 1;
+  }
+
+  return static_cast<unsigned int>(dxl_acceleration);
+}
+
+unsigned int DynamixelXM::to_profile_velocity(const double velocity_rps) {
+  int dxl_velocity = DXL_VELOCITY_FROM_RAD_PER_SEC * velocity_rps;
+  if (dxl_velocity > DXL_MAX_VELOCITY) {
+    dxl_velocity = DXL_MAX_VELOCITY;
+  } else if (dxl_velocity <= 0) {
+    // XMシリーズでは、'0'が最大速度を意味する
+    // よって、速度の最小値は'1'である
+    dxl_velocity = 1;
+  }
+
+  return static_cast<unsigned int>(dxl_velocity);
+}
+
+double DynamixelXM::to_position_radian(const int position) {
+  return (position - HOME_POSITION_) * TO_RADIANS;
+}
+
+double DynamixelXM::to_velocity_rps(const int velocity) {
+  return velocity * TO_VELOCITY_RAD_PER_SEC;
+}
+
+double DynamixelXM::to_current_ampere(const int current) {
+  return current * TO_CURRENT_AMPERE;
+}
+
+double DynamixelXM::to_voltage_volt(const int voltage) {
+  return voltage * TO_VOLTAGE_VOLT;
+}
+
+unsigned int DynamixelXM::from_position_radian(const double position_rad) {
+  return position_rad * TO_DXL_POS + HOME_POSITION_;
+}
+
+unsigned int DynamixelXM::from_velocity_rps(const double velocity_rps) {
+  return velocity_rps * DXL_VELOCITY_FROM_RAD_PER_SEC;
+}
+
+unsigned int DynamixelXM::from_current_ampere(const double current_ampere) {
+  return current_ampere * TO_DXL_CURRENT;
+}
+
 }  // namespace dynamixel_xm