From 0e058856af1e57fec6c8abb087aeff28a189a9cb Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Tue, 13 Feb 2024 18:00:45 +0900 Subject: [PATCH] fix conflicts --- .../include/dynamixel_ph54.hpp | 32 ++++++++++ rt_manipulators_lib/src/CMakeLists.txt | 1 + rt_manipulators_lib/src/dynamixel_ph54.cpp | 56 +++++++++++++++++ rt_manipulators_lib/src/joint.cpp | 3 + rt_manipulators_lib/test/CMakeLists.txt | 1 + .../test/test_dynamixel_ph54.cpp | 61 +++++++++++++++++++ 6 files changed, 154 insertions(+) create mode 100644 rt_manipulators_lib/include/dynamixel_ph54.hpp create mode 100644 rt_manipulators_lib/src/dynamixel_ph54.cpp create mode 100644 rt_manipulators_lib/test/test_dynamixel_ph54.cpp 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/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/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/joint.cpp b/rt_manipulators_lib/src/joint.cpp index 034d1bf..6befb4a 100644 --- a/rt_manipulators_lib/src/joint.cpp +++ b/rt_manipulators_lib/src/joint.cpp @@ -17,6 +17,7 @@ #include "dynamixel_xh430.hpp" #include "dynamixel_xh540.hpp" #include "dynamixel_ph42.hpp" +#include "dynamixel_ph54.hpp" #include "joint.hpp" namespace joint { @@ -45,6 +46,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); } diff --git a/rt_manipulators_lib/test/CMakeLists.txt b/rt_manipulators_lib/test/CMakeLists.txt index bade229..f1c5785 100644 --- a/rt_manipulators_lib/test/CMakeLists.txt +++ b/rt_manipulators_lib/test/CMakeLists.txt @@ -23,6 +23,7 @@ set(list_tests test_dynamixel_xh test_dynamixel_p test_hardware + test_dynamixel_ph54 ) # Download FakeIt 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); +}