diff --git a/dynamixel_hardware/CMakeLists.txt b/dynamixel_hardware/CMakeLists.txt
index d97ca69..c9248ec 100644
--- a/dynamixel_hardware/CMakeLists.txt
+++ b/dynamixel_hardware/CMakeLists.txt
@@ -23,6 +23,7 @@ find_package(lifecycle_msgs REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(dynamixel_workbench_toolbox REQUIRED)
+find_package(tinyxml2_vendor REQUIRED)
add_library(
${PROJECT_NAME}
diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
index d3d5746..744341e 100644
--- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
+++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
@@ -45,6 +45,7 @@ struct Joint
JointValue state{};
JointValue command{};
JointValue prev_command{};
+ double rising_offset{0.0};
};
enum class ControlMode
diff --git a/dynamixel_hardware/package.xml b/dynamixel_hardware/package.xml
index cab2424..3e2b712 100644
--- a/dynamixel_hardware/package.xml
+++ b/dynamixel_hardware/package.xml
@@ -14,6 +14,7 @@
lifecycle_msgs
hardware_interface
pluginlib
+ tinyxml2_vendor
dynamixel_workbench_toolbox
ament_lint_auto
diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp
index 720db9e..9b20afb 100644
--- a/dynamixel_hardware/src/dynamixel_hardware.cpp
+++ b/dynamixel_hardware/src/dynamixel_hardware.cpp
@@ -14,6 +14,8 @@
#include "dynamixel_hardware/dynamixel_hardware.hpp"
+#include
+
#include
#include
#include
@@ -72,6 +74,37 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]);
}
+ // parse URDF for calibration parameters
+ auto urdf = info_.original_xml;
+ if (!urdf.empty()) {
+ tinyxml2::XMLDocument doc;
+ if (doc.Parse(urdf.c_str()) != tinyxml2::XML_SUCCESS) {
+ RCLCPP_ERROR_STREAM(get_logger(), "Failed to parse URDF XML");
+ return hardware_interface::CallbackReturn::ERROR;
+ }
+ const auto * joint_element = doc.RootElement()->FirstChildElement("joint");
+ while (joint_element != nullptr) {
+ const auto * name_attr = joint_element->FindAttribute("name");
+ const auto * calibration_element = joint_element->FirstChildElement("calibration");
+ if (calibration_element != nullptr) {
+ const auto * rising_attr = calibration_element->FindAttribute("rising");
+ if ((rising_attr != nullptr) && (name_attr != nullptr)) {
+ auto rising = std::atof(calibration_element->Attribute("rising"));
+ std::string name = joint_element->Attribute("name");
+ auto itr = std::find_if(
+ info_.joints.begin(), info_.joints.end(),
+ [&name](const hardware_interface::ComponentInfo & joint) {
+ return joint.name == name;
+ });
+ if (itr != info_.joints.end()) {
+ joints_[std::distance(info_.joints.begin(), itr)].rising_offset = rising;
+ }
+ }
+ }
+ joint_element = joint_element->NextSiblingElement("joint");
+ }
+ }
+
if (
info_.hardware_parameters.find("use_dummy") != info_.hardware_parameters.end() &&
info_.hardware_parameters.at("use_dummy") == "true")
@@ -286,7 +319,8 @@ return_type DynamixelHardware::read(
}
for (uint i = 0; i < ids.size(); i++) {
- joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]);
+ joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]) +
+ joints_[i].rising_offset;
joints_[i].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]);
joints_[i].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]);
}
@@ -473,7 +507,7 @@ CallbackReturn DynamixelHardware::set_joint_positions()
for (uint i = 0; i < ids.size(); i++) {
joints_[i].prev_command.position = joints_[i].command.position;
commands[i] = dynamixel_workbench_.convertRadian2Value(
- ids[i], static_cast(joints_[i].command.position));
+ ids[i], static_cast(joints_[i].command.position - joints_[i].rising_offset));
}
if (!dynamixel_workbench_.syncWrite(
kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log))