fix conflicts

rt_manipulators_lib/include/dynamixel_ph54.hpp

@@ -0,0 +1,32 @@
+// Copyright 2024 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH54_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH54_HPP_
+
+#include "dynamixel_p.hpp"
+
+namespace dynamixel_ph54 {
+
+class DynamixelPH54 : public dynamixel_p::DynamixelP {
+ public:
+  explicit DynamixelPH54(const uint8_t id);
+  unsigned int to_profile_acceleration(const double acceleration_rpss) override;
+  double to_position_radian(const int position) override;
+  unsigned int from_position_radian(const double position_rad) override;
+};
+
+}  // namespace dynamixel_ph54
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH54_HPP_

rt_manipulators_lib/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)
 

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 <cmath>
+#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<unsigned int>(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

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<dynamixel_xh540::DynamixelXH540>(id);
   } else if (dynamixel_name == "PH42") {
     dxl = std::make_shared<dynamixel_ph42::DynamixelPH42>(id);
+  } else if (dynamixel_name == "PH54") {
+    dxl = std::make_shared<dynamixel_ph54::DynamixelPH54>(id);
   } else {
     dxl = std::make_shared<dynamixel_base::DynamixelBase>(id);
   }

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

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 <cmath>
+#include <memory>
+
+#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<dynamixel_ph54::DynamixelPH54>(1);
+  }
+
+  virtual void TearDown() {
+    dxl.reset();
+  }
+
+  std::shared_ptr<dynamixel_base::DynamixelBase> 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);
+}