Dynamixelクラスの追加 (#17)

* Lintでdynamixelディレクトリをチェックする

* dynamixelクラスを追加し、hardwareクラスに組み込む

* ライブラリのREADMEにファイルの説明を追加

* sample01のREADMEのコンフィグファイルにdynamixel情報を追加

* 各コンフィグファイルにDynamixel情報を追加

* ライブラリのバージョンを1.1.0に更新

* includeを修正

* 親ディレクトリのincludeが発生していたため、dynamixelフォルダを削除した

* README修正

* ADDR_TORQUE_ENABLEをhardwareから削除

* 使用していない定数を削除

rt_manipulators_lib/README.md

@@ -70,6 +70,8 @@ $ ./uninstall_library.bash
 - `hardware_communicator.hpp/cpp` : `Hardware`クラスのうち、Dynamixelとの通信機能を実装しています
 - `hardware_joints.hpp/cpp` : `Hardware`クラスのうち、ジョイント情報を扱う機能を実装しています
 - `joints.hpp/cpp` : ジョイント情報を定義しています
+- `config_file_parser.hpp/cpp` : コンフィグファイルの読み取りを担います
+- `dynamixel_*` : 各Dynamixelと通信するためのデータ変換を担います
 
 ### 運動学関連
 

rt_manipulators_lib/include/dynamixel_base.hpp

@@ -0,0 +1,44 @@
+// Copyright 2022 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_BASE_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_BASE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "hardware_communicator.hpp"
+
+namespace dynamixel_base {
+
+using comm_t = std::shared_ptr<hardware_communicator::Communicator>;
+
+class DynamixelBase {
+ public:
+  explicit DynamixelBase(const uint8_t id) : id_(id), name_("base") {}
+  ~DynamixelBase() {}
+
+  uint8_t get_id() const { return id_; }
+  std::string get_name() const { return name_; }
+  virtual bool write_torque_enable(
+    const dynamixel_base::comm_t & comm, const bool enable) { return false; }
+
+ protected:
+  uint8_t id_;
+  std::string name_;
+};
+
+}  // namespace dynamixel_base
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_BASE_HPP_

rt_manipulators_lib/include/dynamixel_xm.hpp

@@ -0,0 +1,30 @@
+// Copyright 2022 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM_HPP_
+
+#include "dynamixel_base.hpp"
+
+namespace dynamixel_xm {
+
+class DynamixelXM : public dynamixel_base::DynamixelBase  {
+ public:
+  explicit DynamixelXM(const uint8_t id);
+  bool write_torque_enable(const dynamixel_base::comm_t & comm, const bool enable);
+};
+
+}  // namespace dynamixel_xm
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM_HPP_

rt_manipulators_lib/include/dynamixel_xm430.hpp

@@ -0,0 +1,29 @@
+// Copyright 2022 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM430_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM430_HPP_
+
+#include "dynamixel_xm.hpp"
+
+namespace dynamixel_xm430 {
+
+class DynamixelXM430 : public dynamixel_xm::DynamixelXM {
+ public:
+  explicit DynamixelXM430(const uint8_t id);
+};
+
+}  // namespace dynamixel_xm430
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM430_HPP_

rt_manipulators_lib/include/dynamixel_xm540.hpp

@@ -0,0 +1,29 @@
+// Copyright 2022 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM540_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM540_HPP_
+
+#include "dynamixel_xm.hpp"
+
+namespace dynamixel_xm540 {
+
+class DynamixelXM540 : public dynamixel_xm::DynamixelXM {
+ public:
+  explicit DynamixelXM540(const uint8_t id);
+};
+
+}  // namespace dynamixel_xm540
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM540_HPP_

rt_manipulators_lib/include/hardware.hpp

@@ -86,8 +86,6 @@ class Hardware {
                                        const uint16_t i);
 
  protected:
-  bool write_byte_data_to_group(const std::string& group_name, const uint16_t address,
-                                const uint8_t write_data);
   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,

rt_manipulators_lib/include/joint.hpp

@@ -16,14 +16,18 @@
 #define RT_MANIPULATORS_LIB_INCLUDE_JOINT_HPP_
 
 #include <cstdint>
+#include <memory>
 #include <string>
 #include <vector>
 
+#include "dynamixel_base.hpp"
+
 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 std::string dynamixel_name);
   uint8_t id() const;
   uint8_t operating_mode() const;
   void set_position_limit_margin(const double position_radian);
@@ -50,6 +54,8 @@ class Joint {
   double get_goal_velocity() const;
   double get_goal_current() const;
 
+  std::shared_ptr<dynamixel_base::DynamixelBase> dxl;
+
  private:
   uint8_t id_;
   uint8_t operating_mode_;

rt_manipulators_lib/src/CMakeLists.txt

@@ -12,8 +12,11 @@ add_library(${library_name}
     kinematics.cpp
     kinematics_utils.cpp
     config_file_parser.cpp
+    dynamixel_xm.cpp
+    dynamixel_xm430.cpp
+    dynamixel_xm540.cpp
 )
-set_target_properties(${library_name} PROPERTIES VERSION 1.0.0 SOVERSION 1)
+set_target_properties(${library_name} PROPERTIES VERSION 1.1.0 SOVERSION 1)
 
 target_include_directories(${library_name} PUBLIC
     ${PROJECT_SOURCE_DIR}/include

rt_manipulators_lib/src/config_file_parser.cpp

@@ -64,16 +64,21 @@ bool parse(const std::string& config_yaml, hardware_joints::Joints & parsed_join
       joint_names.push_back(joint_name);
       auto joint_id = config[joint_name]["id"].as<int>();
       auto ope_mode = config[joint_name]["operating_mode"].as<int>();
+      std::string dynamixel_name = "";
       double position_limit_margin = 0;
       double current_limit_margin = 0;
+
+      if (config[joint_name]["dynamixel"]) {
+        dynamixel_name = config[joint_name]["dynamixel"].as<std::string>();
+      }
       if (config[joint_name]["pos_limit_margin"]) {
         position_limit_margin = config[joint_name]["pos_limit_margin"].as<double>();
       }
       if (config[joint_name]["current_limit_margin"]) {
         current_limit_margin = config[joint_name]["current_limit_margin"].as<double>();
       }
 
-      auto joint = joint::Joint(joint_id, ope_mode);
+      auto joint = joint::Joint(joint_id, ope_mode, dynamixel_name);
       joint.set_position_limit_margin(position_limit_margin);
       joint.set_current_limit_margin(current_limit_margin);
       parsed_joints.append_joint(joint_name, joint);

rt_manipulators_lib/src/dynamixel_xm.cpp

@@ -0,0 +1,32 @@
+// Copyright 2022 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include "dynamixel_xm.hpp"
+
+
+namespace dynamixel_xm {
+
+const uint16_t ADDR_TORQUE_ENABLE = 64;
+
+DynamixelXM::DynamixelXM(const uint8_t id)
+  : dynamixel_base::DynamixelBase(id) {
+  name_ = "XM";
+}
+
+bool DynamixelXM::write_torque_enable(const dynamixel_base::comm_t & comm, const bool enable) {
+  return comm->write_byte_data(id_, ADDR_TORQUE_ENABLE, enable);
+}
+
+}  // namespace dynamixel_xm

rt_manipulators_lib/src/dynamixel_xm430.cpp

@@ -0,0 +1,26 @@
+// Copyright 2022 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include "dynamixel_xm430.hpp"
+
+
+namespace dynamixel_xm430 {
+
+DynamixelXM430::DynamixelXM430(const uint8_t id)
+  : dynamixel_xm::DynamixelXM(id) {
+  name_ = "XM430";
+}
+
+}  // namespace dynamixel_xm430

rt_manipulators_lib/src/dynamixel_xm540.cpp

@@ -0,0 +1,26 @@
+// Copyright 2022 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include "dynamixel_xm540.hpp"
+
+
+namespace dynamixel_xm540 {
+
+DynamixelXM540::DynamixelXM540(const uint8_t id)
+  : dynamixel_xm::DynamixelXM(id) {
+  name_ = "XM540";
+}
+
+}  // namespace dynamixel_xm540

rt_manipulators_lib/src/hardware.cpp

@@ -24,7 +24,6 @@ namespace rt_manipulators_cpp {
 // Ref: https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/
 // Ref: https://emanual.robotis.com/docs/en/dxl/x/xm540-w270/
 // Ref: https://emanual.robotis.com/docs/en/dxl/x/xm540-w150/
-const double PROTOCOL_VERSION = 2.0;
 const int DXL_HOME_POSITION = 2048;
 const double TO_RADIANS = (180.0 / 2048.0) * M_PI / 180.0;
 const double TO_DXL_POS = 1.0 / TO_RADIANS;
@@ -47,7 +46,6 @@ 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;
@@ -194,9 +192,11 @@ void Hardware::disconnect() {
 }
 
 bool Hardware::torque_on(const std::string& group_name) {
-  if (!write_byte_data_to_group(group_name, ADDR_TORQUE_ENABLE, 1)) {
-    std::cerr << group_name << "グループのトルクONに失敗しました." << std::endl;
-    return false;
+  for (const auto & joint_name : joints_.group(group_name)->joint_names()) {
+    if (!joints_.joint(joint_name)->dxl->write_torque_enable(comm_, true)) {
+      std::cerr << joint_name << "ジョイントのトルクONに失敗しました." << std::endl;
+      return false;
+    }
   }
 
   // 安全のため、サーボの現在角度を目標角度に設定する
@@ -212,7 +212,14 @@ bool Hardware::torque_on(const std::string& group_name) {
 }
 
 bool Hardware::torque_off(const std::string& group_name) {
-  return write_byte_data_to_group(group_name, ADDR_TORQUE_ENABLE, 0);
+  bool retval = true;
+  for (const auto & joint_name : joints_.group(group_name)->joint_names()) {
+    if (!joints_.joint(joint_name)->dxl->write_torque_enable(comm_, false)) {
+      std::cerr << joint_name << "ジョイントのトルクOFFに失敗しました." << std::endl;
+      retval = false;
+    }
+  }
+  return retval;
 }
 
 bool Hardware::sync_read(const std::string& group_name) {
@@ -709,23 +716,6 @@ bool Hardware::write_velocity_pi_gain_to_group(const std::string& group_name, co
   return true;
 }
 
-bool Hardware::write_byte_data_to_group(const std::string& group_name, const uint16_t address,
-                                        const uint8_t write_data) {
-  if (!joints_.has_group(group_name)) {
-    std::cerr << group_name << "はjoint_groupsに存在しません." << std::endl;
-    return false;
-  }
-
-  bool retval = true;
-  for (const auto & joint_name : joints_.group(group_name)->joint_names()) {
-    auto id = joints_.joint(joint_name)->id();
-    if (!comm_->write_byte_data(id, address, write_data)) {
-      retval = false;
-    }
-  }
-  return retval;
-}
-
 bool Hardware::write_word_data_to_group(const std::string& group_name, const uint16_t address,
                                         const uint16_t write_data) {
   if (!joints_.has_group(group_name)) {