Dynamixelクラスの追加 (#17)

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

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

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

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

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

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

* includeを修正

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

* README修正

* ADDR_TORQUE_ENABLEをhardwareから削除

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

Author: Shota Aoki

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