muriqui用のコードを移植する

rt_manipulators_lib/CMakeLists.txt

@@ -27,12 +27,14 @@ add_library(${library_name}
     src/kinematics_utils.cpp
     src/config_file_parser.cpp
     src/dynamixel_x.cpp
+    src/dynamixel_xc330.cpp
     src/dynamixel_xm430.cpp
     src/dynamixel_xm540.cpp
     src/dynamixel_xh430.cpp
     src/dynamixel_xh540.cpp
     src/dynamixel_p.cpp
     src/dynamixel_ph42.cpp
+    src/dynamixel_ph54.cpp
 )
 target_include_directories(${library_name}
     PUBLIC

rt_manipulators_lib/include/dynamixel_base.hpp

@@ -69,6 +69,7 @@ class DynamixelBase {
   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 double to_analog_voltage_volt(const int input) { 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; }
@@ -89,6 +90,8 @@ class DynamixelBase {
     const dynamixel_base::comm_t & comm) { return false; }
   virtual bool auto_set_indirect_address_of_goal_current(
     const dynamixel_base::comm_t & comm) { return false; }
+  virtual bool auto_set_indirect_address_of_external_port(
+    const dynamixel_base::comm_t & comm, const int number) { return false; }
 
   virtual unsigned int indirect_addr_of_present_position(void) { return 0; }
   virtual unsigned int indirect_addr_of_present_velocity(void) { return 0; }
@@ -98,6 +101,9 @@ class DynamixelBase {
   virtual unsigned int indirect_addr_of_goal_position(void) { return 0; }
   virtual unsigned int indirect_addr_of_goal_velocity(void) { return 0; }
   virtual unsigned int indirect_addr_of_goal_current(void) { return 0; }
+  virtual unsigned int indirect_addr_of_external_port(const int number) { return 0; }
+
+  virtual unsigned int series_addr_of_external_port(const int number) { return 0; }
 
   virtual unsigned int start_address_for_indirect_read(void) { return 0; }
   virtual unsigned int length_of_indirect_data_read(void) { return 0; }
@@ -107,6 +113,14 @@ class DynamixelBase {
   virtual unsigned int length_of_indirect_data_write(void) { return 0; }
   virtual unsigned int next_indirect_addr_write(void) const { return 0; }
 
+  virtual unsigned int start_address_for_direct_read(void) { return 0; }
+  virtual unsigned int length_of_direct_data_read(void) { return 0; }
+  virtual unsigned int next_direct_addr_read(void) const { return 0; }
+
+  virtual unsigned int start_address_for_direct_write(void) { return 0; }
+  virtual unsigned int length_of_direct_data_write(void) { return 0; }
+  virtual unsigned int next_direct_addr_write(void) const { return 0; }
+
   virtual bool extract_present_position_from_sync_read(
     const dynamixel_base::comm_t & comm, const std::string & group_name,
     double & position_rad) { return false; }
@@ -122,14 +136,39 @@ class DynamixelBase {
   virtual bool extract_present_temperature_from_sync_read(
     const dynamixel_base::comm_t & comm, const std::string & group_name,
     int & temperature_deg) { return false; }
+  virtual bool extract_external_port_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    const int number, double & analog_voltage_volt ) { return false; }
+
+  virtual bool extract_default_position_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & position_rad) { return false; }
+  virtual bool extract_default_velocity_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & velocity_rps) { return false; }
+  virtual bool extract_default_current_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & current_ampere) { return false; }
+  virtual bool extract_default_input_voltage_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & voltage_volt) { return false; }
+  virtual bool extract_default_temperature_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    int & temperature_deg) { return false; }
 
   virtual void push_back_position_for_sync_write(
     const double position_rad, std::vector<uint8_t> & write_data) {}
+  virtual void push_back_profile_for_sync_write(
+    const double profile_rps, std::vector<uint8_t> & write_data) {}
   virtual void push_back_velocity_for_sync_write(
     const double velocity_rps, std::vector<uint8_t> & write_data) {}
   virtual void push_back_current_for_sync_write(
     const double current_ampere, std::vector<uint8_t> & write_data) {}
 
+
+  virtual bool set_external_port_mode_to_analog_input(
+    const dynamixel_base::comm_t & comm, const int number) { return false; }
+
  protected:
   uint8_t id_;
   std::string name_;

rt_manipulators_lib/include/dynamixel_p.hpp

@@ -51,6 +51,7 @@ class DynamixelP : public dynamixel_base::DynamixelBase  {
   double to_velocity_rps(const int velocity);
   double to_current_ampere(const int current);
   double to_voltage_volt(const int voltage);
+  double to_analog_voltage_volt(const int input);
   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);
@@ -63,6 +64,8 @@ class DynamixelP : public dynamixel_base::DynamixelBase  {
   bool auto_set_indirect_address_of_goal_position(const dynamixel_base::comm_t & comm);
   bool auto_set_indirect_address_of_goal_velocity(const dynamixel_base::comm_t & comm);
   bool auto_set_indirect_address_of_goal_current(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_external_port(
+    const dynamixel_base::comm_t & comm, const int number);
 
   unsigned int indirect_addr_of_present_position(void);
   unsigned int indirect_addr_of_present_velocity(void);
@@ -72,6 +75,7 @@ class DynamixelP : public dynamixel_base::DynamixelBase  {
   unsigned int indirect_addr_of_goal_position(void);
   unsigned int indirect_addr_of_goal_velocity(void);
   unsigned int indirect_addr_of_goal_current(void);
+  unsigned int indirect_addr_of_external_port(const int number);
 
   unsigned int start_address_for_indirect_read(void);
   unsigned int length_of_indirect_data_read(void);
@@ -96,6 +100,9 @@ class DynamixelP : public dynamixel_base::DynamixelBase  {
   bool extract_present_temperature_from_sync_read(
     const dynamixel_base::comm_t & comm, const std::string & group_name,
     int & temperature_deg);
+  bool extract_external_port_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    const int number, double & analog_voltage_volt);
 
   void push_back_position_for_sync_write(
     const double position_rad, std::vector<uint8_t> & write_data);
@@ -104,6 +111,9 @@ class DynamixelP : public dynamixel_base::DynamixelBase  {
   void push_back_current_for_sync_write(
     const double current_ampere, std::vector<uint8_t> & write_data);
 
+  bool set_external_port_mode_to_analog_input(
+    const dynamixel_base::comm_t & comm, const int number);
+
  protected:
   int HOME_POSITION_;
   unsigned int total_length_of_indirect_addr_read_;
@@ -116,6 +126,10 @@ class DynamixelP : public dynamixel_base::DynamixelBase  {
   uint16_t indirect_addr_of_goal_position_;
   uint16_t indirect_addr_of_goal_velocity_;
   uint16_t indirect_addr_of_goal_current_;
+  uint16_t indirect_addr_of_external_port1_ = 0;
+  uint16_t indirect_addr_of_external_port2_ = 0;
+  uint16_t indirect_addr_of_external_port3_ = 0;
+  uint16_t indirect_addr_of_external_port4_ = 0;
 
   bool set_indirect_address_read(
     const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len,

rt_manipulators_lib/include/dynamixel_ph54.hpp

@@ -0,0 +1,32 @@
+// Copyright 2024 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_PH54_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH54_HPP_
+
+#include "dynamixel_p.hpp"
+
+namespace dynamixel_ph54 {
+
+class DynamixelPH54 : public dynamixel_p::DynamixelP {
+ public:
+  explicit DynamixelPH54(const uint8_t id);
+  unsigned int to_profile_acceleration(const double acceleration_rpss) override;
+  double to_position_radian(const int position) override;
+  unsigned int from_position_radian(const double position_rad) override;
+};
+
+}  // namespace dynamixel_ph54
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH54_HPP_

rt_manipulators_lib/include/dynamixel_x.hpp

@@ -51,6 +51,7 @@ class DynamixelX : public dynamixel_base::DynamixelBase  {
   double to_velocity_rps(const int velocity);
   double to_current_ampere(const int current);
   double to_voltage_volt(const int voltage);
+  double to_analog_voltage_volt(const int input);
   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);
@@ -63,6 +64,19 @@ class DynamixelX : public dynamixel_base::DynamixelBase  {
   bool auto_set_indirect_address_of_goal_position(const dynamixel_base::comm_t & comm);
   bool auto_set_indirect_address_of_goal_velocity(const dynamixel_base::comm_t & comm);
   bool auto_set_indirect_address_of_goal_current(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_external_port(
+    const dynamixel_base::comm_t & comm, const int number);
+
+  bool auto_set_series_address_of_present_position(const dynamixel_base::comm_t & comm);
+  bool auto_set_series_address_of_present_velocity(const dynamixel_base::comm_t & comm);
+  bool auto_set_series_address_of_present_current(const dynamixel_base::comm_t & comm);
+  bool auto_set_series_address_of_present_input_voltage(const dynamixel_base::comm_t & comm);
+  bool auto_set_series_address_of_present_temperature(const dynamixel_base::comm_t & comm);
+  bool auto_set_series_address_of_goal_position(const dynamixel_base::comm_t & comm);
+  bool auto_set_series_address_of_goal_velocity(const dynamixel_base::comm_t & comm);
+  bool auto_set_series_address_of_goal_current(const dynamixel_base::comm_t & comm);
+  bool auto_set_series_address_of_external_port(
+    const dynamixel_base::comm_t & comm, const int number);
 
   unsigned int indirect_addr_of_present_position(void);
   unsigned int indirect_addr_of_present_velocity(void);
@@ -72,6 +86,7 @@ class DynamixelX : public dynamixel_base::DynamixelBase  {
   unsigned int indirect_addr_of_goal_position(void);
   unsigned int indirect_addr_of_goal_velocity(void);
   unsigned int indirect_addr_of_goal_current(void);
+  unsigned int indirect_addr_of_external_port(const int number);
 
   unsigned int start_address_for_indirect_read(void);
   unsigned int length_of_indirect_data_read(void);
@@ -81,6 +96,14 @@ class DynamixelX : public dynamixel_base::DynamixelBase  {
   unsigned int length_of_indirect_data_write(void);
   unsigned int next_indirect_addr_write(void) const;
 
+  unsigned int start_address_for_direct_read(void);
+  unsigned int length_of_direct_data_read(void);
+  unsigned int next_direct_addr_read(void) const;
+
+  unsigned int start_address_for_direct_write(void);
+  unsigned int length_of_direct_data_write(void);
+  unsigned int next_direct_addr_write(void) const;
+
   bool extract_present_position_from_sync_read(
     const dynamixel_base::comm_t & comm, const std::string & group_name,
     double & position_rad);
@@ -96,18 +119,44 @@ class DynamixelX : public dynamixel_base::DynamixelBase  {
   bool extract_present_temperature_from_sync_read(
     const dynamixel_base::comm_t & comm, const std::string & group_name,
     int & temperature_deg);
+  bool extract_external_port_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    const int number, double & analog_voltage_volt);
+
+  bool extract_default_position_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & position_rad);
+  bool extract_default_velocity_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & velocity_rps);
+  bool extract_default_current_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & current_ampere);
+  bool extract_default_input_voltage_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & voltage_volt);
+  bool extract_default_temperature_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    int & temperature_deg);
 
   void push_back_position_for_sync_write(
     const double position_rad, std::vector<uint8_t> & write_data);
+  void push_back_profile_for_sync_write(
+    const double profile_rps, std::vector<uint8_t> & write_data);
   void push_back_velocity_for_sync_write(
     const double velocity_rps, std::vector<uint8_t> & write_data);
   void push_back_current_for_sync_write(
     const double current_ampere, std::vector<uint8_t> & write_data);
 
+  bool set_external_port_mode_to_analog_input(
+    const dynamixel_base::comm_t & comm, const int number);
+
  protected:
   int HOME_POSITION_;
   unsigned int total_length_of_indirect_addr_read_;
   unsigned int total_length_of_indirect_addr_write_;
+  unsigned int total_length_of_direct_addr_read_;
+  unsigned int total_length_of_direct_addr_write_;
   uint16_t indirect_addr_of_present_position_;
   uint16_t indirect_addr_of_present_velocity_;
   uint16_t indirect_addr_of_present_current_;
@@ -116,13 +165,24 @@ class DynamixelX : public dynamixel_base::DynamixelBase  {
   uint16_t indirect_addr_of_goal_position_;
   uint16_t indirect_addr_of_goal_velocity_;
   uint16_t indirect_addr_of_goal_current_;
+  uint16_t indirect_addr_of_external_port1_ = 0;
+  uint16_t indirect_addr_of_external_port2_ = 0;
+  uint16_t indirect_addr_of_external_port3_ = 0;
 
   bool set_indirect_address_read(
     const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len,
     uint16_t & indirect_addr);
   bool set_indirect_address_write(
     const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len,
     uint16_t & indirect_addr);
+
+  bool set_direct_address_read(
+    const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len,
+    uint16_t & direct_addr);
+  bool set_direct_address_write(
+    const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len,
+    uint16_t & direct_addr);
+
 };
 
 }  // namespace dynamixel_x

rt_manipulators_lib/include/dynamixel_xc330.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_XC330_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XC330_HPP_
+
+#include "dynamixel_x.hpp"
+
+namespace dynamixel_xc330 {
+
+class DynamixelXC330 : public dynamixel_x::DynamixelX {
+ public:
+  explicit DynamixelXC330(const uint8_t id);
+};
+
+}  // namespace dynamixel_xc330
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XC330_HPP_