diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index 0eaf026..bb9710f 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -44,18 +44,18 @@ struct Joint { JointValue state{}; JointValue command{}; - JointValue prev_command{}; }; enum class ControlMode { Position, Velocity, Torque, - Currrent, + Current, ExtendedPosition, MultiTurn, CurrentBasedPosition, PWM, + NoControl, }; class DynamixelHardware @@ -73,6 +73,11 @@ class DynamixelHardware DYNAMIXEL_HARDWARE_PUBLIC std::vector export_command_interfaces() override; + DYNAMIXEL_HARDWARE_PUBLIC + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + DYNAMIXEL_HARDWARE_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; @@ -88,21 +93,24 @@ class DynamixelHardware private: return_type enable_torque(const bool enabled); - return_type set_control_mode(const ControlMode & mode, const bool force_set = false); + return_type set_control_mode(const ControlMode & mode); return_type reset_command(); CallbackReturn set_joint_positions(); CallbackReturn set_joint_velocities(); + CallbackReturn set_joint_currents(); CallbackReturn set_joint_params(); DynamixelWorkbench dynamixel_workbench_; std::map control_items_; std::vector joints_; std::vector joint_ids_; + std::vector joint_ids_ttl_; + std::vector joint_ids_rs_; bool torque_enabled_{false}; - ControlMode control_mode_{ControlMode::Position}; - bool mode_changed_{false}; + ControlMode control_mode_{ControlMode::NoControl}; + ControlMode prev_control_mode_{ControlMode::NoControl}; bool use_dummy_{false}; }; } // namespace dynamixel_hardware diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 474f7b1..c15dc78 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -29,9 +29,11 @@ namespace dynamixel_hardware constexpr const char * kDynamixelHardware = "DynamixelHardware"; constexpr uint8_t kGoalPositionIndex = 0; constexpr uint8_t kGoalVelocityIndex = 1; +constexpr uint8_t kGoalCurrentIndex = 2; constexpr uint8_t kPresentPositionVelocityCurrentIndex = 0; constexpr const char * kGoalPositionItem = "Goal_Position"; constexpr const char * kGoalVelocityItem = "Goal_Velocity"; +constexpr const char * kGoalCurrentItem = "Goal_Current"; constexpr const char * kMovingSpeedItem = "Moving_Speed"; constexpr const char * kPresentPositionItem = "Present_Position"; constexpr const char * kPresentVelocityItem = "Present_Velocity"; @@ -67,15 +69,18 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo joints_[i].command.position = std::numeric_limits::quiet_NaN(); joints_[i].command.velocity = std::numeric_limits::quiet_NaN(); joints_[i].command.effort = std::numeric_limits::quiet_NaN(); - joints_[i].prev_command.position = joints_[i].command.position; - joints_[i].prev_command.velocity = joints_[i].command.velocity; - joints_[i].prev_command.effort = joints_[i].command.effort; RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]); + + if(info_.joints[i].parameters.at("protocol") == "TTL"){ + joint_ids_ttl_.push_back(joint_ids_[i]); + }else if(info_.joints[i].parameters.at("protocol") == "RS"){ + joint_ids_rs_.push_back(joint_ids_[i]); + } } if ( info_.hardware_parameters.find("use_dummy") != info_.hardware_parameters.end() && - info_.hardware_parameters.at("use_dummy") == "true") { + (info_.hardware_parameters.at("use_dummy") == "true" || info_.hardware_parameters.at("use_dummy") == "True")) { use_dummy_ = true; RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "dummy mode"); return CallbackReturn::SUCCESS; @@ -102,7 +107,7 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo } enable_torque(false); - set_control_mode(ControlMode::Position, true); + set_control_mode(control_mode_); set_joint_params(); enable_torque(true); @@ -121,6 +126,12 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo return CallbackReturn::ERROR; } + const ControlItem * goal_current = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalCurrentItem); + if (goal_current == nullptr) { + return CallbackReturn::ERROR; + } + const ControlItem * present_position = dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentPositionItem); if (present_position == nullptr) { @@ -147,6 +158,7 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo control_items_[kGoalPositionItem] = goal_position; control_items_[kGoalVelocityItem] = goal_velocity; + control_items_[kGoalCurrentItem] = goal_current; control_items_[kPresentPositionItem] = present_position; control_items_[kPresentVelocityItem] = present_velocity; control_items_[kPresentCurrentItem] = present_current; @@ -165,6 +177,13 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo return CallbackReturn::ERROR; } + if (!dynamixel_workbench_.addSyncWriteHandler( + control_items_[kGoalCurrentItem]->address, control_items_[kGoalCurrentItem]->data_length, + &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + uint16_t start_address = std::min( control_items_[kPresentPositionItem]->address, control_items_[kPresentCurrentItem]->address); uint16_t read_length = control_items_[kPresentPositionItem]->data_length + @@ -183,12 +202,20 @@ std::vector DynamixelHardware::export_state_ RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "export_state_interfaces"); std::vector state_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].state.position)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].state.velocity)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].state.effort)); + for(auto joint_interface:info_.joints[i].state_interfaces){ + if(joint_interface.name == hardware_interface::HW_IF_POSITION){ + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].state.position)); + } + if(joint_interface.name == hardware_interface::HW_IF_VELOCITY){ + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].state.velocity)); + } + if(joint_interface.name == hardware_interface::HW_IF_EFFORT){ + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].state.effort)); + } + } } return state_interfaces; @@ -199,15 +226,105 @@ std::vector DynamixelHardware::export_comm RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "export_command_interfaces"); std::vector command_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].command.position)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].command.velocity)); + for(auto joint_interface:info_.joints[i].command_interfaces){ + if(joint_interface.name == hardware_interface::HW_IF_POSITION){ + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].command.position)); + } + if(joint_interface.name == hardware_interface::HW_IF_VELOCITY){ + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].command.velocity)); + } + if(joint_interface.name == hardware_interface::HW_IF_EFFORT){ + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].command.effort)); + } + } } return command_interfaces; } + +return_type DynamixelHardware::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + std::vector new_modes; + if(control_mode_ == ControlMode::CurrentBasedPosition){ + new_modes.push_back(ControlMode::Position); + new_modes.push_back(ControlMode::Current); + } else if(control_mode_ != ControlMode::NoControl){ + new_modes.push_back(control_mode_); + } else { //cotrol_mode_ == ControlMode::NoControl + } + + std::vector> new_joint_modes(info_.joints.size()); + for (std::size_t i = 0; i < info_.joints.size(); i++){ + for (std::string key : start_interfaces){ + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION){ + new_joint_modes[i].push_back(ControlMode::Position); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY){ + new_joint_modes[i].push_back(ControlMode::Velocity); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT){ + new_joint_modes[i].push_back(ControlMode::Current); + } + } + } + if (std::all_of( + new_joint_modes.begin() + 1, new_joint_modes.end(), + [&](const std::vector& mode) { return mode == new_joint_modes[0]; })){ + for(auto mode:new_joint_modes[0]){ + new_modes.push_back(mode); + } + } else { + return return_type::ERROR; + } + + std::vector> stop_joint_modes(info_.joints.size()); + for (std::string key : stop_interfaces){ + for (std::size_t i = 0; i < info_.joints.size(); i++){ + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION){ + stop_joint_modes[i].push_back(ControlMode::Position); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY){ + stop_joint_modes[i].push_back(ControlMode::Velocity); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT){ + stop_joint_modes[i].push_back(ControlMode::Current); + } + } + } + if (std::all_of( + stop_joint_modes.begin() + 1, stop_joint_modes.end(), + [&](const std::vector& mode) { return mode == stop_joint_modes[0];})){ + for(auto mode:stop_joint_modes[0]){ + auto new_end = std::remove(new_modes.begin(), new_modes.end(), mode); + new_modes.erase(new_end, new_modes.end()); + } + } else { + return return_type::ERROR; + } + + if(new_modes.size() == 1){ + control_mode_ = new_modes[0]; + }else if( + std::find(new_modes.begin(), new_modes.end(), ControlMode::Position) != new_modes.end() && + std::find(new_modes.begin(), new_modes.end(), ControlMode::Current) != new_modes.end()){ + control_mode_ = ControlMode::CurrentBasedPosition; + }else if(new_modes.size() == 0){ + RCLCPP_WARN(rclcpp::get_logger(kDynamixelHardware), "No Cotrol Interface, Torque disabled"); + control_mode_ = ControlMode::NoControl; + } else { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Not implemented hardware interfaces"); + return return_type::ERROR; + } + + return return_type::OK; +} + CallbackReturn DynamixelHardware::on_activate(const rclcpp_lifecycle::State & /* previous_state */) { RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "start"); @@ -236,47 +353,96 @@ return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcp if (use_dummy_) { return return_type::OK; } + + if(!joint_ids_ttl_.empty() && !joint_ids_rs_.empty()){ + std::vector* ids_each[] = {&joint_ids_ttl_, &joint_ids_rs_}; + for(auto& idt: ids_each){ + std::vector ids(idt->size(), 0); + std::vector positions(idt->size(), 0); + std::vector velocities(idt->size(), 0); + std::vector currents(idt->size(), 0); + std::copy(idt->begin(), idt->end(), ids.begin()); + const char * log = nullptr; + + if (!dynamixel_workbench_.syncRead( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - std::vector ids(info_.joints.size(), 0); - std::vector positions(info_.joints.size(), 0); - std::vector velocities(info_.joints.size(), 0); - std::vector currents(info_.joints.size(), 0); + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentCurrentItem]->address, + control_items_[kPresentCurrentItem]->data_length, currents.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); - const char * log = nullptr; + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentVelocityItem]->address, + control_items_[kPresentVelocityItem]->data_length, velocities.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - if (!dynamixel_workbench_.syncRead( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); - } + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentPositionItem]->address, + control_items_[kPresentPositionItem]->data_length, positions.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - if (!dynamixel_workbench_.getSyncReadData( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), - control_items_[kPresentCurrentItem]->address, - control_items_[kPresentCurrentItem]->data_length, currents.data(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); - } + for(uint i = 0; i < ids.size(); i++){ + auto it = std::find(joint_ids_.begin(), joint_ids_.end(), ids[i]); + if(it != joint_ids_.end()){ + int index = std::distance(joint_ids_.begin(), it); + joints_[index].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]); + joints_[index].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]); + joints_[index].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]); + } + } + } + }else{//When TTL and RS485 Protocols are not used at the same time + std::vector ids(info_.joints.size(), 0); + std::vector positions(info_.joints.size(), 0); + std::vector velocities(info_.joints.size(), 0); + std::vector currents(info_.joints.size(), 0); + + std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); + const char * log = nullptr; + + if (!dynamixel_workbench_.syncRead( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - if (!dynamixel_workbench_.getSyncReadData( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), - control_items_[kPresentVelocityItem]->address, - control_items_[kPresentVelocityItem]->data_length, velocities.data(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); - } + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentCurrentItem]->address, + control_items_[kPresentCurrentItem]->data_length, currents.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - if (!dynamixel_workbench_.getSyncReadData( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), - control_items_[kPresentPositionItem]->address, - control_items_[kPresentPositionItem]->data_length, positions.data(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); - } + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentVelocityItem]->address, + control_items_[kPresentVelocityItem]->data_length, velocities.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - for (uint i = 0; i < ids.size(); i++) { - joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]); - joints_[i].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]); - joints_[i].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]); + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentPositionItem]->address, + control_items_[kPresentPositionItem]->data_length, positions.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + for (uint i = 0; i < ids.size(); i++) { + joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]); + joints_[i].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]); + joints_[i].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]); + } } + return return_type::OK; } @@ -284,45 +450,21 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc { if (use_dummy_) { for (auto & joint : joints_) { - joint.prev_command.position = joint.command.position; joint.state.position = joint.command.position; } return return_type::OK; } - - // Velocity control - if (std::any_of( - joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.velocity != j.prev_command.velocity; })) { - set_control_mode(ControlMode::Velocity); - if(mode_changed_) - { - set_joint_params(); - } - set_joint_velocities(); - return return_type::OK; - } - // Position control - if (std::any_of( - joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.position != j.prev_command.position; })) { - set_control_mode(ControlMode::Position); - if(mode_changed_) - { - set_joint_params(); - } - set_joint_positions(); - return return_type::OK; - } - - // Effort control - if (std::any_of( - joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.effort != 0.0; })) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Effort control is not implemented"); - return return_type::ERROR; + if(control_mode_ != prev_control_mode_){ + set_control_mode(control_mode_); + set_joint_params(); + prev_control_mode_ = control_mode_; } - // if all command values are unchanged, then remain in existing control mode and set corresponding command values switch (control_mode_) { + case ControlMode::NoControl: + return return_type::OK; + break; case ControlMode::Velocity: set_joint_velocities(); return return_type::OK; @@ -331,7 +473,16 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc set_joint_positions(); return return_type::OK; break; - default: // effort, etc + case ControlMode::Current: + set_joint_currents(); + return return_type::OK; + break; + case ControlMode::CurrentBasedPosition: + set_joint_currents(); + set_joint_positions(); + return return_type::OK; + break; + default: // torque, etc RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Control mode not implemented"); return return_type::ERROR; break; @@ -366,39 +517,45 @@ return_type DynamixelHardware::enable_torque(const bool enabled) return return_type::OK; } -return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const bool force_set) +return_type DynamixelHardware::set_control_mode(const ControlMode & mode) { const char * log = nullptr; - mode_changed_ = false; - if (mode == ControlMode::Velocity && (force_set || control_mode_ != ControlMode::Velocity)) { - bool torque_enabled = torque_enabled_; - if (torque_enabled) { + if (mode == ControlMode::NoControl) { + if (torque_enabled_) { enable_torque(false); } for (uint i = 0; i < joint_ids_.size(); ++i) { - if (!dynamixel_workbench_.setVelocityControlMode(joint_ids_[i], &log)) { + if (!dynamixel_workbench_.setPositionControlMode(joint_ids_[i], &log)) { RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); return return_type::ERROR; } } - RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control"); - if(control_mode_ != ControlMode::Velocity) - { - mode_changed_ = true; - control_mode_ = ControlMode::Velocity; + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control,but no torque mode"); + + return return_type::OK; + } + + if (mode == ControlMode::Velocity) { + if (torque_enabled_) { + enable_torque(false); } - if (torque_enabled) { - enable_torque(true); + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setVelocityControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control"); + + enable_torque(true); return return_type::OK; } - if (mode == ControlMode::Position && (force_set || control_mode_ != ControlMode::Position)) { - bool torque_enabled = torque_enabled_; - if (torque_enabled) { + if (mode == ControlMode::Position) { + if (torque_enabled_) { enable_torque(false); } @@ -409,21 +566,48 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control"); - if(control_mode_ != ControlMode::Position) - { - mode_changed_ = true; - control_mode_ = ControlMode::Position; + + enable_torque(true); + return return_type::OK; + } + + if (mode == ControlMode::Current) { + if (torque_enabled_) { + enable_torque(false); } - if (torque_enabled) { - enable_torque(true); + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setTorqueControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Current control"); + + enable_torque(true); return return_type::OK; } - - if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position) { + + if (mode == ControlMode::CurrentBasedPosition) { + if (torque_enabled_) { + enable_torque(false); + } + + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setCurrentBasedPositionControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "CurrentBasedPosition control"); + + enable_torque(true); + return return_type::OK; + } + + if (mode != ControlMode::Velocity && mode != ControlMode::Position && mode != ControlMode::Current && mode != ControlMode::CurrentBasedPosition) { RCLCPP_FATAL( - rclcpp::get_logger(kDynamixelHardware), "Only position/velocity control are implemented"); + rclcpp::get_logger(kDynamixelHardware), "Only position/velocity/current/current based position control are implemented"); return return_type::ERROR; } @@ -436,9 +620,6 @@ return_type DynamixelHardware::reset_command() joints_[i].command.position = joints_[i].state.position; joints_[i].command.velocity = 0.0; joints_[i].command.effort = 0.0; - joints_[i].prev_command.position = joints_[i].command.position; - joints_[i].prev_command.velocity = joints_[i].command.velocity; - joints_[i].prev_command.effort = joints_[i].command.effort; } return return_type::OK; @@ -452,7 +633,6 @@ CallbackReturn DynamixelHardware::set_joint_positions() std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); 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)); } @@ -471,7 +651,6 @@ CallbackReturn DynamixelHardware::set_joint_velocities() std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); for (uint i = 0; i < ids.size(); i++) { - joints_[i].prev_command.velocity = joints_[i].command.velocity; commands[i] = dynamixel_workbench_.convertVelocity2Value( ids[i], static_cast(joints_[i].command.velocity)); } @@ -482,6 +661,24 @@ CallbackReturn DynamixelHardware::set_joint_velocities() return CallbackReturn::SUCCESS; } +CallbackReturn DynamixelHardware::set_joint_currents() +{ + const char * log = nullptr; + std::vector commands(info_.joints.size(), 0); + std::vector ids(info_.joints.size(), 0); + + std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); + for (uint i = 0; i < ids.size(); i++) { + commands[i] = dynamixel_workbench_.convertCurrent2Value( + ids[i], static_cast(joints_[i].command.effort)); + } + if (!dynamixel_workbench_.syncWrite( + kGoalCurrentIndex, ids.data(), ids.size(), commands.data(), 1, &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + return CallbackReturn::SUCCESS; +} + CallbackReturn DynamixelHardware::set_joint_params() { const char * log = nullptr;