From 707e8d77a38ecb04604a572aca4e8259acdb5fb6 Mon Sep 17 00:00:00 2001 From: Shota Aoki Date: Wed, 30 Mar 2022 13:29:00 +0900 Subject: [PATCH] =?UTF-8?q?XH430=E3=80=81XH540=E3=80=81PH42=E3=82=AF?= =?UTF-8?q?=E3=83=A9=E3=82=B9=E3=82=92=E8=BF=BD=E5=8A=A0=20(#22)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * xh430, xh540クラスを追加 * ジョイントクラスにXHシリーズのインスタンス生成を追加 * PH42クラスを追加 * dynamixel_pのテストを追加 --- rt_manipulators_lib/include/dynamixel_p.hpp | 130 +++++ .../include/dynamixel_ph42.hpp | 29 ++ .../include/dynamixel_xh430.hpp | 31 ++ .../include/dynamixel_xh540.hpp | 29 ++ rt_manipulators_lib/src/CMakeLists.txt | 4 + rt_manipulators_lib/src/dynamixel_p.cpp | 449 ++++++++++++++++++ rt_manipulators_lib/src/dynamixel_ph42.cpp | 26 + rt_manipulators_lib/src/dynamixel_xh430.cpp | 37 ++ rt_manipulators_lib/src/dynamixel_xh540.cpp | 26 + rt_manipulators_lib/src/joint.cpp | 9 + rt_manipulators_lib/test/CMakeLists.txt | 2 + .../test/config/ok_has_dynamixel_name.yaml | 6 + .../test/test_config_file_parser.cpp | 3 + rt_manipulators_lib/test/test_dynamixel_p.cpp | 305 ++++++++++++ .../test/test_dynamixel_xh.cpp | 45 ++ 15 files changed, 1131 insertions(+) create mode 100644 rt_manipulators_lib/include/dynamixel_p.hpp create mode 100644 rt_manipulators_lib/include/dynamixel_ph42.hpp create mode 100644 rt_manipulators_lib/include/dynamixel_xh430.hpp create mode 100644 rt_manipulators_lib/include/dynamixel_xh540.hpp create mode 100644 rt_manipulators_lib/src/dynamixel_p.cpp create mode 100644 rt_manipulators_lib/src/dynamixel_ph42.cpp create mode 100644 rt_manipulators_lib/src/dynamixel_xh430.cpp create mode 100644 rt_manipulators_lib/src/dynamixel_xh540.cpp create mode 100644 rt_manipulators_lib/test/test_dynamixel_p.cpp create mode 100644 rt_manipulators_lib/test/test_dynamixel_xh.cpp diff --git a/rt_manipulators_lib/include/dynamixel_p.hpp b/rt_manipulators_lib/include/dynamixel_p.hpp new file mode 100644 index 0000000..430eed8 --- /dev/null +++ b/rt_manipulators_lib/include/dynamixel_p.hpp @@ -0,0 +1,130 @@ +// 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_P_HPP_ +#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_P_HPP_ + +#include +#include + +#include "dynamixel_base.hpp" + +namespace dynamixel_p { + +class DynamixelP : public dynamixel_base::DynamixelBase { + public: + explicit DynamixelP(const uint8_t id, const int home_position = 0); + + bool read_operating_mode(const dynamixel_base::comm_t & comm, uint8_t & mode); + bool write_operating_mode(const dynamixel_base::comm_t & comm, const uint8_t mode); + bool read_current_limit(const dynamixel_base::comm_t & comm, double & limit_ampere); + bool read_max_position_limit(const dynamixel_base::comm_t & comm, double & limit_radian); + bool read_min_position_limit(const dynamixel_base::comm_t & comm, double & limit_radian); + + bool write_torque_enable(const dynamixel_base::comm_t & comm, const bool enable); + + bool write_velocity_i_gain(const dynamixel_base::comm_t & comm, const unsigned int gain); + bool write_velocity_p_gain(const dynamixel_base::comm_t & comm, const unsigned int gain); + bool write_position_d_gain(const dynamixel_base::comm_t & comm, const unsigned int gain); + bool write_position_i_gain(const dynamixel_base::comm_t & comm, const unsigned int gain); + bool write_position_p_gain(const dynamixel_base::comm_t & comm, const unsigned int gain); + + bool write_profile_acceleration( + const dynamixel_base::comm_t & comm, const double acceleration_rpss); + bool write_profile_velocity( + const dynamixel_base::comm_t & comm, const double velocity_rps); + + unsigned int to_profile_acceleration(const double acceleration_rpss); + unsigned int to_profile_velocity(const double velocity_rps); + double to_position_radian(const int position); + double to_velocity_rps(const int velocity); + double to_current_ampere(const int current); + double to_voltage_volt(const int voltage); + unsigned int from_position_radian(const double position_rad); + unsigned int from_velocity_rps(const double velocity_rps); + unsigned int from_current_ampere(const double current_ampere); + + bool auto_set_indirect_address_of_present_position(const dynamixel_base::comm_t & comm); + bool auto_set_indirect_address_of_present_velocity(const dynamixel_base::comm_t & comm); + bool auto_set_indirect_address_of_present_current(const dynamixel_base::comm_t & comm); + bool auto_set_indirect_address_of_present_input_voltage(const dynamixel_base::comm_t & comm); + bool auto_set_indirect_address_of_present_temperature(const dynamixel_base::comm_t & comm); + 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); + + unsigned int indirect_addr_of_present_position(void); + unsigned int indirect_addr_of_present_velocity(void); + unsigned int indirect_addr_of_present_current(void); + unsigned int indirect_addr_of_present_input_voltage(void); + unsigned int indirect_addr_of_present_temperature(void); + 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 start_address_for_indirect_read(void); + unsigned int length_of_indirect_data_read(void); + unsigned int next_indirect_addr_read(void) const; + + unsigned int start_address_for_indirect_write(void); + unsigned int length_of_indirect_data_write(void); + unsigned int next_indirect_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); + bool extract_present_velocity_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + double & velocity_rps); + bool extract_present_current_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + double & current_ampere); + bool extract_present_input_voltage_from_sync_read( + const dynamixel_base::comm_t & comm, const std::string & group_name, + double & voltage_volt); + bool extract_present_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_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); + + protected: + int HOME_POSITION_; + unsigned int total_length_of_indirect_addr_read_; + unsigned int total_length_of_indirect_addr_write_; + uint16_t indirect_addr_of_present_position_; + uint16_t indirect_addr_of_present_velocity_; + uint16_t indirect_addr_of_present_current_; + uint16_t indirect_addr_of_present_input_voltage_; + uint16_t indirect_addr_of_present_temperature_; + uint16_t indirect_addr_of_goal_position_; + uint16_t indirect_addr_of_goal_velocity_; + uint16_t indirect_addr_of_goal_current_; + + bool set_indirect_address_read( + const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len, + uint16_t & indirect_addr); + bool set_indirect_address_write( + const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len, + uint16_t & indirect_addr); +}; + +} // namespace dynamixel_p + +#endif // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_P_HPP_ diff --git a/rt_manipulators_lib/include/dynamixel_ph42.hpp b/rt_manipulators_lib/include/dynamixel_ph42.hpp new file mode 100644 index 0000000..3832786 --- /dev/null +++ b/rt_manipulators_lib/include/dynamixel_ph42.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_PH42_HPP_ +#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH42_HPP_ + +#include "dynamixel_p.hpp" + +namespace dynamixel_ph42 { + +class DynamixelPH42 : public dynamixel_p::DynamixelP { + public: + explicit DynamixelPH42(const uint8_t id); +}; + +} // namespace dynamixel_ph42 + +#endif // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH42_HPP_ diff --git a/rt_manipulators_lib/include/dynamixel_xh430.hpp b/rt_manipulators_lib/include/dynamixel_xh430.hpp new file mode 100644 index 0000000..eac0d11 --- /dev/null +++ b/rt_manipulators_lib/include/dynamixel_xh430.hpp @@ -0,0 +1,31 @@ +// 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_XH430_HPP_ +#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH430_HPP_ + +#include "dynamixel_x.hpp" + +namespace dynamixel_xh430 { + +class DynamixelXH430 : public dynamixel_x::DynamixelX { + public: + explicit DynamixelXH430(const uint8_t id); + double to_current_ampere(const int current) override; + unsigned int from_current_ampere(const double current_ampere) override; +}; + +} // namespace dynamixel_xh430 + +#endif // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH430_HPP_ diff --git a/rt_manipulators_lib/include/dynamixel_xh540.hpp b/rt_manipulators_lib/include/dynamixel_xh540.hpp new file mode 100644 index 0000000..be04d71 --- /dev/null +++ b/rt_manipulators_lib/include/dynamixel_xh540.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_XH540_HPP_ +#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH540_HPP_ + +#include "dynamixel_x.hpp" + +namespace dynamixel_xh540 { + +class DynamixelXH540 : public dynamixel_x::DynamixelX { + public: + explicit DynamixelXH540(const uint8_t id); +}; + +} // namespace dynamixel_xh540 + +#endif // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH540_HPP_ diff --git a/rt_manipulators_lib/src/CMakeLists.txt b/rt_manipulators_lib/src/CMakeLists.txt index 1aeb7aa..3ea13ef 100644 --- a/rt_manipulators_lib/src/CMakeLists.txt +++ b/rt_manipulators_lib/src/CMakeLists.txt @@ -15,6 +15,10 @@ add_library(${library_name} dynamixel_x.cpp dynamixel_xm430.cpp dynamixel_xm540.cpp + dynamixel_xh430.cpp + dynamixel_xh540.cpp + dynamixel_p.cpp + dynamixel_ph42.cpp ) set_target_properties(${library_name} PROPERTIES VERSION 1.1.0 SOVERSION 1) diff --git a/rt_manipulators_lib/src/dynamixel_p.cpp b/rt_manipulators_lib/src/dynamixel_p.cpp new file mode 100644 index 0000000..62d5ac9 --- /dev/null +++ b/rt_manipulators_lib/src/dynamixel_p.cpp @@ -0,0 +1,449 @@ +// 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 + +#include "dynamixel_p.hpp" + + +namespace dynamixel_p { + +const uint16_t ADDR_OPERATING_MODE = 11; +const uint16_t ADDR_CURRENT_LIMIT = 38; +const uint16_t ADDR_MAX_POSITION_LIMIT = 48; +const uint16_t ADDR_MIN_POSITION_LIMIT = 52; +const uint16_t ADDR_TORQUE_ENABLE = 512; +const uint16_t ADDR_VELOCITY_I_GAIN = 524; +const uint16_t ADDR_VELOCITY_P_GAIN = 526; +const uint16_t ADDR_POSITION_D_GAIN = 528; +const uint16_t ADDR_POSITION_I_GAIN = 530; +const uint16_t ADDR_POSITION_P_GAIN = 532; +const uint16_t ADDR_GOAL_CURRENT = 550; +const uint16_t ADDR_GOAL_VELOCITY = 552; +const uint16_t ADDR_PROFILE_ACCELERATION = 556; +const uint16_t ADDR_PROFILE_VELOCITY = 560; +const uint16_t ADDR_GOAL_POSITION = 564; +const uint16_t ADDR_PRESENT_CURRENT = 574; +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_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; + +// XMシリーズと同じアドレスで通信するため、インダイレクトアドレスの使用範囲を絞る +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 LEN_PRESENT_CURRENT = 2; +const uint16_t LEN_PRESENT_VELOCITY = 4; +const uint16_t LEN_PRESENT_POSITION = 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_GOAL_POSITION = 4; +const uint16_t LEN_INDIRECT_ADDRESS = 2; + +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 = 4306173; +const double TO_VELOCITY_REV_PER_MIN = 0.01; +const double TO_VELOCITY_RAD_PER_MIN = TO_VELOCITY_REV_PER_MIN * 2.0 * M_PI; +const double TO_VELOCITY_RAD_PER_SEC = TO_VELOCITY_RAD_PER_MIN / 60.0; +const double DXL_VELOCITY_FROM_RAD_PER_SEC = 1.0 / TO_VELOCITY_RAD_PER_SEC; +const int DXL_MAX_VELOCITY = 2920; +const double TO_RADIANS = (180.0 / 303750.0) * M_PI / 180.0; +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; + +DynamixelP::DynamixelP(const uint8_t id, const int home_position) + : dynamixel_base::DynamixelBase(id), HOME_POSITION_(home_position), + total_length_of_indirect_addr_read_(0), total_length_of_indirect_addr_write_(0), + indirect_addr_of_present_position_(0), indirect_addr_of_present_velocity_(0), + indirect_addr_of_present_current_(0), indirect_addr_of_present_input_voltage_(0), + indirect_addr_of_present_temperature_(0), indirect_addr_of_goal_position_(0), + indirect_addr_of_goal_velocity_(0), indirect_addr_of_goal_current_(0) { + name_ = "P"; +} + +bool DynamixelP::read_operating_mode(const dynamixel_base::comm_t & comm, uint8_t & mode) { + return comm->read_byte_data(id_, ADDR_OPERATING_MODE, mode); +} + +bool DynamixelP::write_operating_mode(const dynamixel_base::comm_t & comm, const uint8_t mode) { + return comm->write_byte_data(id_, ADDR_OPERATING_MODE, mode); +} + +bool DynamixelP::read_current_limit( + const dynamixel_base::comm_t & comm, double & limit_ampere) { + uint16_t dxl_current_limit = 0; + bool retval = comm->read_word_data(id_, ADDR_CURRENT_LIMIT, dxl_current_limit); + limit_ampere = to_current_ampere(dxl_current_limit); + return retval; +} + +bool DynamixelP::read_max_position_limit( + const dynamixel_base::comm_t & comm, double & limit_radian) { + uint32_t dxl_position_limit = 0; + bool retval = comm->read_double_word_data(id_, ADDR_MAX_POSITION_LIMIT, dxl_position_limit); + limit_radian = to_position_radian(dxl_position_limit); + return retval; +} + +bool DynamixelP::read_min_position_limit( + const dynamixel_base::comm_t & comm, double & limit_radian) { + uint32_t dxl_position_limit = 0; + bool retval = comm->read_double_word_data(id_, ADDR_MIN_POSITION_LIMIT, dxl_position_limit); + limit_radian = to_position_radian(dxl_position_limit); + return retval; +} + +bool DynamixelP::write_torque_enable(const dynamixel_base::comm_t & comm, const bool enable) { + return comm->write_byte_data(id_, ADDR_TORQUE_ENABLE, enable); +} + +bool DynamixelP::write_velocity_i_gain( + const dynamixel_base::comm_t & comm, const unsigned int gain) { + return comm->write_word_data(id_, ADDR_VELOCITY_I_GAIN, static_cast(gain)); +} + +bool DynamixelP::write_velocity_p_gain( + const dynamixel_base::comm_t & comm, const unsigned int gain) { + return comm->write_word_data(id_, ADDR_VELOCITY_P_GAIN, static_cast(gain)); +} + +bool DynamixelP::write_position_d_gain( + const dynamixel_base::comm_t & comm, const unsigned int gain) { + return comm->write_word_data(id_, ADDR_POSITION_D_GAIN, static_cast(gain)); +} + +bool DynamixelP::write_position_i_gain( + const dynamixel_base::comm_t & comm, const unsigned int gain) { + return comm->write_word_data(id_, ADDR_POSITION_I_GAIN, static_cast(gain)); +} + +bool DynamixelP::write_position_p_gain( + const dynamixel_base::comm_t & comm, const unsigned int gain) { + return comm->write_word_data(id_, ADDR_POSITION_P_GAIN, static_cast(gain)); +} + +bool DynamixelP::write_profile_acceleration( + const dynamixel_base::comm_t & comm, const double acceleration_rpss) { + return comm->write_double_word_data( + id_, + ADDR_PROFILE_ACCELERATION, + static_cast(to_profile_acceleration(acceleration_rpss))); +} + +bool DynamixelP::write_profile_velocity( + const dynamixel_base::comm_t & comm, const double velocity_rps) { + return comm->write_double_word_data( + id_, + ADDR_PROFILE_VELOCITY, + static_cast(to_profile_velocity(velocity_rps))); +} + +unsigned int DynamixelP::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); +} + +unsigned int DynamixelP::to_profile_velocity(const double velocity_rps) { + int dxl_velocity = DXL_VELOCITY_FROM_RAD_PER_SEC * velocity_rps; + if (dxl_velocity > DXL_MAX_VELOCITY) { + dxl_velocity = DXL_MAX_VELOCITY; + } else if (dxl_velocity <= 0) { + // PHシリーズでは、'0'が最大速度を意味する + // よって、速度の最小値は'1'である + dxl_velocity = 1; + } + + return static_cast(dxl_velocity); +} + +double DynamixelP::to_position_radian(const int position) { + return (position - HOME_POSITION_) * TO_RADIANS; +} + +double DynamixelP::to_velocity_rps(const int velocity) { + return velocity * TO_VELOCITY_RAD_PER_SEC; +} + +double DynamixelP::to_current_ampere(const int current) { + return current * TO_CURRENT_AMPERE; +} + +double DynamixelP::to_voltage_volt(const int voltage) { + return voltage * TO_VOLTAGE_VOLT; +} + +unsigned int DynamixelP::from_position_radian(const double position_rad) { + return position_rad * TO_DXL_POS + HOME_POSITION_; +} + +unsigned int DynamixelP::from_velocity_rps(const double velocity_rps) { + return velocity_rps * DXL_VELOCITY_FROM_RAD_PER_SEC; +} + +unsigned int DynamixelP::from_current_ampere(const double current_ampere) { + return current_ampere * TO_DXL_CURRENT; +} + +bool DynamixelP::auto_set_indirect_address_of_present_position( + const dynamixel_base::comm_t & comm) { + return set_indirect_address_read( + comm, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION, indirect_addr_of_present_position_); +} + +bool DynamixelP::auto_set_indirect_address_of_present_velocity( + const dynamixel_base::comm_t & comm) { + return set_indirect_address_read( + comm, ADDR_PRESENT_VELOCITY, LEN_PRESENT_VELOCITY, indirect_addr_of_present_velocity_); +} + +bool DynamixelP::auto_set_indirect_address_of_present_current( + const dynamixel_base::comm_t & comm) { + return set_indirect_address_read( + comm, ADDR_PRESENT_CURRENT, LEN_PRESENT_CURRENT, indirect_addr_of_present_current_); +} + +bool DynamixelP::auto_set_indirect_address_of_present_input_voltage( + const dynamixel_base::comm_t & comm) { + return set_indirect_address_read( + comm, ADDR_PRESENT_VOLTAGE, LEN_PRESENT_VOLTAGE, indirect_addr_of_present_input_voltage_); +} + +bool DynamixelP::auto_set_indirect_address_of_present_temperature( + const dynamixel_base::comm_t & comm) { + return set_indirect_address_read( + comm, ADDR_PRESENT_TEMPERATURE, LEN_PRESENT_TEMPERATURE, indirect_addr_of_present_temperature_); +} + +bool DynamixelP::auto_set_indirect_address_of_goal_position( + const dynamixel_base::comm_t & comm) { + return set_indirect_address_write( + comm, ADDR_GOAL_POSITION, LEN_GOAL_POSITION, indirect_addr_of_goal_position_); +} + +bool DynamixelP::auto_set_indirect_address_of_goal_velocity( + const dynamixel_base::comm_t & comm) { + return set_indirect_address_write( + comm, ADDR_GOAL_VELOCITY, LEN_GOAL_VELOCITY, indirect_addr_of_goal_velocity_); +} + +bool DynamixelP::auto_set_indirect_address_of_goal_current( + const dynamixel_base::comm_t & comm) { + return set_indirect_address_write( + comm, ADDR_GOAL_CURRENT, LEN_GOAL_CURRENT, indirect_addr_of_goal_current_); +} + +unsigned int DynamixelP::indirect_addr_of_present_position(void) { + return indirect_addr_of_present_position_; +} + +unsigned int DynamixelP::indirect_addr_of_present_velocity(void) { + return indirect_addr_of_present_velocity_; +} + +unsigned int DynamixelP::indirect_addr_of_present_current(void) { + return indirect_addr_of_present_current_; +} + +unsigned int DynamixelP::indirect_addr_of_present_input_voltage(void) { + return indirect_addr_of_present_input_voltage_; +} + +unsigned int DynamixelP::indirect_addr_of_present_temperature(void) { + return indirect_addr_of_present_temperature_; +} + +unsigned int DynamixelP::indirect_addr_of_goal_position(void) { + return indirect_addr_of_goal_position_; +} + +unsigned int DynamixelP::indirect_addr_of_goal_velocity(void) { + return indirect_addr_of_goal_velocity_; +} + +unsigned int DynamixelP::indirect_addr_of_goal_current(void) { + return indirect_addr_of_goal_current_; +} + +unsigned int DynamixelP::start_address_for_indirect_read(void) { + return ADDR_START_INDIRECT_DATA_READ; +} + +unsigned int DynamixelP::length_of_indirect_data_read(void) { + return total_length_of_indirect_addr_read_; +} + +unsigned int DynamixelP::next_indirect_addr_read(void) const { + return ADDR_START_INDIRECT_ADDR_READ + + LEN_INDIRECT_ADDRESS * total_length_of_indirect_addr_read_; +} + +unsigned int DynamixelP::start_address_for_indirect_write(void) { + return ADDR_START_INDIRECT_DATA_WRITE; +} + +unsigned int DynamixelP::length_of_indirect_data_write(void) { + return total_length_of_indirect_addr_write_; +} + +unsigned int DynamixelP::next_indirect_addr_write(void) const { + return ADDR_START_INDIRECT_ADDR_WRITE + + LEN_INDIRECT_ADDRESS * total_length_of_indirect_addr_write_; +} + +bool DynamixelP::extract_present_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_, indirect_addr_of_present_position(), LEN_PRESENT_POSITION, data)) { + return false; + } + position_rad = to_position_radian(static_cast(data)); + return true; +} + +bool DynamixelP::extract_present_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_, indirect_addr_of_present_velocity(), LEN_PRESENT_VELOCITY, data)) { + return false; + } + velocity_rps = to_velocity_rps(static_cast(data)); + return true; +} + +bool DynamixelP::extract_present_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_, indirect_addr_of_present_current(), LEN_PRESENT_CURRENT, data)) { + return false; + } + current_ampere = to_current_ampere(static_cast(data)); + return true; +} + +bool DynamixelP::extract_present_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_, indirect_addr_of_present_input_voltage(), LEN_PRESENT_VOLTAGE, data)) { + return false; + } + voltage_volt = to_voltage_volt(static_cast(data)); + return true; +} + +bool DynamixelP::extract_present_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_, indirect_addr_of_present_temperature(), LEN_PRESENT_TEMPERATURE, data)) { + return false; + } + temperature_deg = 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); + write_data.push_back(DXL_LOBYTE(DXL_LOWORD(dxl_position))); + write_data.push_back(DXL_HIBYTE(DXL_LOWORD(dxl_position))); + write_data.push_back(DXL_LOBYTE(DXL_HIWORD(dxl_position))); + write_data.push_back(DXL_HIBYTE(DXL_HIWORD(dxl_position))); +} + +void DynamixelP::push_back_velocity_for_sync_write( + const double velocity_rps, std::vector & write_data) { + uint32_t dxl_velocity = from_velocity_rps(velocity_rps); + write_data.push_back(DXL_LOBYTE(DXL_LOWORD(dxl_velocity))); + write_data.push_back(DXL_HIBYTE(DXL_LOWORD(dxl_velocity))); + write_data.push_back(DXL_LOBYTE(DXL_HIWORD(dxl_velocity))); + write_data.push_back(DXL_HIBYTE(DXL_HIWORD(dxl_velocity))); +} + +void DynamixelP::push_back_current_for_sync_write( + const double current_ampere, std::vector & write_data) { + uint16_t dxl_current = from_current_ampere(current_ampere); + write_data.push_back(DXL_LOBYTE(dxl_current)); + write_data.push_back(DXL_HIBYTE(dxl_current)); +} + +bool DynamixelP::set_indirect_address_read( + const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len, + uint16_t & indirect_addr) { + bool retval = true; + 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; + } + } + // テストしやすくするため、write_word_dataに失敗しても変数を更新する + indirect_addr = ADDR_START_INDIRECT_DATA_READ + + total_length_of_indirect_addr_read_; + total_length_of_indirect_addr_read_ += len; + return retval; +} + +bool DynamixelP::set_indirect_address_write( + const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len, + uint16_t & indirect_addr) { + bool retval = true; + 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; + if (!comm->write_word_data( + id_, target_indirect_address, target_data_address)) { + retval = false; + } + } + // テストしやすくするため、write_word_dataに失敗しても変数を更新する + indirect_addr = ADDR_START_INDIRECT_DATA_WRITE + + total_length_of_indirect_addr_write_; + total_length_of_indirect_addr_write_ += len; + return retval; +} + +} // namespace dynamixel_p diff --git a/rt_manipulators_lib/src/dynamixel_ph42.cpp b/rt_manipulators_lib/src/dynamixel_ph42.cpp new file mode 100644 index 0000000..4fe511e --- /dev/null +++ b/rt_manipulators_lib/src/dynamixel_ph42.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_ph42.hpp" + + +namespace dynamixel_ph42 { + +DynamixelPH42::DynamixelPH42(const uint8_t id) + : dynamixel_p::DynamixelP(id) { + name_ = "PH42"; +} + +} // namespace dynamixel_ph42 diff --git a/rt_manipulators_lib/src/dynamixel_xh430.cpp b/rt_manipulators_lib/src/dynamixel_xh430.cpp new file mode 100644 index 0000000..e52a1f3 --- /dev/null +++ b/rt_manipulators_lib/src/dynamixel_xh430.cpp @@ -0,0 +1,37 @@ +// 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_xh430.hpp" + + +namespace dynamixel_xh430 { + +const double TO_CURRENT_AMPERE = 0.00134; +const double TO_DXL_CURRENT = 1.0 / TO_CURRENT_AMPERE; + +DynamixelXH430::DynamixelXH430(const uint8_t id) + : dynamixel_x::DynamixelX(id) { + name_ = "XH430"; +} + +double DynamixelXH430::to_current_ampere(const int current) { + return current * TO_CURRENT_AMPERE; +} + +unsigned int DynamixelXH430::from_current_ampere(const double current_ampere) { + return current_ampere * TO_DXL_CURRENT; +} + +} // namespace dynamixel_xh430 diff --git a/rt_manipulators_lib/src/dynamixel_xh540.cpp b/rt_manipulators_lib/src/dynamixel_xh540.cpp new file mode 100644 index 0000000..cfee288 --- /dev/null +++ b/rt_manipulators_lib/src/dynamixel_xh540.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_xh540.hpp" + + +namespace dynamixel_xh540 { + +DynamixelXH540::DynamixelXH540(const uint8_t id) + : dynamixel_x::DynamixelX(id) { + name_ = "XH540"; +} + +} // namespace dynamixel_xh540 diff --git a/rt_manipulators_lib/src/joint.cpp b/rt_manipulators_lib/src/joint.cpp index 6ba4da6..034d1bf 100644 --- a/rt_manipulators_lib/src/joint.cpp +++ b/rt_manipulators_lib/src/joint.cpp @@ -14,6 +14,9 @@ #include "dynamixel_xm430.hpp" #include "dynamixel_xm540.hpp" +#include "dynamixel_xh430.hpp" +#include "dynamixel_xh540.hpp" +#include "dynamixel_ph42.hpp" #include "joint.hpp" namespace joint { @@ -36,6 +39,12 @@ Joint::Joint(const uint8_t id, const uint8_t operating_mode, const std::string d dxl = std::make_shared(id); } else if (dynamixel_name == "XM540") { dxl = std::make_shared(id); + } else if (dynamixel_name == "XH430") { + dxl = std::make_shared(id); + } else if (dynamixel_name == "XH540") { + dxl = std::make_shared(id); + } else if (dynamixel_name == "PH42") { + dxl = std::make_shared(id); } else { dxl = std::make_shared(id); } diff --git a/rt_manipulators_lib/test/CMakeLists.txt b/rt_manipulators_lib/test/CMakeLists.txt index 4328770..25e5a80 100644 --- a/rt_manipulators_lib/test/CMakeLists.txt +++ b/rt_manipulators_lib/test/CMakeLists.txt @@ -20,6 +20,8 @@ set(list_tests test_kinematics test_config_file_parser test_dynamixel_x + test_dynamixel_xh + test_dynamixel_p ) foreach(test_executable IN LISTS list_tests) message("${test_executable}") diff --git a/rt_manipulators_lib/test/config/ok_has_dynamixel_name.yaml b/rt_manipulators_lib/test/config/ok_has_dynamixel_name.yaml index afedbc2..d2031ea 100644 --- a/rt_manipulators_lib/test/config/ok_has_dynamixel_name.yaml +++ b/rt_manipulators_lib/test/config/ok_has_dynamixel_name.yaml @@ -3,6 +3,12 @@ joint_groups: joints: - joint1 - joint2 + - joint3 + - joint4 + - joint5 joint1: { id: 1, dynamixel: "XM430", operating_mode: 3} joint2: { id: 2, dynamixel: "XM540", operating_mode: 3} +joint3: { id: 3, dynamixel: "XH430", operating_mode: 3} +joint4: { id: 4, dynamixel: "XH540", operating_mode: 3} +joint5: { id: 5, 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 6b9a7c3..04ac34e 100644 --- a/rt_manipulators_lib/test/test_config_file_parser.cpp +++ b/rt_manipulators_lib/test/test_config_file_parser.cpp @@ -70,6 +70,9 @@ TEST(ConfigFileParserTest, has_dynamixel_name) { EXPECT_EQ(parsed_joints.joint("joint1")->dxl->get_id(), 1); EXPECT_EQ(parsed_joints.joint("joint1")->dxl->get_name(), "XM430"); EXPECT_EQ(parsed_joints.joint("joint2")->dxl->get_name(), "XM540"); + EXPECT_EQ(parsed_joints.joint("joint3")->dxl->get_name(), "XH430"); + EXPECT_EQ(parsed_joints.joint("joint4")->dxl->get_name(), "XH540"); + EXPECT_EQ(parsed_joints.joint("joint5")->dxl->get_name(), "PH42"); } TEST(ConfigFileParserTest, has_same_groups) { diff --git a/rt_manipulators_lib/test/test_dynamixel_p.cpp b/rt_manipulators_lib/test/test_dynamixel_p.cpp new file mode 100644 index 0000000..d11e283 --- /dev/null +++ b/rt_manipulators_lib/test/test_dynamixel_p.cpp @@ -0,0 +1,305 @@ +// 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 +#include + +#include "gtest/gtest.h" +#include "rt_manipulators_cpp/dynamixel_base.hpp" +#include "rt_manipulators_cpp/dynamixel_p.hpp" +#include "rt_manipulators_cpp/hardware_communicator.hpp" + + +class PTestFixture : public ::testing::Test { + protected: + virtual void SetUp() { + dxl = std::make_shared(1); + comm = std::make_shared("dummy_port"); + } + + virtual void TearDown() { + dxl.reset(); + } + + std::shared_ptr dxl; + std::shared_ptr comm; +}; + +TEST_F(PTestFixture, get_id) { + ASSERT_EQ(dxl->get_id(), 1); +} + +TEST_F(PTestFixture, read_operating_mode) { + uint8_t mode; + // Dynamixelが接続されていないため、通信が関わるテストはFalseを返す + ASSERT_FALSE(dxl->read_operating_mode(comm, mode)); +} + +TEST_F(PTestFixture, write_operating_mode) { + ASSERT_FALSE(dxl->write_operating_mode(comm, false)); +} + +TEST_F(PTestFixture, read_current_limit) { + double limit; + ASSERT_FALSE(dxl->read_current_limit(comm, limit)); +} + +TEST_F(PTestFixture, read_max_position_limit) { + double limit; + ASSERT_FALSE(dxl->read_max_position_limit(comm, limit)); +} + +TEST_F(PTestFixture, read_min_position_limit) { + double limit; + ASSERT_FALSE(dxl->read_min_position_limit(comm, limit)); +} + +TEST_F(PTestFixture, write_torque_enable) { + ASSERT_FALSE(dxl->write_torque_enable(comm, false)); +} + +TEST_F(PTestFixture, write_velocity_i_gain) { + ASSERT_FALSE(dxl->write_velocity_i_gain(comm, 123)); +} + +TEST_F(PTestFixture, write_velocity_p_gain) { + ASSERT_FALSE(dxl->write_velocity_p_gain(comm, 123)); +} + +TEST_F(PTestFixture, write_position_d_gain) { + ASSERT_FALSE(dxl->write_position_d_gain(comm, 123)); +} + +TEST_F(PTestFixture, write_position_i_gain) { + ASSERT_FALSE(dxl->write_position_i_gain(comm, 123)); +} + +TEST_F(PTestFixture, write_position_p_gain) { + ASSERT_FALSE(dxl->write_position_p_gain(comm, 123)); +} + +TEST_F(PTestFixture, write_profile_acceleration) { + ASSERT_FALSE(dxl->write_profile_acceleration(comm, 0)); +} + +TEST_F(PTestFixture, write_profile_velocity) { + ASSERT_FALSE(dxl->write_profile_velocity(comm, 0)); +} + +TEST_F(PTestFixture, 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), 4306173); +} + +TEST_F(PTestFixture, to_profile_velocity) { + // rad/s to rev/min + // 0以下に対しては1を返すことを期待 + EXPECT_EQ(dxl->to_profile_velocity(-1), 1); + EXPECT_EQ(dxl->to_profile_velocity(0), 1); + EXPECT_EQ(dxl->to_profile_velocity(0.010472), 10); + EXPECT_EQ(dxl->to_profile_velocity(1000000), 2920); +} + +TEST_F(PTestFixture, to_position_radian) { + EXPECT_DOUBLE_EQ(dxl->to_position_radian(0), 0.0); + // 151875 = 0x0002 5143 + // -151875 = 0xFFFD AEBD + EXPECT_DOUBLE_EQ(dxl->to_position_radian(0x00025143), M_PI_2); + EXPECT_DOUBLE_EQ(dxl->to_position_radian(0xFFFDAEBD), -M_PI_2); +} + +TEST_F(PTestFixture, to_velocity_rps) { + EXPECT_DOUBLE_EQ(dxl->to_velocity_rps(0), 0.0); + // -1 = 0xFFFF FFFF + EXPECT_NEAR(dxl->to_velocity_rps(0x00000001), 0.001047, 0.0001); + EXPECT_NEAR(dxl->to_velocity_rps(0xFFFFFFFF), -0.001047, 0.0001); +} + +TEST_F(PTestFixture, to_current_ampere) { + // 1000 = 0x0000 03E8 + // -1000 = 0xFFFF FC18 + EXPECT_DOUBLE_EQ(dxl->to_current_ampere(0), 0.0); + EXPECT_DOUBLE_EQ(dxl->to_current_ampere(0x000003E8), 1.0); + EXPECT_DOUBLE_EQ(dxl->to_current_ampere(0xFFFFFC18), -1.0); +} + +TEST_F(PTestFixture, to_voltage_volt) { + EXPECT_DOUBLE_EQ(dxl->to_voltage_volt(0), 0.0); + EXPECT_DOUBLE_EQ(dxl->to_voltage_volt(0x0001), 0.1); + EXPECT_DOUBLE_EQ(dxl->to_voltage_volt(0x0002), 0.2); +} + +TEST_F(PTestFixture, from_position_radian) { + EXPECT_EQ(dxl->from_position_radian(0.0), 0); + EXPECT_EQ(dxl->from_position_radian(M_PI_2), 151875); + EXPECT_EQ(dxl->from_position_radian(-M_PI_2), -151875); +} + +TEST_F(PTestFixture, from_velocity_rps) { + EXPECT_EQ(dxl->from_velocity_rps(0.0), 0); + EXPECT_EQ(dxl->from_velocity_rps(0.01048), 10); + EXPECT_EQ(dxl->from_velocity_rps(-0.01048), 0xFFFFFFF6); // -10 +} + +TEST_F(PTestFixture, from_current_ampere) { + EXPECT_EQ(dxl->from_current_ampere(0.0), 0); + EXPECT_EQ(dxl->from_current_ampere(1.0), 1000); + EXPECT_EQ(dxl->from_current_ampere(-1.0), 0xFFFFFC18); // -1000 +} + +TEST_F(PTestFixture, set_indirect_addresses_read) { + 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_present_position(comm)); + // indirect_dataの開始位置は変わらないことを期待 + EXPECT_EQ(dxl->start_address_for_indirect_read(), 634); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 4); + EXPECT_EQ(dxl->next_indirect_addr_read(), 176); + EXPECT_EQ(dxl->indirect_addr_of_present_position(), 634); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_present_velocity(comm)); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 8); + EXPECT_EQ(dxl->next_indirect_addr_read(), 184); + EXPECT_EQ(dxl->indirect_addr_of_present_velocity(), 638); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_present_current(comm)); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 10); + EXPECT_EQ(dxl->next_indirect_addr_read(), 188); + EXPECT_EQ(dxl->indirect_addr_of_present_current(), 642); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_present_input_voltage(comm)); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 12); + EXPECT_EQ(dxl->next_indirect_addr_read(), 192); + EXPECT_EQ(dxl->indirect_addr_of_present_input_voltage(), 644); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_present_temperature(comm)); + EXPECT_EQ(dxl->length_of_indirect_data_read(), 13); + EXPECT_EQ(dxl->next_indirect_addr_read(), 194); + EXPECT_EQ(dxl->indirect_addr_of_present_temperature(), 646); +} + +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); + EXPECT_EQ(dxl->next_indirect_addr_write(), 198); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_goal_position(comm)); + // indirect_dataの開始位置は変わらないことを期待 + EXPECT_EQ(dxl->start_address_for_indirect_write(), 649); + EXPECT_EQ(dxl->length_of_indirect_data_write(), 4); + EXPECT_EQ(dxl->next_indirect_addr_write(), 206); + EXPECT_EQ(dxl->indirect_addr_of_goal_position(), 649); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_goal_velocity(comm)); + EXPECT_EQ(dxl->length_of_indirect_data_write(), 8); + EXPECT_EQ(dxl->next_indirect_addr_write(), 214); + EXPECT_EQ(dxl->indirect_addr_of_goal_velocity(), 653); + + EXPECT_FALSE(dxl->auto_set_indirect_address_of_goal_current(comm)); + EXPECT_EQ(dxl->length_of_indirect_data_write(), 10); + EXPECT_EQ(dxl->next_indirect_addr_write(), 218); + EXPECT_EQ(dxl->indirect_addr_of_goal_current(), 657); +} + +TEST_F(PTestFixture, extract_present_position_from_sync_read) { + std::string group_name = "test"; + double position; + ASSERT_FALSE(dxl->extract_present_position_from_sync_read(comm, group_name, position)); +} + +TEST_F(PTestFixture, extract_present_velocity_from_sync_read) { + std::string group_name = "test"; + double velocity; + ASSERT_FALSE(dxl->extract_present_velocity_from_sync_read(comm, group_name, velocity)); +} + +TEST_F(PTestFixture, extract_present_current_from_sync_read) { + std::string group_name = "test"; + double current; + ASSERT_FALSE(dxl->extract_present_current_from_sync_read(comm, group_name, current)); +} + +TEST_F(PTestFixture, extract_present_input_voltage_from_sync_read) { + std::string group_name = "test"; + double voltage; + ASSERT_FALSE(dxl->extract_present_input_voltage_from_sync_read(comm, group_name, voltage)); +} + +TEST_F(PTestFixture, extract_present_temperature_from_sync_read) { + std::string group_name = "test"; + int temp; + ASSERT_FALSE(dxl->extract_present_temperature_from_sync_read(comm, group_name, temp)); +} + +TEST_F(PTestFixture, push_back_position_for_sync_write) { + std::vector test_data; + dxl->push_back_position_for_sync_write(M_PI_2, test_data); + ASSERT_EQ(test_data.size(), 4); + // from_position_radian(M_PI_2) = 151875 (0x0002 5143)がセットされる + EXPECT_EQ(test_data.at(0), 0x43); + EXPECT_EQ(test_data.at(1), 0x51); + EXPECT_EQ(test_data.at(2), 0x02); + EXPECT_EQ(test_data.at(3), 0x00); + + test_data.clear(); + dxl->push_back_position_for_sync_write(-M_PI_2, test_data); + ASSERT_EQ(test_data.size(), 4); + // from_position_radian(-M_PI_2) = -151875 (0xFFFD AEBD)がセットされる + EXPECT_EQ(test_data.at(0), 0xBD); + EXPECT_EQ(test_data.at(1), 0xAE); + EXPECT_EQ(test_data.at(2), 0xFD); + EXPECT_EQ(test_data.at(3), 0xFF); +} + +TEST_F(PTestFixture, push_back_velocity_for_sync_write) { + std::vector test_data; + dxl->push_back_velocity_for_sync_write(0.01048, test_data); + ASSERT_EQ(test_data.size(), 4); + // from_velocity_rps(0.01048) = 10 (0x0000 000A)がセットされる + EXPECT_EQ(test_data.at(0), 0x0A); + EXPECT_EQ(test_data.at(1), 0x00); + EXPECT_EQ(test_data.at(2), 0x00); + EXPECT_EQ(test_data.at(3), 0x00); + + test_data.clear(); + dxl->push_back_velocity_for_sync_write(-0.01048, test_data); + ASSERT_EQ(test_data.size(), 4); + // from_velocity_rps(-0.01047) = -10 (0xFFFF FFF6)がセットされる + EXPECT_EQ(test_data.at(0), 0xF6); + EXPECT_EQ(test_data.at(1), 0xFF); + EXPECT_EQ(test_data.at(2), 0xFF); + EXPECT_EQ(test_data.at(3), 0xFF); +} + +TEST_F(PTestFixture, push_back_current_for_sync_write) { + std::vector test_data; + dxl->push_back_current_for_sync_write(1.0, test_data); + ASSERT_EQ(test_data.size(), 2); + // from_current_ampere(1.0) = 1000 (0x03E8)がセットされる + EXPECT_EQ(test_data.at(0), 0xE8); + EXPECT_EQ(test_data.at(1), 0x03); + + test_data.clear(); + dxl->push_back_current_for_sync_write(-1.0, test_data); + ASSERT_EQ(test_data.size(), 2); + // from_current_ampere(-1.0) = -1000 (0xFC18)がセットされる + EXPECT_EQ(test_data.at(0), 0x18); + EXPECT_EQ(test_data.at(1), 0xFC); +} diff --git a/rt_manipulators_lib/test/test_dynamixel_xh.cpp b/rt_manipulators_lib/test/test_dynamixel_xh.cpp new file mode 100644 index 0000000..82b8a43 --- /dev/null +++ b/rt_manipulators_lib/test/test_dynamixel_xh.cpp @@ -0,0 +1,45 @@ +// 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 +#include + +#include "gtest/gtest.h" +#include "rt_manipulators_cpp/dynamixel_base.hpp" +#include "rt_manipulators_cpp/dynamixel_xh430.hpp" +#include "rt_manipulators_cpp/dynamixel_xh540.hpp" + +TEST(DynamixelXHTest, create_xh_series_instance) { + std::shared_ptr dxl; + dxl = std::make_shared(1); + EXPECT_EQ(dxl->get_name(), "XH430"); + dxl = std::make_shared(1); + EXPECT_EQ(dxl->get_name(), "XH540"); +} + +TEST(DynamixelXHTest, to_current_ampere_430) { + std::shared_ptr dxl; + dxl = std::make_shared(1); + EXPECT_DOUBLE_EQ(dxl->to_current_ampere(0), 0.0); + EXPECT_DOUBLE_EQ(dxl->to_current_ampere(1000), 1.34); + EXPECT_DOUBLE_EQ(dxl->to_current_ampere(-1000), -1.34); +} + +TEST(DynamixelXHTest, from_current_ampere_430) { + std::shared_ptr dxl; + dxl = std::make_shared(1); + EXPECT_EQ(dxl->from_current_ampere(0.0), 0); + EXPECT_EQ(dxl->from_current_ampere(1.341), 1000); + EXPECT_EQ(dxl->from_current_ampere(-1.341), 0xFFFFFC18); // -1000 +}