diff --git a/rt_manipulators_lib/CMakeLists.txt b/rt_manipulators_lib/CMakeLists.txt index 8cbde20..b8a5dd9 100644 --- a/rt_manipulators_lib/CMakeLists.txt +++ b/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 diff --git a/rt_manipulators_lib/include/dynamixel_base.hpp b/rt_manipulators_lib/include/dynamixel_base.hpp index 9441433..1747109 100644 --- a/rt_manipulators_lib/include/dynamixel_base.hpp +++ b/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 & write_data) {} + virtual void push_back_profile_for_sync_write( + const double profile_rps, std::vector & write_data) {} virtual void push_back_velocity_for_sync_write( const double velocity_rps, std::vector & write_data) {} virtual void push_back_current_for_sync_write( const double current_ampere, std::vector & 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_; diff --git a/rt_manipulators_lib/include/dynamixel_p.hpp b/rt_manipulators_lib/include/dynamixel_p.hpp index 430eed8..db5921a 100644 --- a/rt_manipulators_lib/include/dynamixel_p.hpp +++ b/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 & 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 & 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, diff --git a/rt_manipulators_lib/include/dynamixel_ph54.hpp b/rt_manipulators_lib/include/dynamixel_ph54.hpp new file mode 100644 index 0000000..69e0ed9 --- /dev/null +++ b/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_ diff --git a/rt_manipulators_lib/include/dynamixel_x.hpp b/rt_manipulators_lib/include/dynamixel_x.hpp index cf84d68..65d6d24 100644 --- a/rt_manipulators_lib/include/dynamixel_x.hpp +++ b/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 & write_data); + void push_back_profile_for_sync_write( + const double profile_rps, std::vector & write_data); void push_back_velocity_for_sync_write( const double velocity_rps, std::vector & write_data); void push_back_current_for_sync_write( const double current_ampere, std::vector & 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,6 +165,9 @@ 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, @@ -123,6 +175,14 @@ class DynamixelX : public dynamixel_base::DynamixelBase { 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 diff --git a/rt_manipulators_lib/include/dynamixel_xc330.hpp b/rt_manipulators_lib/include/dynamixel_xc330.hpp new file mode 100644 index 0000000..d255407 --- /dev/null +++ b/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_ diff --git a/rt_manipulators_lib/include/hardware.hpp b/rt_manipulators_lib/include/hardware.hpp index 4ae387f..8c317ce 100644 --- a/rt_manipulators_lib/include/hardware.hpp +++ b/rt_manipulators_lib/include/hardware.hpp @@ -18,8 +18,10 @@ #include #include #include +#include #include #include +#include #include #include "joint.hpp" @@ -34,6 +36,7 @@ using JointName = std::string; class Hardware { public: explicit Hardware(const std::string device_name); + explicit Hardware(std::unique_ptr comm); ~Hardware(); bool load_config_file(const std::string& config_yaml); bool connect(const int baudrate = 3000000); @@ -62,6 +65,8 @@ class Hardware { bool get_temperatures(const std::string& group_name, std::vector& temperatures); bool get_max_position_limit(const uint8_t & id, double & max_position_limit); bool get_min_position_limit(const uint8_t & id, double & min_position_limit); + bool get_external_port_voltage(const uint8_t id, const int number, double& voltage); + bool get_external_port_voltage(const std::string& joint_name, const int number, double& voltage); 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& positions); @@ -85,6 +90,57 @@ class Hardware { bool write_velocity_pi_gain_to_group(const std::string& group_name, const uint16_t p, const uint16_t i); + template + bool write_data(const IdentifyT & identify, const uint16_t & addr, const DataT & data) + { + if (!joints_.has_joint(identify)) { + std::cerr << "Joint: " << identify << " is not registered." << std::endl; + return false; + } + + if (std::is_same::value) { + return comm_->write_byte_data(joints_.joint(identify)->id(), addr, data); + } + if (std::is_same::value) { + return comm_->write_word_data(joints_.joint(identify)->id(), addr, data); + } + if (std::is_same::value) { + return comm_->write_double_word_data(joints_.joint(identify)->id(), addr, data); + } + + return false; + } + + template + bool read_data(const IdentifyT & identify, const uint16_t & addr, DataT & data) + { + if (!joints_.has_joint(identify)) { + std::cerr << "Joint: " << identify << " is not registered." << std::endl; + return false; + } + + bool result = false; + if (std::is_same::value) { + uint8_t tmp_data = 0x00; + result = comm_->read_byte_data(joints_.joint(identify)->id(), addr, tmp_data); + data = tmp_data; + } + if (std::is_same::value) { + uint16_t tmp_data = 0x00; + result = comm_->read_word_data(joints_.joint(identify)->id(), addr, tmp_data); + data = tmp_data; + } + if (std::is_same::value) { + uint32_t tmp_data = 0x00; + result = comm_->read_double_word_data(joints_.joint(identify)->id(), addr, tmp_data); + data = tmp_data; + } + + return result; + } + + + protected: std::shared_ptr comm_; @@ -92,8 +148,10 @@ class Hardware { bool write_operating_mode(const std::string& group_name); bool limit_goal_velocity_by_present_position(const std::string& group_name); bool limit_goal_current_by_present_position(const std::string& group_name); + bool search_unsupport_indirect_addr_hw_type(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 create_sync_write_group_direct_addr(const std::string& group_name); void read_write_thread(const std::vector& group_names, const std::chrono::milliseconds& update_cycle_ms); @@ -105,6 +163,7 @@ class Hardware { std::map addr_sync_read_temperature_; bool thread_enable_; std::shared_ptr read_write_thread_; + bool use_direct_addr_enabled_; }; } // namespace rt_manipulators_cpp diff --git a/rt_manipulators_lib/include/hardware_communicator.hpp b/rt_manipulators_lib/include/hardware_communicator.hpp index 36d6e24..1335148 100644 --- a/rt_manipulators_lib/include/hardware_communicator.hpp +++ b/rt_manipulators_lib/include/hardware_communicator.hpp @@ -39,33 +39,38 @@ using GroupSyncWrite = dynamixel::GroupSyncWrite; class Communicator{ public: explicit Communicator(const std::string device_name); - ~Communicator(); - bool is_connected(); - bool connect(const int baudrate = 3000000); - void disconnect(); - void make_sync_read_group(const group_name_t & group_name, const dxl_address_t & start_address, - const dxl_data_length_t & data_length); - void make_sync_write_group(const group_name_t & group_name, const dxl_address_t & start_address, - const dxl_data_length_t & data_length); - bool append_id_to_sync_read_group(const group_name_t & group_name, const dxl_id_t & id); - bool append_id_to_sync_write_group(const group_name_t & group_name, const dxl_id_t & id, + virtual ~Communicator(); + virtual bool is_connected(); + virtual bool connect(const int baudrate = 3000000); + virtual void disconnect(); + virtual void make_sync_read_group( + const group_name_t & group_name, const dxl_address_t & start_address, + const dxl_data_length_t & data_length); + virtual void make_sync_write_group( + const group_name_t & group_name, const dxl_address_t & start_address, + const dxl_data_length_t & data_length); + virtual bool append_id_to_sync_read_group(const group_name_t & group_name, const dxl_id_t & id); + virtual bool append_id_to_sync_write_group(const group_name_t & group_name, const dxl_id_t & id, std::vector & init_data); - bool send_sync_read_packet(const group_name_t & group_name); - bool send_sync_write_packet(const group_name_t & group_name); - bool get_sync_read_data(const group_name_t & group_name, const dxl_id_t id, + virtual bool send_sync_read_packet(const group_name_t & group_name); + virtual bool send_sync_write_packet(const group_name_t & group_name); + virtual bool get_sync_read_data(const group_name_t & group_name, const dxl_id_t id, const dxl_address_t & address, const dxl_data_length_t & length, dxl_double_word_t & read_data); - bool set_sync_write_data(const group_name_t & group_name, const dxl_id_t id, + virtual bool set_sync_write_data(const group_name_t & group_name, const dxl_id_t id, std::vector & write_data); - bool write_byte_data(const dxl_id_t & id, const dxl_address_t & address, + virtual bool write_byte_data(const dxl_id_t & id, const dxl_address_t & address, const dxl_byte_t & write_data); - bool write_word_data(const dxl_id_t & id, const dxl_address_t & address, + virtual bool write_word_data(const dxl_id_t & id, const dxl_address_t & address, const dxl_word_t & write_data); - bool write_double_word_data(const dxl_id_t & id, const dxl_address_t & address, - const dxl_double_word_t & write_data); - bool read_byte_data(const dxl_id_t & id, const dxl_address_t & address, dxl_byte_t & read_data); - bool read_word_data(const dxl_id_t & id, const dxl_address_t & address, dxl_word_t & read_data); - bool read_double_word_data(const dxl_id_t & id, const dxl_address_t & address, + virtual bool write_double_word_data( + const dxl_id_t & id, const dxl_address_t & address, + const dxl_double_word_t & write_data); + virtual bool read_byte_data( + const dxl_id_t & id, const dxl_address_t & address, dxl_byte_t & read_data); + virtual bool read_word_data( + const dxl_id_t & id, const dxl_address_t & address, dxl_word_t & read_data); + virtual bool read_double_word_data(const dxl_id_t & id, const dxl_address_t & address, dxl_double_word_t & read_data); private: @@ -80,7 +85,6 @@ class Communicator{ bool is_connected_; std::shared_ptr port_handler_; - std::shared_ptr packet_handler_; std::map> sync_read_groups_; std::map> sync_write_groups_; }; diff --git a/rt_manipulators_lib/include/hardware_joints.hpp b/rt_manipulators_lib/include/hardware_joints.hpp index da29ef9..6669d9c 100644 --- a/rt_manipulators_lib/include/hardware_joints.hpp +++ b/rt_manipulators_lib/include/hardware_joints.hpp @@ -66,6 +66,9 @@ class Joints{ bool get_temperatures(const group_name_t & group_name, std::vector& temperatures); bool get_max_position_limit(const dxl_id_t & id, position_t & max_position_limit); bool get_min_position_limit(const dxl_id_t & id, position_t & min_position_limit); + bool get_external_port_voltage(const dxl_id_t & id, const int number, double& voltage); + bool get_external_port_voltage( + const joint_name_t & joint_name, const int number, double& voltage); bool set_position(const dxl_id_t & id, const position_t & position); bool set_position(const joint_name_t & joint_name, const position_t & position); bool set_positions(const group_name_t & group_name, const std::vector & positions); diff --git a/rt_manipulators_lib/include/joint.hpp b/rt_manipulators_lib/include/joint.hpp index f4d0a60..560d49f 100644 --- a/rt_manipulators_lib/include/joint.hpp +++ b/rt_manipulators_lib/include/joint.hpp @@ -30,6 +30,7 @@ class Joint { Joint(const uint8_t id, const uint8_t operating_mode, const std::string dynamixel_name); uint8_t id() const; uint8_t operating_mode() const; + std::string name() const; void set_position_limit_margin(const double position_radian); void set_position_limit(const double min_position_radian, const double max_position_radian); double max_position_limit() const; @@ -54,11 +55,15 @@ class Joint { double get_goal_velocity() const; double get_goal_current() const; + void set_external_port_voltage(const int number, const double analog_voltage); + double get_external_port_voltage(const int number) const; + std::shared_ptr dxl; private: uint8_t id_; uint8_t operating_mode_; + std::string name_; double position_limit_margin_; double max_position_limit_; double min_position_limit_; @@ -73,6 +78,8 @@ class Joint { double goal_position_; double goal_velocity_; double goal_current_; + + std::vector external_port_voltage_ = {0.0, 0.0, 0.0, 0.0}; }; class JointGroup { @@ -90,6 +97,11 @@ class JointGroup { bool sync_write_velocity_enabled() const; bool sync_write_current_enabled() const; + bool sync_read_external_port1_enabled() const; + bool sync_read_external_port2_enabled() const; + bool sync_read_external_port3_enabled() const; + bool sync_read_external_port4_enabled() const; + private: std::vector joint_names_; bool sync_read_position_enabled_; @@ -100,6 +112,11 @@ class JointGroup { bool sync_write_position_enabled_; bool sync_write_velocity_enabled_; bool sync_write_current_enabled_; + + bool sync_read_external_port1_enabled_ = false; + bool sync_read_external_port2_enabled_ = false; + bool sync_read_external_port3_enabled_ = false; + bool sync_read_external_port4_enabled_ = false; }; } // namespace joint diff --git a/rt_manipulators_lib/src/CMakeLists.txt b/rt_manipulators_lib/src/CMakeLists.txt index 3ea13ef..e243468 100644 --- a/rt_manipulators_lib/src/CMakeLists.txt +++ b/rt_manipulators_lib/src/CMakeLists.txt @@ -19,6 +19,7 @@ add_library(${library_name} dynamixel_xh540.cpp dynamixel_p.cpp dynamixel_ph42.cpp + dynamixel_ph54.cpp ) set_target_properties(${library_name} PROPERTIES VERSION 1.1.0 SOVERSION 1) diff --git a/rt_manipulators_lib/src/config_file_parser.cpp b/rt_manipulators_lib/src/config_file_parser.cpp index e6c6e8c..798a936 100644 --- a/rt_manipulators_lib/src/config_file_parser.cpp +++ b/rt_manipulators_lib/src/config_file_parser.cpp @@ -30,6 +30,9 @@ bool parse(const std::string& config_yaml, hardware_joints::Joints & parsed_join return false; } + // Reset parsed_joints + parsed_joints = hardware_joints::Joints(); + YAML::Node config = YAML::LoadFile(config_yaml); for (const auto & config_joint_group : config["joint_groups"]) { auto group_name = config_joint_group.first.as(); diff --git a/rt_manipulators_lib/src/dynamixel_p.cpp b/rt_manipulators_lib/src/dynamixel_p.cpp index 62d5ac9..01c3fbc 100644 --- a/rt_manipulators_lib/src/dynamixel_p.cpp +++ b/rt_manipulators_lib/src/dynamixel_p.cpp @@ -23,6 +23,10 @@ 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_EXTERNAL_PORT_MODE1 = 56; +const uint16_t ADDR_EXTERNAL_PORT_MODE2 = 57; +const uint16_t ADDR_EXTERNAL_PORT_MODE3 = 58; +const uint16_t ADDR_EXTERNAL_PORT_MODE4 = 59; const uint16_t ADDR_TORQUE_ENABLE = 512; const uint16_t ADDR_VELOCITY_I_GAIN = 524; const uint16_t ADDR_VELOCITY_P_GAIN = 526; @@ -39,16 +43,24 @@ const uint16_t ADDR_PRESENT_VELOCITY = 576; const uint16_t ADDR_PRESENT_POSITION = 580; const uint16_t ADDR_PRESENT_VOLTAGE = 592; const uint16_t ADDR_PRESENT_TEMPERATURE = 594; +const uint16_t ADDR_EXTERNAL_PORT_DATA1 = 600; +const uint16_t ADDR_EXTERNAL_PORT_DATA2 = 602; +const uint16_t ADDR_EXTERNAL_PORT_DATA3 = 604; +const uint16_t ADDR_EXTERNAL_PORT_DATA4 = 606; const uint16_t ADDR_INDIRECT_ADDRESS_1 = 168; const uint16_t ADDR_INDIRECT_DATA_1 = 634; -const uint16_t ADDR_INDIRECT_ADDRESS_16 = 198; -const uint16_t ADDR_INDIRECT_DATA_16 = 649; +const uint16_t ADDR_INDIRECT_ADDRESS_21 = ADDR_INDIRECT_ADDRESS_1 + 2 * 20; +const uint16_t ADDR_INDIRECT_DATA_21 = ADDR_INDIRECT_DATA_1 + 1 * 20; +// sync_readやsync_writeを使うとき全サーボの同じアドレスへアクセスする。 // XMシリーズと同じアドレスで通信するため、インダイレクトアドレスの使用範囲を絞る +// インダイレクトアドレス有効範囲：1 ~ 28（合計28個） +// sync_writeは多くてもpositionとcurrentしか同時設定されないので、 +// readの領域を多めに取る const uint16_t ADDR_START_INDIRECT_ADDR_READ = ADDR_INDIRECT_ADDRESS_1; const uint16_t ADDR_START_INDIRECT_DATA_READ = ADDR_INDIRECT_DATA_1; -const uint16_t ADDR_START_INDIRECT_ADDR_WRITE = ADDR_INDIRECT_ADDRESS_16; -const uint16_t ADDR_START_INDIRECT_DATA_WRITE = ADDR_INDIRECT_DATA_16; +const uint16_t ADDR_START_INDIRECT_ADDR_WRITE = ADDR_INDIRECT_ADDRESS_21; +const uint16_t ADDR_START_INDIRECT_DATA_WRITE = ADDR_INDIRECT_DATA_21; const uint16_t LEN_PRESENT_CURRENT = 2; const uint16_t LEN_PRESENT_VELOCITY = 4; @@ -59,6 +71,9 @@ const uint16_t LEN_GOAL_CURRENT = 2; const uint16_t LEN_GOAL_VELOCITY = 4; const uint16_t LEN_GOAL_POSITION = 4; const uint16_t LEN_INDIRECT_ADDRESS = 2; +const uint16_t LEN_EXTERNAL_PORT_DATA = 2; + +const uint8_t EXTERNAL_PORT_MODE_ANALOG_INPUT = 0; const double TO_ACCELERATION_REV_PER_MM = 1.0; const double TO_ACCELERATION_TO_RAD_PER_MM = TO_ACCELERATION_REV_PER_MM * 2.0 * M_PI; @@ -75,6 +90,7 @@ const double TO_CURRENT_AMPERE = 0.001; 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; +const double TO_ANALOG_VOLTAGE_VOLT = 3.3 / 4095.0; DynamixelP::DynamixelP(const uint8_t id, const int home_position) : dynamixel_base::DynamixelBase(id), HOME_POSITION_(home_position), @@ -205,6 +221,10 @@ double DynamixelP::to_voltage_volt(const int voltage) { return voltage * TO_VOLTAGE_VOLT; } +double DynamixelP::to_analog_voltage_volt(const int input) { + return input * TO_ANALOG_VOLTAGE_VOLT; +} + unsigned int DynamixelP::from_position_radian(const double position_rad) { return position_rad * TO_DXL_POS + HOME_POSITION_; } @@ -265,6 +285,24 @@ bool DynamixelP::auto_set_indirect_address_of_goal_current( comm, ADDR_GOAL_CURRENT, LEN_GOAL_CURRENT, indirect_addr_of_goal_current_); } +bool DynamixelP::auto_set_indirect_address_of_external_port( + const dynamixel_base::comm_t & comm, const int number) { + if (number == 1) { + return set_indirect_address_read( + comm, ADDR_EXTERNAL_PORT_DATA1, LEN_EXTERNAL_PORT_DATA, indirect_addr_of_external_port1_); + } else if (number == 2) { + return set_indirect_address_read( + comm, ADDR_EXTERNAL_PORT_DATA2, LEN_EXTERNAL_PORT_DATA, indirect_addr_of_external_port2_); + } else if (number == 3) { + return set_indirect_address_read( + comm, ADDR_EXTERNAL_PORT_DATA3, LEN_EXTERNAL_PORT_DATA, indirect_addr_of_external_port3_); + } else if (number == 4) { + return set_indirect_address_read( + comm, ADDR_EXTERNAL_PORT_DATA4, LEN_EXTERNAL_PORT_DATA, indirect_addr_of_external_port4_); + } + return false; +} + unsigned int DynamixelP::indirect_addr_of_present_position(void) { return indirect_addr_of_present_position_; } @@ -297,6 +335,20 @@ unsigned int DynamixelP::indirect_addr_of_goal_current(void) { return indirect_addr_of_goal_current_; } +unsigned int DynamixelP::indirect_addr_of_external_port(const int number) { + if (number == 1) { + return indirect_addr_of_external_port1_; + } else if (number == 2) { + return indirect_addr_of_external_port2_; + } else if (number == 3) { + return indirect_addr_of_external_port3_; + } else if (number == 4) { + return indirect_addr_of_external_port4_; + } + + return indirect_addr_of_external_port1_; +} + unsigned int DynamixelP::start_address_for_indirect_read(void) { return ADDR_START_INDIRECT_DATA_READ; } @@ -383,6 +435,18 @@ bool DynamixelP::extract_present_temperature_from_sync_read( return true; } +bool DynamixelP::extract_external_port_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + const int number, double & analog_voltage_volt) { + uint32_t data = 0; + if (!comm->get_sync_read_data( + group_name, id_, indirect_addr_of_external_port(number), LEN_EXTERNAL_PORT_DATA, data)) { + return false; + } + analog_voltage_volt = to_analog_voltage_volt(static_cast(data)); + return true; +} + void DynamixelP::push_back_position_for_sync_write( const double position_rad, std::vector & write_data) { uint32_t dxl_position = from_position_radian(position_rad); @@ -408,6 +472,32 @@ void DynamixelP::push_back_current_for_sync_write( write_data.push_back(DXL_HIBYTE(dxl_current)); } +bool DynamixelP::set_external_port_mode_to_analog_input( + const dynamixel_base::comm_t & comm, const int number) { + uint16_t target_addr = 0; + if (number == 1) { + target_addr = ADDR_EXTERNAL_PORT_MODE1; + } else if (number == 2) { + target_addr = ADDR_EXTERNAL_PORT_MODE2; + } else if (number == 3) { + target_addr = ADDR_EXTERNAL_PORT_MODE3; + } else if (number == 4) { + target_addr = ADDR_EXTERNAL_PORT_MODE4; + } + + // Skip if the mode is already set + uint8_t present_mode = 0; + if (!comm->read_byte_data(id_, target_addr, present_mode)) { + return false; + } + if (present_mode == EXTERNAL_PORT_MODE_ANALOG_INPUT) { + return true; + } + + return comm->write_byte_data( + id_, target_addr, EXTERNAL_PORT_MODE_ANALOG_INPUT); +} + bool DynamixelP::set_indirect_address_read( const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len, uint16_t & indirect_addr) { @@ -415,6 +505,14 @@ bool DynamixelP::set_indirect_address_read( for (int i = 0; i < len; i++) { uint16_t target_indirect_address = next_indirect_addr_read() + LEN_INDIRECT_ADDRESS * i; uint16_t target_data_address = addr + i; + + // Skip if the address is already set + uint16_t present_data_address = 0; + retval &= comm->read_word_data(id_, target_indirect_address, present_data_address); + if (present_data_address == target_data_address) { + continue; + } + if (!comm->write_word_data( id_, target_indirect_address, target_data_address)) { retval = false; @@ -434,6 +532,14 @@ bool DynamixelP::set_indirect_address_write( for (int i = 0; i < len; i++) { uint16_t target_indirect_address = next_indirect_addr_write() + LEN_INDIRECT_ADDRESS * i; uint16_t target_data_address = addr + i; + + // Skip if the address is already set + uint16_t present_data_address = 0; + retval &= comm->read_word_data(id_, target_indirect_address, present_data_address); + if (present_data_address == target_data_address) { + continue; + } + if (!comm->write_word_data( id_, target_indirect_address, target_data_address)) { retval = false; diff --git a/rt_manipulators_lib/src/dynamixel_ph54.cpp b/rt_manipulators_lib/src/dynamixel_ph54.cpp new file mode 100644 index 0000000..07ed296 --- /dev/null +++ b/rt_manipulators_lib/src/dynamixel_ph54.cpp @@ -0,0 +1,56 @@ +// 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. + + +#include +#include "dynamixel_ph54.hpp" + + +namespace dynamixel_ph54 { + +const double TO_ACCELERATION_REV_PER_MM = 1.0; +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 = 4255632; +const double TO_RADIANS = (180.0 / 501923.0) * M_PI / 180.0; +const double TO_DXL_POS = 1.0 / TO_RADIANS; + +DynamixelPH54::DynamixelPH54(const uint8_t id) + : dynamixel_p::DynamixelP(id) { + name_ = "PH54"; +} + +unsigned int DynamixelPH54::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) { + // PHシリーズでは、'0'が最大加速度を意味する + // よって、加速度の最小値は'1'である + dxl_acceleration = 1; + } + + return static_cast(dxl_acceleration); +} + +double DynamixelPH54::to_position_radian(const int position) { + return (position - HOME_POSITION_) * TO_RADIANS; +} + +unsigned int DynamixelPH54::from_position_radian(const double position_rad) { + return position_rad * TO_DXL_POS + HOME_POSITION_; +} + +} // namespace dynamixel_ph54 diff --git a/rt_manipulators_lib/src/dynamixel_x.cpp b/rt_manipulators_lib/src/dynamixel_x.cpp index 117abc7..d17fdb0 100644 --- a/rt_manipulators_lib/src/dynamixel_x.cpp +++ b/rt_manipulators_lib/src/dynamixel_x.cpp @@ -23,6 +23,9 @@ 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_EXTERNAL_PORT_MODE1 = 56; +const uint16_t ADDR_EXTERNAL_PORT_MODE2 = 57; +const uint16_t ADDR_EXTERNAL_PORT_MODE3 = 58; const uint16_t ADDR_TORQUE_ENABLE = 64; const uint16_t ADDR_VELOCITY_I_GAIN = 76; const uint16_t ADDR_VELOCITY_P_GAIN = 78; @@ -37,28 +40,49 @@ const uint16_t ADDR_GOAL_POSITION = 116; const uint16_t ADDR_PRESENT_CURRENT = 126; const uint16_t ADDR_PRESENT_VELOCITY = 128; const uint16_t ADDR_PRESENT_POSITION = 132; +const uint16_t ADDR_VELOCITY_TRAJECTORY = 136; +const uint16_t ADDR_POSITION_TRAJECTORY = 140; const uint16_t ADDR_PRESENT_VOLTAGE = 144; const uint16_t ADDR_PRESENT_TEMPERATURE = 146; +const uint16_t ADDR_EXTERNAL_PORT_DATA1 = 152; +const uint16_t ADDR_EXTERNAL_PORT_DATA2 = 154; +const uint16_t ADDR_EXTERNAL_PORT_DATA3 = 156; const uint16_t ADDR_INDIRECT_ADDRESS_29 = 578; const uint16_t ADDR_INDIRECT_DATA_29 = 634; -const uint16_t ADDR_INDIRECT_ADDRESS_44 = 608; -const uint16_t ADDR_INDIRECT_DATA_44 = 649; +const uint16_t ADDR_INDIRECT_ADDRESS_49 = ADDR_INDIRECT_ADDRESS_29 + 2 * 20; +const uint16_t ADDR_INDIRECT_DATA_49 = ADDR_INDIRECT_DATA_29 + 1 * 20; +// sync_readやsync_writeを使うとき全サーボの同じアドレスへアクセスする。 // PHシリーズと同じアドレスで通信するため29番以降のインダイレクトアドレスを使用する +// インダイレクトアドレス有効範囲：29 ~ 56（合計28個） +// sync_writeは多くてもpositionとcurrentしか同時設定されないので、 +// readの領域を多めに取る const uint16_t ADDR_START_INDIRECT_ADDR_READ = ADDR_INDIRECT_ADDRESS_29; const uint16_t ADDR_START_INDIRECT_DATA_READ = ADDR_INDIRECT_DATA_29; -const uint16_t ADDR_START_INDIRECT_ADDR_WRITE = ADDR_INDIRECT_ADDRESS_44; -const uint16_t ADDR_START_INDIRECT_DATA_WRITE = ADDR_INDIRECT_DATA_44; +const uint16_t ADDR_START_INDIRECT_ADDR_WRITE = ADDR_INDIRECT_ADDRESS_49; +const uint16_t ADDR_START_INDIRECT_DATA_WRITE = ADDR_INDIRECT_DATA_49; + +const uint16_t ADDR_START_SERIES_ADDR_READ = ADDR_PRESENT_CURRENT; +const uint16_t ADDR_START_SERIES_DATA_READ = ADDR_PRESENT_CURRENT; +const uint16_t ADDR_START_SERIES_ADDR_WRITE = ADDR_GOAL_CURRENT; +const uint16_t ADDR_START_SERIES_DATA_WRITE = ADDR_GOAL_CURRENT; const uint16_t LEN_PRESENT_CURRENT = 2; const uint16_t LEN_PRESENT_VELOCITY = 4; const uint16_t LEN_PRESENT_POSITION = 4; +const uint16_t LEN_VELOCITY_TRAJECTORY = 4; +const uint16_t LEN_POSITION_TRAJECTORY = 4; const uint16_t LEN_PRESENT_VOLTAGE = 2; const uint16_t LEN_PRESENT_TEMPERATURE = 1; const uint16_t LEN_GOAL_CURRENT = 2; const uint16_t LEN_GOAL_VELOCITY = 4; +const uint16_t LEN_PROFILE_ACCELERATION = 4; +const uint16_t LEN_PROFILE_VELOCITY = 4; const uint16_t LEN_GOAL_POSITION = 4; const uint16_t LEN_INDIRECT_ADDRESS = 2; +const uint16_t LEN_EXTERNAL_PORT_DATA = 2; + +const uint8_t EXTERNAL_PORT_MODE_ANALOG_INPUT = 0; 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; @@ -75,6 +99,7 @@ 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; +const double TO_ANALOG_VOLTAGE_VOLT = 3.3 / 4095.0; DynamixelX::DynamixelX(const uint8_t id, const int home_position) : dynamixel_base::DynamixelBase(id), HOME_POSITION_(home_position), @@ -205,6 +230,10 @@ double DynamixelX::to_voltage_volt(const int voltage) { return voltage * TO_VOLTAGE_VOLT; } +double DynamixelX::to_analog_voltage_volt(const int input) { + return input * TO_ANALOG_VOLTAGE_VOLT; +} + unsigned int DynamixelX::from_position_radian(const double position_rad) { return position_rad * TO_DXL_POS + HOME_POSITION_; } @@ -265,6 +294,21 @@ bool DynamixelX::auto_set_indirect_address_of_goal_current( comm, ADDR_GOAL_CURRENT, LEN_GOAL_CURRENT, indirect_addr_of_goal_current_); } +bool DynamixelX::auto_set_indirect_address_of_external_port( + const dynamixel_base::comm_t & comm, const int number) { + if (number == 1) { + return set_indirect_address_read( + comm, ADDR_EXTERNAL_PORT_DATA1, LEN_EXTERNAL_PORT_DATA, indirect_addr_of_external_port1_); + } else if (number == 2) { + return set_indirect_address_read( + comm, ADDR_EXTERNAL_PORT_DATA2, LEN_EXTERNAL_PORT_DATA, indirect_addr_of_external_port2_); + } else if (number == 3) { + return set_indirect_address_read( + comm, ADDR_EXTERNAL_PORT_DATA3, LEN_EXTERNAL_PORT_DATA, indirect_addr_of_external_port3_); + } + return false; +} + unsigned int DynamixelX::indirect_addr_of_present_position(void) { return indirect_addr_of_present_position_; } @@ -297,6 +341,18 @@ unsigned int DynamixelX::indirect_addr_of_goal_current(void) { return indirect_addr_of_goal_current_; } +unsigned int DynamixelX::indirect_addr_of_external_port(const int number) { + if (number == 1) { + return indirect_addr_of_external_port1_; + } else if (number == 2) { + return indirect_addr_of_external_port2_; + } else if (number == 3) { + return indirect_addr_of_external_port3_; + } + + return indirect_addr_of_external_port1_; +} + unsigned int DynamixelX::start_address_for_indirect_read(void) { return ADDR_START_INDIRECT_DATA_READ; } @@ -310,6 +366,23 @@ unsigned int DynamixelX::next_indirect_addr_read(void) const { LEN_INDIRECT_ADDRESS * total_length_of_indirect_addr_read_; } +unsigned int DynamixelX::start_address_for_direct_read(void) { + return ADDR_PRESENT_CURRENT; +} + +unsigned int DynamixelX::length_of_direct_data_read(void) { + return LEN_PRESENT_CURRENT + + LEN_PRESENT_VELOCITY + + LEN_PRESENT_POSITION + LEN_VELOCITY_TRAJECTORY + LEN_POSITION_TRAJECTORY + + LEN_PRESENT_VOLTAGE + + LEN_PRESENT_TEMPERATURE; +} + +unsigned int DynamixelX::next_direct_addr_read(void) const { + return ADDR_START_SERIES_ADDR_READ + + LEN_INDIRECT_ADDRESS * total_length_of_direct_addr_read_; +} + unsigned int DynamixelX::start_address_for_indirect_write(void) { return ADDR_START_INDIRECT_DATA_WRITE; } @@ -323,6 +396,21 @@ unsigned int DynamixelX::next_indirect_addr_write(void) const { LEN_INDIRECT_ADDRESS * total_length_of_indirect_addr_write_; } +unsigned int DynamixelX::start_address_for_direct_write(void) { + return ADDR_GOAL_CURRENT; +} + +unsigned int DynamixelX::length_of_direct_data_write(void) { + return LEN_GOAL_CURRENT + + LEN_GOAL_VELOCITY + LEN_PROFILE_ACCELERATION + LEN_PROFILE_VELOCITY + + LEN_GOAL_POSITION; +} + +unsigned int DynamixelX::next_direct_addr_write(void) const { + return ADDR_START_SERIES_ADDR_WRITE + + LEN_INDIRECT_ADDRESS * total_length_of_direct_addr_write_; +} + bool DynamixelX::extract_present_position_from_sync_read( const dynamixel_base::comm_t & comm, const std::string & group_name, double & position_rad) { @@ -335,6 +423,18 @@ bool DynamixelX::extract_present_position_from_sync_read( return true; } +bool DynamixelX::extract_default_position_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + double & position_rad) { + uint32_t data = 0; + if (!comm->get_sync_read_data( + group_name, id_, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION, data)) { + return false; + } + position_rad = to_position_radian(static_cast(data)); + return true; +} + bool DynamixelX::extract_present_velocity_from_sync_read( const dynamixel_base::comm_t & comm, const std::string & group_name, double & velocity_rps) { @@ -347,6 +447,18 @@ bool DynamixelX::extract_present_velocity_from_sync_read( return true; } +bool DynamixelX::extract_default_velocity_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + double & velocity_rps) { + uint32_t data = 0; + if (!comm->get_sync_read_data( + group_name, id_, ADDR_PRESENT_VELOCITY, LEN_PRESENT_VELOCITY, data)) { + return false; + } + velocity_rps = to_velocity_rps(static_cast(data)); + return true; +} + bool DynamixelX::extract_present_current_from_sync_read( const dynamixel_base::comm_t & comm, const std::string & group_name, double & current_ampere) { @@ -359,6 +471,18 @@ bool DynamixelX::extract_present_current_from_sync_read( return true; } +bool DynamixelX::extract_default_current_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + double & current_ampere) { + uint32_t data = 0; + if (!comm->get_sync_read_data( + group_name, id_, ADDR_PRESENT_CURRENT, LEN_PRESENT_CURRENT, data)) { + return false; + } + current_ampere = to_current_ampere(static_cast(data)); + return true; +} + bool DynamixelX::extract_present_input_voltage_from_sync_read( const dynamixel_base::comm_t & comm, const std::string & group_name, double & voltage_volt) { @@ -371,6 +495,18 @@ bool DynamixelX::extract_present_input_voltage_from_sync_read( return true; } +bool DynamixelX::extract_default_input_voltage_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + double & voltage_volt) { + uint32_t data = 0; + if (!comm->get_sync_read_data( + group_name, id_, ADDR_PRESENT_VOLTAGE, LEN_PRESENT_VOLTAGE, data)) { + return false; + } + voltage_volt = to_voltage_volt(static_cast(data)); + return true; +} + bool DynamixelX::extract_present_temperature_from_sync_read( const dynamixel_base::comm_t & comm, const std::string & group_name, int & temperature_deg) { @@ -383,6 +519,30 @@ bool DynamixelX::extract_present_temperature_from_sync_read( return true; } +bool DynamixelX::extract_default_temperature_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + int & temperature_deg) { + uint32_t data = 0; + if (!comm->get_sync_read_data( + group_name, id_, ADDR_PRESENT_TEMPERATURE, LEN_PRESENT_TEMPERATURE, data)) { + return false; + } + temperature_deg = static_cast(data); + return true; +} + +bool DynamixelX::extract_external_port_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + const int number, double & analog_voltage_volt) { + uint32_t data = 0; + if (!comm->get_sync_read_data( + group_name, id_, indirect_addr_of_external_port(number), LEN_EXTERNAL_PORT_DATA, data)) { + return false; + } + analog_voltage_volt = to_analog_voltage_volt(static_cast(data)); + return true; +} + void DynamixelX::push_back_position_for_sync_write( const double position_rad, std::vector & write_data) { uint32_t dxl_position = from_position_radian(position_rad); @@ -392,6 +552,19 @@ void DynamixelX::push_back_position_for_sync_write( write_data.push_back(DXL_HIBYTE(DXL_HIWORD(dxl_position))); } +void DynamixelX::push_back_profile_for_sync_write( + const double profile_rps, std::vector & write_data) { + uint32_t dxl_profile = profile_rps; + write_data.push_back(DXL_LOBYTE(DXL_LOWORD(dxl_profile))); + write_data.push_back(DXL_HIBYTE(DXL_LOWORD(dxl_profile))); + write_data.push_back(DXL_LOBYTE(DXL_HIWORD(dxl_profile))); + write_data.push_back(DXL_HIBYTE(DXL_HIWORD(dxl_profile))); + write_data.push_back(DXL_LOBYTE(DXL_LOWORD(dxl_profile))); + write_data.push_back(DXL_HIBYTE(DXL_LOWORD(dxl_profile))); + write_data.push_back(DXL_LOBYTE(DXL_HIWORD(dxl_profile))); + write_data.push_back(DXL_HIBYTE(DXL_HIWORD(dxl_profile))); +} + void DynamixelX::push_back_velocity_for_sync_write( const double velocity_rps, std::vector & write_data) { uint32_t dxl_velocity = from_velocity_rps(velocity_rps); @@ -408,6 +581,30 @@ void DynamixelX::push_back_current_for_sync_write( write_data.push_back(DXL_HIBYTE(dxl_current)); } +bool DynamixelX::set_external_port_mode_to_analog_input( + const dynamixel_base::comm_t & comm, const int number) { + uint16_t target_addr = 0; + if (number == 1) { + target_addr = ADDR_EXTERNAL_PORT_MODE1; + } else if (number == 2) { + target_addr = ADDR_EXTERNAL_PORT_MODE2; + } else if (number == 3) { + target_addr = ADDR_EXTERNAL_PORT_MODE3; + } + + // Skip if the mode is already set + uint8_t present_mode = 0; + if (!comm->read_byte_data(id_, target_addr, present_mode)) { + return false; + } + if (present_mode == EXTERNAL_PORT_MODE_ANALOG_INPUT) { + return true; + } + + return comm->write_byte_data( + id_, target_addr, EXTERNAL_PORT_MODE_ANALOG_INPUT); +} + bool DynamixelX::set_indirect_address_read( const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len, uint16_t & indirect_addr) { @@ -415,9 +612,11 @@ bool DynamixelX::set_indirect_address_read( for (int i = 0; i < len; i++) { uint16_t target_indirect_address = next_indirect_addr_read() + LEN_INDIRECT_ADDRESS * i; uint16_t target_data_address = addr + i; - if (!comm->write_word_data( - id_, target_indirect_address, target_data_address)) { - retval = false; + if (get_name() != "XC330"){ + if (!comm->write_word_data( + id_, target_indirect_address, target_data_address)) { + retval = false; + } } } // テストしやすくするため、write_word_dataに失敗しても変数を更新する @@ -446,4 +645,36 @@ bool DynamixelX::set_indirect_address_write( return retval; } +bool DynamixelX::set_direct_address_read( + const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len, + uint16_t & direct_addr) { + bool retval = true; + for (int i = 0; i < len; i++) { + uint16_t target_direct_address = next_direct_addr_read() + LEN_INDIRECT_ADDRESS * i; + uint16_t target_data_address = addr + i; + } + // テストしやすくするため、write_word_dataに失敗しても変数を更新する + direct_addr = ADDR_START_INDIRECT_DATA_READ + + total_length_of_direct_addr_read_; + total_length_of_direct_addr_read_ += len; + return retval; +} + + +bool DynamixelX::set_direct_address_write( +//ここではtotal_lengthの更新ができれば良さそう + const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len, + uint16_t & direct_addr) { + bool retval = true; + for (int i = 0; i < len; i++) { + uint16_t target_direct_address = next_direct_addr_write() + LEN_INDIRECT_ADDRESS * i; + uint16_t target_data_address = addr + i; + } + // テストしやすくするため、write_word_dataに失敗しても変数を更新する + direct_addr = ADDR_START_SERIES_DATA_WRITE + + total_length_of_direct_addr_write_; + total_length_of_direct_addr_write_ += len; + return retval; +} + } // namespace dynamixel_x diff --git a/rt_manipulators_lib/src/dynamixel_xc330.cpp b/rt_manipulators_lib/src/dynamixel_xc330.cpp new file mode 100644 index 0000000..dca36e5 --- /dev/null +++ b/rt_manipulators_lib/src/dynamixel_xc330.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_xc330.hpp" + + +namespace dynamixel_xc330 { + +DynamixelXC330::DynamixelXC330(const uint8_t id) + : dynamixel_x::DynamixelX(id) { + name_ = "XC330"; +} + +} // namespace dynamixel_xm430 diff --git a/rt_manipulators_lib/src/hardware.cpp b/rt_manipulators_lib/src/hardware.cpp index c870973..cf0fd23 100644 --- a/rt_manipulators_lib/src/hardware.cpp +++ b/rt_manipulators_lib/src/hardware.cpp @@ -23,7 +23,12 @@ namespace rt_manipulators_cpp { Hardware::Hardware(const std::string device_name) : thread_enable_(false) { - comm_ = std::make_shared(device_name); + comm_ = std::make_unique(device_name); +} + +Hardware::Hardware(std::unique_ptr comm) : + thread_enable_(false) { + comm_ = std::move(comm); } Hardware::~Hardware() { @@ -65,13 +70,25 @@ bool Hardware::load_config_file(const std::string& config_yaml) { return false; } + if (!search_unsupport_indirect_addr_hw_type(group_name)) { + std::cerr << group_name << "のindirect ADDR/DATAを利用しません." << std::endl; + } + if (!create_sync_read_group(group_name)) { std::cerr << group_name << "のsync readグループを作成できません." << std::endl; return false; } - if (!create_sync_write_group(group_name)) { - std::cerr << group_name << "のsync writeグループを作成できません." << std::endl; - return false; + + if(!use_direct_addr_enabled_){ + if (!create_sync_write_group(group_name)) { + std::cerr << group_name << "のsync writeグループを作成できません." << std::endl; + return false; + } + } else { + if (!create_sync_write_group_direct_addr(group_name)) { + std::cerr << group_name << "のsync writeグループを作成できません." << std::endl; + return false; + } } } @@ -179,12 +196,22 @@ bool Hardware::sync_read(const std::string& group_name) { if (joints_.group(group_name)->sync_read_position_enabled()) { for (const auto & joint_name : joints_.group(group_name)->joint_names()) { double position = 0.0; - if (joints_.joint(joint_name)->dxl->extract_present_position_from_sync_read( - comm_, group_name, position)) { - joints_.joint(joint_name)->set_present_position(position); - } else { - std::cerr << joint_name << "のpresent_positionを取得できません." << std::endl; - retval = false; + if(!use_direct_addr_enabled_) { + if (joints_.joint(joint_name)->dxl->extract_present_position_from_sync_read( + comm_, group_name, position)) { + joints_.joint(joint_name)->set_present_position(position); + } else { + std::cerr << joint_name << "のpresent_positionを取得できません." << std::endl; + retval = false; + } + }else{ + if (joints_.joint(joint_name)->dxl->extract_default_position_from_sync_read( + comm_, group_name, position)) { + joints_.joint(joint_name)->set_present_position(position); + } else { + std::cerr << joint_name << "のpresent_positionを取得できません." << std::endl; + retval = false; + } } } } @@ -192,12 +219,22 @@ bool Hardware::sync_read(const std::string& group_name) { if (joints_.group(group_name)->sync_read_velocity_enabled()) { for (const auto & joint_name : joints_.group(group_name)->joint_names()) { double velocity = 0.0; - if (joints_.joint(joint_name)->dxl->extract_present_velocity_from_sync_read( - comm_, group_name, velocity)) { - joints_.joint(joint_name)->set_present_velocity(velocity); - } else { - std::cerr << joint_name << "のpresent_velocityを取得できません." << std::endl; - retval = false; + if(!use_direct_addr_enabled_) { + if (joints_.joint(joint_name)->dxl->extract_present_velocity_from_sync_read( + comm_, group_name, velocity)) { + joints_.joint(joint_name)->set_present_velocity(velocity); + } else { + std::cerr << joint_name << "のpresent_velocityを取得できません." << std::endl; + retval = false; + } + }else{ + if (joints_.joint(joint_name)->dxl->extract_default_velocity_from_sync_read( + comm_, group_name, velocity)) { + joints_.joint(joint_name)->set_present_velocity(velocity); + } else { + std::cerr << joint_name << "のpresent_velocityを取得できません." << std::endl; + retval = false; + } } } } @@ -205,12 +242,22 @@ bool Hardware::sync_read(const std::string& group_name) { if (joints_.group(group_name)->sync_read_current_enabled()) { for (const auto & joint_name : joints_.group(group_name)->joint_names()) { double current = 0.0; - if (joints_.joint(joint_name)->dxl->extract_present_current_from_sync_read( - comm_, group_name, current)) { - joints_.joint(joint_name)->set_present_current(current); - } else { - std::cerr << joint_name << "のpresent_currentを取得できません." << std::endl; - retval = false; + if(!use_direct_addr_enabled_){ + if (joints_.joint(joint_name)->dxl->extract_present_current_from_sync_read( + comm_, group_name, current)) { + joints_.joint(joint_name)->set_present_current(current); + } else { + std::cerr << joint_name << "のpresent_currentを取得できません." << std::endl; + retval = false; + } + }else{ + if (joints_.joint(joint_name)->dxl->extract_default_current_from_sync_read( + comm_, group_name, current)) { + joints_.joint(joint_name)->set_present_current(current); + } else { + std::cerr << joint_name << "のpresent_currentを取得できません." << std::endl; + retval = false; + } } } } @@ -218,12 +265,22 @@ bool Hardware::sync_read(const std::string& group_name) { if (joints_.group(group_name)->sync_read_voltage_enabled()) { for (const auto & joint_name : joints_.group(group_name)->joint_names()) { double voltage = 0.0; - if (joints_.joint(joint_name)->dxl->extract_present_input_voltage_from_sync_read( - comm_, group_name, voltage)) { - joints_.joint(joint_name)->set_present_voltage(voltage); - } else { - std::cerr << joint_name << "のpresent_voltageを取得できません." << std::endl; - retval = false; + if(!use_direct_addr_enabled_){ + if (joints_.joint(joint_name)->dxl->extract_present_input_voltage_from_sync_read( + comm_, group_name, voltage)) { + joints_.joint(joint_name)->set_present_voltage(voltage); + } else { + std::cerr << joint_name << "のpresent_voltageを取得できません." << std::endl; + retval = false; + } + }else{ + if (joints_.joint(joint_name)->dxl->extract_default_input_voltage_from_sync_read( + comm_, group_name, voltage)) { + joints_.joint(joint_name)->set_present_voltage(voltage); + } else { + std::cerr << joint_name << "のpresent_voltageを取得できません." << std::endl; + retval = false; + } } } } @@ -231,14 +288,55 @@ bool Hardware::sync_read(const std::string& group_name) { if (joints_.group(group_name)->sync_read_temperature_enabled()) { for (const auto & joint_name : joints_.group(group_name)->joint_names()) { int temperature = 0; - if (joints_.joint(joint_name)->dxl->extract_present_temperature_from_sync_read( - comm_, group_name, temperature)) { - joints_.joint(joint_name)->set_present_temperature(temperature); + if(!use_direct_addr_enabled_){ + if (joints_.joint(joint_name)->dxl->extract_present_temperature_from_sync_read( + comm_, group_name, temperature)) { + joints_.joint(joint_name)->set_present_temperature(temperature); + } else { + std::cerr << joint_name << "のpresent_temperatureを取得できません." << std::endl; + retval = false; + } + }else{ + if (joints_.joint(joint_name)->dxl->extract_default_temperature_from_sync_read( + comm_, group_name, temperature)) { + joints_.joint(joint_name)->set_present_temperature(temperature); + } else { + std::cerr << joint_name << "のpresent_temperatureを取得できません." << std::endl; + retval = false; + } + } + } + } + + auto read_external_port = [&, this](const int number) { + for (const auto & joint_name : joints_.group(group_name)->joint_names()) { + double voltage = 0.0; + if (joints_.joint(joint_name)->dxl->extract_external_port_from_sync_read( + comm_, group_name, number, voltage)) { + joints_.joint(joint_name)->set_external_port_voltage(number, voltage); } else { - std::cerr << joint_name << "のpresent_temperatureを取得できません." << std::endl; - retval = false; + std::cerr << joint_name << "のexternal_port" << std::to_string(number); + std::cerr << "を取得できません." << std::endl; + return false; } } + return true; + }; + + if (joints_.group(group_name)->sync_read_external_port1_enabled()) { + retval = read_external_port(1); + } + + if (joints_.group(group_name)->sync_read_external_port2_enabled()) { + retval = read_external_port(2); + } + + if (joints_.group(group_name)->sync_read_external_port3_enabled()) { + retval = read_external_port(3); + } + + if (joints_.group(group_name)->sync_read_external_port4_enabled()) { + retval = read_external_port(4); } return retval; @@ -254,19 +352,57 @@ bool Hardware::sync_write(const std::string& group_name) { for (const auto & joint_name : joints_.group(group_name)->joint_names()) { std::vector write_data; - if (joints_.group(group_name)->sync_write_position_enabled()) { - joints_.joint(joint_name)->dxl->push_back_position_for_sync_write( - joints_.joint(joint_name)->get_goal_position(), write_data); - } + if (!use_direct_addr_enabled_) { + if (joints_.group(group_name)->sync_write_position_enabled()) { + joints_.joint(joint_name)->dxl->push_back_position_for_sync_write( + joints_.joint(joint_name)->get_goal_position(), write_data); + } - if (joints_.group(group_name)->sync_write_velocity_enabled()) { - joints_.joint(joint_name)->dxl->push_back_velocity_for_sync_write( - joints_.joint(joint_name)->get_goal_velocity(), write_data); - } + if (joints_.group(group_name)->sync_write_velocity_enabled()) { + joints_.joint(joint_name)->dxl->push_back_velocity_for_sync_write( + joints_.joint(joint_name)->get_goal_velocity(), write_data); + } + + if (joints_.group(group_name)->sync_write_current_enabled()) { + joints_.joint(joint_name)->dxl->push_back_current_for_sync_write( + joints_.joint(joint_name)->get_goal_current(), write_data); + } + } else { + if (joints_.group(group_name)->sync_write_current_enabled()) { + joints_.joint(joint_name)->dxl->push_back_current_for_sync_write( + joints_.joint(joint_name)->get_goal_current(), write_data); + } else { + if (use_direct_addr_enabled_){ + joints_.joint(joint_name)->dxl->push_back_current_for_sync_write( + 0, write_data); + } + } + + if (joints_.group(group_name)->sync_write_velocity_enabled()) { + joints_.joint(joint_name)->dxl->push_back_velocity_for_sync_write( + joints_.joint(joint_name)->get_goal_velocity(), write_data); + } else { + if (use_direct_addr_enabled_){ + joints_.joint(joint_name)->dxl->push_back_velocity_for_sync_write( + 0, write_data); + } + } + + if (use_direct_addr_enabled_){ + joints_.joint(joint_name)->dxl->push_back_profile_for_sync_write( + 0, write_data); + } + + if (joints_.group(group_name)->sync_write_position_enabled()) { + joints_.joint(joint_name)->dxl->push_back_position_for_sync_write( + joints_.joint(joint_name)->get_goal_position(), write_data); + } else { + if (use_direct_addr_enabled_){ + joints_.joint(joint_name)->dxl->push_back_position_for_sync_write( + 0, write_data); + } + } - if (joints_.group(group_name)->sync_write_current_enabled()) { - joints_.joint(joint_name)->dxl->push_back_current_for_sync_write( - joints_.joint(joint_name)->get_goal_current(), write_data); } auto id = joints_.joint(joint_name)->id(); @@ -414,6 +550,15 @@ bool Hardware::get_min_position_limit(const uint8_t & id, double & min_position_ return joints_.get_min_position_limit(id, min_position_limit); } +bool Hardware::get_external_port_voltage(const uint8_t id, const int number, double& voltage) { + return joints_.get_external_port_voltage(id, number, voltage); +} + +bool Hardware::get_external_port_voltage( + const std::string& joint_name, const int number, double& voltage) { + return joints_.get_external_port_voltage(joint_name, number, voltage); +} + bool Hardware::set_position(const uint8_t id, const double position) { return joints_.set_position(id, position); } @@ -715,68 +860,133 @@ bool Hardware::limit_goal_current_by_present_position(const std::string& group_n return retval; } +bool Hardware::search_unsupport_indirect_addr_hw_type(const std::string& group_name) { + bool retval = true; + for (const auto & joint_name : joints_.group(group_name)->joint_names()) { + if (joints_.joint(joint_name)->dxl->get_name() == "XC330") { + use_direct_addr_enabled_ = true; + retval = false; + } + } + + return retval; +} + bool Hardware::create_sync_read_group(const std::string& group_name) { // HardwareCommunicatorに、指定されたデータを読むSyncReadGroupを追加する // できるだけ多くのデータをSyncReadで読み取るため、インダイレクトアドレスを活用する - if (joints_.group(group_name)->sync_read_position_enabled()) { - for (const auto & joint_name : joints_.group(group_name)->joint_names()) { - if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_position(comm_)) { - std::cerr << joint_name << "ジョイントの" << std::endl; - std::cerr << "present_positionをindirect addressにセットできません." << std::endl; - return false; + if (!use_direct_addr_enabled_) { + if (joints_.group(group_name)->sync_read_position_enabled()) { + for (const auto & joint_name : joints_.group(group_name)->joint_names()) { + if (joints_.joint(joint_name)->dxl->get_name() != "XC330") { + if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_position(comm_)) { + std::cerr << joint_name << "ジョイントの" << std::endl; + std::cerr << "present_positionをindirect addressにセットできません." << std::endl; + return false; + } + } } } - } - if (joints_.group(group_name)->sync_read_velocity_enabled()) { - for (const auto & joint_name : joints_.group(group_name)->joint_names()) { - if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_velocity(comm_)) { - std::cerr << joint_name << "ジョイントの" << std::endl; - std::cerr << "present_velocityをindirect addressにセットできません." << std::endl; - return false; + if (joints_.group(group_name)->sync_read_velocity_enabled()) { + for (const auto & joint_name : joints_.group(group_name)->joint_names()) { + if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_velocity(comm_)) { + std::cerr << joint_name << "ジョイントの" << std::endl; + std::cerr << "present_velocityをindirect addressにセットできません." << std::endl; + return false; + } } } - } - if (joints_.group(group_name)->sync_read_current_enabled()) { - for (const auto & joint_name : joints_.group(group_name)->joint_names()) { - if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_current(comm_)) { - std::cerr << joint_name << "ジョイントの" << std::endl; - std::cerr << "present_currentをindirect addressにセットできません." << std::endl; - return false; + if (joints_.group(group_name)->sync_read_current_enabled()) { + for (const auto & joint_name : joints_.group(group_name)->joint_names()) { + if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_current(comm_)) { + std::cerr << joint_name << "ジョイントの" << std::endl; + std::cerr << "present_currentをindirect addressにセットできません." << std::endl; + return false; + } } } - } - if (joints_.group(group_name)->sync_read_voltage_enabled()) { - for (const auto & joint_name : joints_.group(group_name)->joint_names()) { - if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_input_voltage( - comm_)) { - std::cerr << joint_name << "ジョイントの" << std::endl; - std::cerr << "present_input_voltageをindirect addressにセットできません." << std::endl; - return false; + if (joints_.group(group_name)->sync_read_voltage_enabled()) { + for (const auto & joint_name : joints_.group(group_name)->joint_names()) { + if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_input_voltage( + comm_)) { + std::cerr << joint_name << "ジョイントの" << std::endl; + std::cerr << "present_input_voltageをindirect addressにセットできません." << std::endl; + return false; + } } } - } - if (joints_.group(group_name)->sync_read_temperature_enabled()) { - for (const auto & joint_name : joints_.group(group_name)->joint_names()) { - if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_temperature( - comm_)) { - std::cerr << joint_name << "ジョイントの" << std::endl; - std::cerr << "present_temperatureをindirect addressにセットできません." << std::endl; + if (joints_.group(group_name)->sync_read_temperature_enabled()) { + for (const auto & joint_name : joints_.group(group_name)->joint_names()) { + if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_present_temperature( + comm_)) { + std::cerr << joint_name << "ジョイントの" << std::endl; + std::cerr << "present_temperatureをindirect addressにセットできません." << std::endl; + return false; + } + } + } + + auto set_external_port = [&, this](const int number) { + for (const auto & joint_name : joints_.group(group_name)->joint_names()) { + if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_external_port( + comm_, number)) { + std::cerr << joint_name << "ジョイントの" << std::endl; + std::cerr << "external_port" << std::to_string(number); + std::cerr << "をindirect addressにセットできません." << std::endl; + return false; + } + + if (!joints_.joint(joint_name)->dxl->set_external_port_mode_to_analog_input( + comm_, number)) { + std::cerr << joint_name << "ジョイントの" << std::endl; + std::cerr << "external_port" << std::to_string(number); + std::cerr << "をアナログ入力モードにセットできません." << std::endl; + return false; + } + } + return true; + }; + + if (joints_.group(group_name)->sync_read_external_port1_enabled()) { + if (!set_external_port(1)) { + return false; + } + } + if (joints_.group(group_name)->sync_read_external_port2_enabled()) { + if (!set_external_port(2)) { + return false; + } + } + if (joints_.group(group_name)->sync_read_external_port3_enabled()) { + if (!set_external_port(3)) { + return false; + } + } + if (joints_.group(group_name)->sync_read_external_port4_enabled()) { + if (!set_external_port(4)) { return false; } } - } - // 代表1ジョイントを抽出し、sync_readの開始アドレスとデータ長を取得する - const auto a_name = joints_.group(group_name)->joint_names().front(); - comm_->make_sync_read_group( - group_name, - joints_.joint(a_name)->dxl->start_address_for_indirect_read(), - joints_.joint(a_name)->dxl->length_of_indirect_data_read()); + // 代表1ジョイントを抽出し、sync_readの開始アドレスとデータ長を取得する + const auto a_name = joints_.group(group_name)->joint_names().front(); + comm_->make_sync_read_group( + group_name, + joints_.joint(a_name)->dxl->start_address_for_indirect_read(), + joints_.joint(a_name)->dxl->length_of_indirect_data_read()); + } else { + // indirectではないのでデータを一括で取得する範囲で設定 + const auto a_name = joints_.group(group_name)->joint_names().front(); + comm_->make_sync_read_group( + group_name, + joints_.joint(a_name)->dxl->start_address_for_direct_read(), + joints_.joint(a_name)->dxl->length_of_direct_data_read()); + } for (const auto & joint_name : joints_.group(group_name)->joint_names()) { auto id = joints_.joint(joint_name)->id(); @@ -791,7 +1001,6 @@ bool Hardware::create_sync_read_group(const std::string& group_name) { bool Hardware::create_sync_write_group(const std::string& group_name) { // HardwareCommunicatorに、指定されたデータを書き込むSyncWriteGroupを追加する // できるだけ多くのデータをSyncWriteで書き込むため、インダイレクトアドレスを活用する - if (joints_.group(group_name)->sync_write_position_enabled()) { for (const auto & joint_name : joints_.group(group_name)->joint_names()) { if (!joints_.joint(joint_name)->dxl->auto_set_indirect_address_of_goal_position(comm_)) { @@ -826,10 +1035,28 @@ bool Hardware::create_sync_write_group(const std::string& group_name) { const auto a_name = joints_.group(group_name)->joint_names().front(); const auto length = joints_.joint(a_name)->dxl->length_of_indirect_data_write(); comm_->make_sync_write_group( - group_name, - joints_.joint(a_name)->dxl->start_address_for_indirect_write(), - length); + group_name, + joints_.joint(a_name)->dxl->start_address_for_indirect_write(), + length); + std::vector init_data(length, 0); + for (const auto & joint_name : joints_.group(group_name)->joint_names()) { + auto id = joints_.joint(joint_name)->id(); + if (!comm_->append_id_to_sync_write_group(group_name, id, init_data)) { + return false; + } + } + return true; +} + +bool Hardware::create_sync_write_group_direct_addr(const std::string& group_name) { + // 代表1ジョイントを抽出し、sync_readの開始アドレスとデータ長を取得する + const auto a_name = joints_.group(group_name)->joint_names().front(); + const auto length = joints_.joint(a_name)->dxl->length_of_direct_data_write(); + comm_->make_sync_write_group( + group_name, + joints_.joint(a_name)->dxl->start_address_for_direct_write(), + length); std::vector init_data(length, 0); for (const auto & joint_name : joints_.group(group_name)->joint_names()) { auto id = joints_.joint(joint_name)->id(); @@ -844,7 +1071,6 @@ bool Hardware::create_sync_write_group(const std::string& group_name) { void Hardware::read_write_thread(const std::vector& group_names, const std::chrono::milliseconds& update_cycle_ms) { // sync_read、sync_writeを繰り返すスレッド - static auto current_time = std::chrono::steady_clock::now(); auto next_start_time = current_time; while (thread_enable_) { diff --git a/rt_manipulators_lib/src/hardware_communicator.cpp b/rt_manipulators_lib/src/hardware_communicator.cpp index 5d98c1a..829df74 100644 --- a/rt_manipulators_lib/src/hardware_communicator.cpp +++ b/rt_manipulators_lib/src/hardware_communicator.cpp @@ -20,12 +20,14 @@ namespace hardware_communicator { const double PROTOCOL_VERSION = 2.0; +auto packet_handler = []() { + return dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION); +}; + Communicator::Communicator(const std::string device_name) : is_connected_(false) { port_handler_ = std::shared_ptr( dynamixel::PortHandler::getPortHandler(device_name.c_str())); - packet_handler_ = std::shared_ptr( - dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION)); } Communicator::~Communicator() { @@ -60,7 +62,7 @@ void Communicator::make_sync_read_group( const group_name_t & group_name, const dxl_address_t & start_address, const dxl_data_length_t & data_length) { auto group_ptr = std::make_shared( - port_handler_.get(), packet_handler_.get(), start_address, data_length); + port_handler_.get(), packet_handler(), start_address, data_length); sync_read_groups_.emplace(group_name, group_ptr); } @@ -68,7 +70,7 @@ void Communicator::make_sync_write_group( const group_name_t & group_name, const dxl_address_t & start_address, const dxl_data_length_t & data_length) { auto group_ptr = std::make_shared( - port_handler_.get(), packet_handler_.get(), start_address, data_length); + port_handler_.get(), packet_handler(), start_address, data_length); sync_write_groups_.emplace(group_name, group_ptr); } @@ -153,7 +155,7 @@ bool Communicator::write_byte_data( const dxl_id_t & id, const dxl_address_t & address, const dxl_byte_t & write_data) { dxl_error_t dxl_error = 0; dxl_result_t dxl_result = - packet_handler_->write1ByteTxRx(port_handler_.get(), id, address, write_data, &dxl_error); + packet_handler()->write1ByteTxRx(port_handler_.get(), id, address, write_data, &dxl_error); if (!parse_dxl_error(std::string(__func__), id, address, dxl_result, dxl_error)) { return false; @@ -165,7 +167,7 @@ bool Communicator::write_word_data( const dxl_id_t & id, const dxl_address_t & address, const dxl_word_t & write_data) { dxl_error_t dxl_error = 0; dxl_result_t dxl_result = - packet_handler_->write2ByteTxRx(port_handler_.get(), id, address, write_data, &dxl_error); + packet_handler()->write2ByteTxRx(port_handler_.get(), id, address, write_data, &dxl_error); if (!parse_dxl_error(std::string(__func__), id, address, dxl_result, dxl_error)) { return false; @@ -177,7 +179,7 @@ bool Communicator::write_double_word_data( const dxl_id_t & id, const dxl_address_t & address, const dxl_double_word_t & write_data) { dxl_error_t dxl_error = 0; dxl_result_t dxl_result = - packet_handler_->write4ByteTxRx(port_handler_.get(), id, address, write_data, &dxl_error); + packet_handler()->write4ByteTxRx(port_handler_.get(), id, address, write_data, &dxl_error); if (!parse_dxl_error(std::string(__func__), id, address, dxl_result, dxl_error)) { return false; @@ -190,7 +192,7 @@ bool Communicator::read_byte_data( dxl_error_t dxl_error = 0; dxl_byte_t data = 0; dxl_result_t dxl_result = - packet_handler_->read1ByteTxRx(port_handler_.get(), id, address, &data, &dxl_error); + packet_handler()->read1ByteTxRx(port_handler_.get(), id, address, &data, &dxl_error); if (!parse_dxl_error(std::string(__func__), id, address, dxl_result, dxl_error)) { return false; @@ -204,7 +206,7 @@ bool Communicator::read_word_data( dxl_error_t dxl_error = 0; dxl_word_t data = 0; dxl_result_t dxl_result = - packet_handler_->read2ByteTxRx(port_handler_.get(), id, address, &data, &dxl_error); + packet_handler()->read2ByteTxRx(port_handler_.get(), id, address, &data, &dxl_error); if (!parse_dxl_error(std::string(__func__), id, address, dxl_result, dxl_error)) { return false; @@ -218,7 +220,7 @@ bool Communicator::read_double_word_data( dxl_error_t dxl_error = 0; dxl_double_word_t data = 0; dxl_result_t dxl_result = - packet_handler_->read4ByteTxRx(port_handler_.get(), id, address, &data, &dxl_error); + packet_handler()->read4ByteTxRx(port_handler_.get(), id, address, &data, &dxl_error); if (!parse_dxl_error(std::string(__func__), id, address, dxl_result, dxl_error)) { return false; @@ -252,7 +254,7 @@ bool Communicator::parse_dxl_error( std::cerr << "Function:" << func_name; std::cerr << ", ID:" << std::to_string(id); std::cerr << ", Address:" << std::to_string(address); - std::cerr << ", CommError:" << std::string(packet_handler_->getTxRxResult(dxl_comm_result)) + std::cerr << ", CommError:" << std::string(packet_handler()->getTxRxResult(dxl_comm_result)) << std::endl; retval = false; } @@ -262,7 +264,7 @@ bool Communicator::parse_dxl_error( std::cerr << ", ID:" << std::to_string(id); std::cerr << ", Address:" << std::to_string(address); std::cerr << ", PacketError:" - << std::string(packet_handler_->getRxPacketError(dxl_packet_error)) << std::endl; + << std::string(packet_handler()->getRxPacketError(dxl_packet_error)) << std::endl; retval = false; } @@ -273,7 +275,7 @@ bool Communicator::parse_dxl_error( const std::string & func_name, const dxl_result_t & dxl_comm_result) { if (dxl_comm_result != COMM_SUCCESS) { std::cerr << "Function:" << func_name; - std::cerr << ", CommError:" << std::string(packet_handler_->getTxRxResult(dxl_comm_result)); + std::cerr << ", CommError:" << std::string(packet_handler()->getTxRxResult(dxl_comm_result)); std::cerr << std::endl; return false; } diff --git a/rt_manipulators_lib/src/hardware_joints.cpp b/rt_manipulators_lib/src/hardware_joints.cpp index dfdde3f..4f323b1 100644 --- a/rt_manipulators_lib/src/hardware_joints.cpp +++ b/rt_manipulators_lib/src/hardware_joints.cpp @@ -229,6 +229,25 @@ bool Joints::get_min_position_limit(const dxl_id_t & id, position_t & min_positi return true; } +bool Joints::get_external_port_voltage(const dxl_id_t & id, const int number, double& voltage) { + if (!has_joint(id)) { + std::cerr << "ID:" << std::to_string(id) << "のジョイントは存在しません." << std::endl; + return false; + } + voltage = joint(id)->get_external_port_voltage(number); + return true; +} + +bool Joints::get_external_port_voltage( + const joint_name_t & joint_name, const int number, double& voltage) { + if (!has_joint(joint_name)) { + std::cerr << joint_name << "ジョイントは存在しません." << std::endl; + return false; + } + voltage = joint(joint_name)->get_external_port_voltage(number); + return true; +} + bool Joints::set_position(const dxl_id_t & id, const position_t & position) { if (!has_joint(id)) { std::cerr << "ID:" << std::to_string(id) << "のジョイントは存在しません." << std::endl; diff --git a/rt_manipulators_lib/src/joint.cpp b/rt_manipulators_lib/src/joint.cpp index 034d1bf..99724b8 100644 --- a/rt_manipulators_lib/src/joint.cpp +++ b/rt_manipulators_lib/src/joint.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "dynamixel_xc330.hpp" #include "dynamixel_xm430.hpp" #include "dynamixel_xm540.hpp" #include "dynamixel_xh430.hpp" #include "dynamixel_xh540.hpp" #include "dynamixel_ph42.hpp" +#include "dynamixel_ph54.hpp" #include "joint.hpp" namespace joint { @@ -37,6 +39,8 @@ Joint::Joint(const uint8_t id, const uint8_t operating_mode, const std::string d : Joint(id, operating_mode) { if (dynamixel_name == "XM430") { dxl = std::make_shared(id); + } else if (dynamixel_name == "XC330") { + dxl = std::make_shared(id); } else if (dynamixel_name == "XM540") { dxl = std::make_shared(id); } else if (dynamixel_name == "XH430") { @@ -45,6 +49,8 @@ Joint::Joint(const uint8_t id, const uint8_t operating_mode, const std::string d dxl = std::make_shared(id); } else if (dynamixel_name == "PH42") { dxl = std::make_shared(id); + } else if (dynamixel_name == "PH54") { + dxl = std::make_shared(id); } else { dxl = std::make_shared(id); } @@ -128,6 +134,20 @@ double Joint::get_goal_velocity() const { return goal_velocity_; } double Joint::get_goal_current() const { return goal_current_; } +void Joint::set_external_port_voltage(const int number, const double analog_voltage) { + if (number < 1 || number > 4) { + return; + } + external_port_voltage_[number - 1] = analog_voltage; +} + +double Joint::get_external_port_voltage(const int number) const { + if (number < 1 || number > 4) { + return 0; + } + return external_port_voltage_.at(number - 1); +} + JointGroup::JointGroup(const std::vector& joint_names, const std::vector& sync_read_targets, const std::vector& sync_write_targets) @@ -146,6 +166,10 @@ JointGroup::JointGroup(const std::vector& joint_names, if (target == "current") sync_read_current_enabled_ = true; if (target == "voltage") sync_read_voltage_enabled_ = true; if (target == "temperature") sync_read_temperature_enabled_ = true; + if (target == "external_port1") sync_read_external_port1_enabled_ = true; + if (target == "external_port2") sync_read_external_port2_enabled_ = true; + if (target == "external_port3") sync_read_external_port3_enabled_ = true; + if (target == "external_port4") sync_read_external_port4_enabled_ = true; } for (const auto & target : sync_write_targets) { @@ -173,4 +197,20 @@ bool JointGroup::sync_write_velocity_enabled() const { return sync_write_velocit bool JointGroup::sync_write_current_enabled() const { return sync_write_current_enabled_; } +bool JointGroup::sync_read_external_port1_enabled() const { + return sync_read_external_port1_enabled_; +} + +bool JointGroup::sync_read_external_port2_enabled() const { + return sync_read_external_port2_enabled_; +} + +bool JointGroup::sync_read_external_port3_enabled() const { + return sync_read_external_port3_enabled_; +} + +bool JointGroup::sync_read_external_port4_enabled() const { + return sync_read_external_port4_enabled_; +} + } // namespace joint diff --git a/rt_manipulators_lib/src/kinematics_utils.cpp b/rt_manipulators_lib/src/kinematics_utils.cpp index 6efe09b..32b54cd 100644 --- a/rt_manipulators_lib/src/kinematics_utils.cpp +++ b/rt_manipulators_lib/src/kinematics_utils.cpp @@ -66,6 +66,16 @@ std::vector parse_link_config_file(const std::string & std::cout << "リンク情報ファイル:" << file_path << "を読み込みます" << std::endl; + bool no_coordinate_transformation = false; + if(file_path.find("bonobo") != std::string::npos){ + std::cout << "bonobo_links" << std::endl; + no_coordinate_transformation = true; + } + if(file_path.find("muriqui") != std::string::npos){ + std::cout << "muriqui_links" << std::endl; + no_coordinate_transformation = true; + } + std::vector links; links.push_back(manipulators_link::Link()); // 0番目には空のリンクをセット std::ifstream ifs(file_path); @@ -157,8 +167,12 @@ std::vector parse_link_config_file(const std::string & rot = rotation_from_euler_ZYX(0, 0, M_PI); link.a << 0, 0, -1; } - link.c = rot * link.c; - link.I = rot * link.I * rot.transpose(); + + // 読み込むモデルによって、座標系の設定が違うので場合分け + if (!no_coordinate_transformation){ + link.c = rot * link.c; + link.I = rot * link.I * rot.transpose(); + } try { link.dxl_id = std::stoi(str_vec[COL_DXL_ID]); diff --git a/rt_manipulators_lib/test/CMakeLists.txt b/rt_manipulators_lib/test/CMakeLists.txt index 25e5a80..f1c5785 100644 --- a/rt_manipulators_lib/test/CMakeLists.txt +++ b/rt_manipulators_lib/test/CMakeLists.txt @@ -22,7 +22,20 @@ set(list_tests test_dynamixel_x test_dynamixel_xh test_dynamixel_p + test_hardware + test_dynamixel_ph54 ) + +# Download FakeIt +Set(FETCHCONTENT_QUIET FALSE) +include(FetchContent) +FetchContent_Declare( + fakeit + GIT_REPOSITORY https://github.com/eranpeer/FakeIt + GIT_TAG 2.4.0 + GIT_PROGRESS TRUE) +FetchContent_MakeAvailable(fakeit) + foreach(test_executable IN LISTS list_tests) message("${test_executable}") add_executable(${test_executable} @@ -33,5 +46,8 @@ foreach(test_executable IN LISTS list_tests) GTest::Main rt_manipulators_cpp ) + target_include_directories(${test_executable} PRIVATE + ${fakeit_SOURCE_DIR}/single_header/gtest + ) gtest_discover_tests(${test_executable}) endforeach() diff --git a/rt_manipulators_lib/test/config/ok_read_external_port.yaml b/rt_manipulators_lib/test/config/ok_read_external_port.yaml new file mode 100644 index 0000000..5208e52 --- /dev/null +++ b/rt_manipulators_lib/test/config/ok_read_external_port.yaml @@ -0,0 +1,11 @@ +joint_groups: + test_group: + joints: + - joint1 + sync_read: + - external_port1 + - external_port2 + - external_port3 + - external_port4 + +joint1: { id: 1, dynamixel: "PH42", operating_mode: 3} diff --git a/rt_manipulators_lib/test/test_config_file_parser.cpp b/rt_manipulators_lib/test/test_config_file_parser.cpp index 04ac34e..91e1402 100644 --- a/rt_manipulators_lib/test/test_config_file_parser.cpp +++ b/rt_manipulators_lib/test/test_config_file_parser.cpp @@ -129,3 +129,14 @@ TEST(ConfigFileParserTest, write_current_without_reading_position) { "../config/ng_write_current_without_reading_position.yaml", parsed_joints)); } + +TEST(ConfigFileParserTest, read_external_port) { + // グループのsync_readにexternal_portがセットされていることを期待 + hardware_joints::Joints parsed_joints; + ASSERT_TRUE(config_file_parser::parse("../config/ok_read_external_port.yaml", parsed_joints)); + EXPECT_EQ(parsed_joints.groups().size(), 1); + EXPECT_TRUE(parsed_joints.group("test_group")->sync_read_external_port1_enabled()); + EXPECT_TRUE(parsed_joints.group("test_group")->sync_read_external_port2_enabled()); + EXPECT_TRUE(parsed_joints.group("test_group")->sync_read_external_port3_enabled()); + EXPECT_TRUE(parsed_joints.group("test_group")->sync_read_external_port4_enabled()); +} diff --git a/rt_manipulators_lib/test/test_dynamixel_p.cpp b/rt_manipulators_lib/test/test_dynamixel_p.cpp index d11e283..fa080b4 100644 --- a/rt_manipulators_lib/test/test_dynamixel_p.cpp +++ b/rt_manipulators_lib/test/test_dynamixel_p.cpp @@ -195,6 +195,34 @@ TEST_F(PTestFixture, set_indirect_addresses_read) { EXPECT_EQ(dxl->indirect_addr_of_present_temperature(), 646); } +TEST_F(PTestFixture, set_indirect_addresses_read_for_external_port) { + EXPECT_EQ(dxl->start_address_for_indirect_read(), 634); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 0); + EXPECT_EQ(dxl->next_indirect_addr_read(), 168); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_external_port(comm, 1)); + // indirect_dataの開始位置は変わらないことを期待 + EXPECT_EQ(dxl->start_address_for_indirect_read(), 634); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 2); + EXPECT_EQ(dxl->next_indirect_addr_read(), 172); + EXPECT_EQ(dxl->indirect_addr_of_external_port(1), 634); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_external_port(comm, 2)); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 4); + EXPECT_EQ(dxl->next_indirect_addr_read(), 176); + EXPECT_EQ(dxl->indirect_addr_of_external_port(2), 636); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_external_port(comm, 3)); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 6); + EXPECT_EQ(dxl->next_indirect_addr_read(), 180); + EXPECT_EQ(dxl->indirect_addr_of_external_port(3), 638); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_external_port(comm, 4)); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 8); + EXPECT_EQ(dxl->next_indirect_addr_read(), 184); + EXPECT_EQ(dxl->indirect_addr_of_external_port(4), 640); +} + TEST_F(PTestFixture, set_indirect_addresses_write) { EXPECT_EQ(dxl->start_address_for_indirect_write(), 649); EXPECT_EQ(dxl->length_of_indirect_data_write(), 0); diff --git a/rt_manipulators_lib/test/test_dynamixel_ph54.cpp b/rt_manipulators_lib/test/test_dynamixel_ph54.cpp new file mode 100644 index 0000000..b1204c2 --- /dev/null +++ b/rt_manipulators_lib/test/test_dynamixel_ph54.cpp @@ -0,0 +1,61 @@ +// 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. + +#include +#include + +#include "gtest/gtest.h" +#include "rt_manipulators_cpp/dynamixel_base.hpp" +#include "rt_manipulators_cpp/dynamixel_ph54.hpp" + + +class PH54TestFixture : public ::testing::Test { + protected: + virtual void SetUp() { + dxl = std::make_shared(1); + } + + virtual void TearDown() { + dxl.reset(); + } + + std::shared_ptr dxl; +}; + +TEST_F(PH54TestFixture, create_ph54_instance) { + EXPECT_EQ(dxl->get_name(), "PH54"); +} + +TEST_F(PH54TestFixture, to_profile_acceleration) { + // rad/s^2 to rev/min^2 + // 0以下に対しては1を返すことを期待 + EXPECT_EQ(dxl->to_profile_acceleration(-1), 1); + EXPECT_EQ(dxl->to_profile_acceleration(0), 1); + EXPECT_EQ(dxl->to_profile_acceleration(0.017454), 10); + EXPECT_EQ(dxl->to_profile_acceleration(1000000), 4255632); +} + +TEST_F(PH54TestFixture, to_position_radian) { + EXPECT_DOUBLE_EQ(dxl->to_position_radian(0), 0.0); + // 250961 = 0x0003 D451 + // 250961 = 0xFFFC 2BAF + EXPECT_NEAR(dxl->to_position_radian(0x0003D451), M_PI_2, 0.0001); + EXPECT_NEAR(dxl->to_position_radian(0xFFFC2BAF), -M_PI_2, 0.0001); +} + +TEST_F(PH54TestFixture, from_position_radian) { + EXPECT_EQ(dxl->from_position_radian(0.0), 0); + EXPECT_EQ(dxl->from_position_radian(M_PI_2), 250961); + EXPECT_EQ(dxl->from_position_radian(-M_PI_2), -250961); +} diff --git a/rt_manipulators_lib/test/test_hardware.cpp b/rt_manipulators_lib/test/test_hardware.cpp new file mode 100644 index 0000000..e70f921 --- /dev/null +++ b/rt_manipulators_lib/test/test_hardware.cpp @@ -0,0 +1,178 @@ +// Copyright 2023 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 + +#include "fakeit.hpp" +#include "gtest/gtest.h" +#include "rt_manipulators_cpp/hardware.hpp" +#include "rt_manipulators_cpp/hardware_communicator.hpp" + +using fakeit::Mock; +using fakeit::Verify; +using fakeit::When; + +Mock create_comm_mock(void) { + Mock mock; + When(Method(mock, is_connected)).AlwaysReturn(true); + When(Method(mock, connect)).AlwaysReturn(true); + When(Method(mock, disconnect)).AlwaysReturn(); + When(Method(mock, make_sync_write_group)).AlwaysReturn(); + When(Method(mock, make_sync_read_group)).AlwaysReturn(); + When(Method(mock, append_id_to_sync_write_group)).AlwaysReturn(true); + When(Method(mock, append_id_to_sync_read_group)).AlwaysReturn(true); + When(Method(mock, send_sync_read_packet)).AlwaysReturn(true); + When(Method(mock, send_sync_write_packet)).AlwaysReturn(true); + When(Method(mock, get_sync_read_data)).AlwaysReturn(true); + When(Method(mock, set_sync_write_data)).AlwaysReturn(true); + When(Method(mock, write_byte_data)).AlwaysReturn(true); + When(Method(mock, write_word_data)).AlwaysReturn(true); + When(Method(mock, write_double_word_data)).AlwaysReturn(true); + When(Method(mock, read_byte_data)).AlwaysReturn(true); + When(Method(mock, read_word_data)).AlwaysReturn(true); + When(Method(mock, read_double_word_data)).AlwaysReturn(true); + + return mock; +} + +TEST(HardwareTest, load_config_file) { + // Expect the load_config_file method to be called twice and return true and false respectively. + auto mock = create_comm_mock(); + + rt_manipulators_cpp::Hardware hardware( + std::unique_ptr(&mock.get())); + + EXPECT_TRUE(hardware.load_config_file("../config/ok_has_dynamixel_name.yaml")); + EXPECT_FALSE(hardware.load_config_file("../config/ng_has_same_joints.yaml")); +} + +TEST(HardwareTest, connect) { + // Expect the connect method to be called twice and return true and false respectively. + auto mock = create_comm_mock(); + When(Method(mock, connect)).Return(true, false); // Return true then false. + + rt_manipulators_cpp::Hardware hardware( + std::unique_ptr(&mock.get())); + + EXPECT_TRUE(hardware.connect()); + EXPECT_FALSE(hardware.connect()); +} + +TEST(HardwareTest, disconnect) { + // Expect the disconnect method to be called once and never. + auto mock = create_comm_mock(); + When(Method(mock, is_connected)).Return(false).AlwaysReturn(true); // Return false then true. + When(Method(mock, disconnect)).AlwaysReturn(); + + rt_manipulators_cpp::Hardware hardware( + std::unique_ptr(&mock.get())); + + hardware.disconnect(); + Verify(Method(mock, disconnect)).Never(); + + hardware.disconnect(); + Verify(Method(mock, disconnect)).Once(); +} + +TEST(HardwareTest, write_data) { + auto mock = create_comm_mock(); + rt_manipulators_cpp::Hardware hardware( + std::unique_ptr(&mock.get())); + + EXPECT_TRUE(hardware.load_config_file("../config/ok_has_dynamixel_name.yaml")); + + // Return false when joint name or id is not found + mock.ClearInvocationHistory(); + EXPECT_FALSE(hardware.write_data("joint0", 0x00, static_cast(0x00))); + EXPECT_FALSE(hardware.write_data(0, 0x00, static_cast(0x00))); + Verify(Method(mock, write_byte_data)).Never(); + Verify(Method(mock, write_word_data)).Never(); + Verify(Method(mock, write_double_word_data)).Never(); + + // Identify joint via joint name + EXPECT_TRUE(hardware.write_data("joint1", 0x00, static_cast(0x00))); + Verify(Method(mock, write_byte_data)).Once(); + + // Identify joint via joint id + EXPECT_TRUE(hardware.write_data(2, 0x00, static_cast(0x00))); + Verify(Method(mock, write_word_data)).Once(); + + EXPECT_TRUE(hardware.write_data("joint3", 0x00, static_cast(0x00))); + Verify(Method(mock, write_double_word_data)).Once(); + + // Return false when data type is not matched + mock.ClearInvocationHistory(); + EXPECT_FALSE(hardware.write_data("joint1", 0x00, 0x00)); + Verify(Method(mock, write_byte_data)).Never(); + Verify(Method(mock, write_word_data)).Never(); + Verify(Method(mock, write_double_word_data)).Never(); +} + +TEST(HardwareTest, read_data) { + const uint8_t TEST_BYTE_DATA = 0x12; + const uint16_t TEST_WORD_DATA = 0x1234; + const uint32_t TEST_DOUBLE_WORD_DATA = 0x12345678; + auto mock = create_comm_mock(); + + When(Method(mock, read_byte_data)).AlwaysDo([](uint8_t id, uint16_t addr, uint8_t& result) { + result = TEST_BYTE_DATA; + return true; + }); + When(Method(mock, read_word_data)).AlwaysDo([](uint8_t id, uint16_t addr, uint16_t& result) { + result = TEST_WORD_DATA; + return true; + }); + When(Method(mock, read_double_word_data)).AlwaysDo([](uint8_t id, uint16_t addr, uint32_t& result) { + result = TEST_DOUBLE_WORD_DATA; + return true; + }); + + rt_manipulators_cpp::Hardware hardware( + std::unique_ptr(&mock.get())); + + EXPECT_TRUE(hardware.load_config_file("../config/ok_has_dynamixel_name.yaml")); + + uint8_t byte_data = 0x00; + uint16_t word_data = 0x00; + uint32_t double_word_data = 0x00; + // Return false when joint name or id is not found + mock.ClearInvocationHistory(); + EXPECT_FALSE(hardware.read_data("joint0", 0x00, byte_data)); + EXPECT_FALSE(hardware.read_data(0, 0x00, byte_data)); + Verify(Method(mock, read_byte_data)).Never(); + Verify(Method(mock, read_word_data)).Never(); + Verify(Method(mock, read_double_word_data)).Never(); + + // Identify joint via joint name + EXPECT_TRUE(hardware.read_data("joint1", 0x00, byte_data)); + Verify(Method(mock, read_byte_data)).Once(); + EXPECT_EQ(byte_data, TEST_BYTE_DATA); + + // Identify joint via joint id + EXPECT_TRUE(hardware.read_data(2, 0x00, word_data)); + Verify(Method(mock, read_word_data)).Once(); + EXPECT_EQ(word_data, TEST_WORD_DATA); + + EXPECT_TRUE(hardware.read_data("joint3", 0x00, double_word_data)); + Verify(Method(mock, read_double_word_data)).Once(); + EXPECT_EQ(double_word_data, TEST_DOUBLE_WORD_DATA); + + // Return false when data type is not matched + double invalid_type_data = 0.0; + mock.ClearInvocationHistory(); + EXPECT_FALSE(hardware.read_data("joint1", 0x00, invalid_type_data)); + Verify(Method(mock, read_byte_data)).Never(); + Verify(Method(mock, read_word_data)).Never(); + Verify(Method(mock, read_double_word_data)).Never(); +}