README.md

@@ -7,27 +7,29 @@ are welcome too.
 Make sure the [DynamixelSDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) for ROS2 is installed.
 Clone it in your workspace and checkout the branch ros2:
 ```
-git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git
+git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git -b ros2
 cd DynamixelSDK/
-git checkout ros2
 ```
 finally build with colcon
 `colcon build`
 
 ## Configure Dynamixel motor parameters via URDF
 
-Add `serial_port`, `baud_rate`, and `joint_ids` parameters to the ros2_control part of your `.urdf` / `.xarco` files.
+Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part of your `.urdf` / `.xarco` files. 
 
 ```xml
 <ros2_control name="my_robot" type="system">
       <hardware>
         <plugin>dynamixel_ros2_control/DynamixelHardwareInterface</plugin>
-        <param name="usb_port">/dev/ttyUSB0</param>
         <param name="serial_port">/dev/ttyUSB0</param>
         <param name="baud_rate">4000000</param>
       </hardware>
       <joint name="joint1">
         <param name="id">10</param>
+        <param name="p_gain">1120.0</param>
+        <param name="i_gain">170.0</param>
+        <param name="d_gain">3600.0</param>   
+        <!--param name="reboot_on_error">true</param-->
         <command_interface name="position"/>
         <!--command_interface name="velocity"/-->
         <state_interface name="position"/>
@@ -36,6 +38,10 @@ Add `serial_port`, `baud_rate`, and `joint_ids` parameters to the ros2_control p
       </joint>
       ...
 ```
+### Optional parameters
+When `reboot_on_error` is "true" the motor automaticaly reboots on mechanical overload error (it still loses torque for a brief moment, but that should be enogh to let the motor recover the fault).
+`p_gain`, `i_gain` and `d_gain` set the PID gain values of the motor.
+
 ## Run a test
 You need the urdf of the robot with the ros2_control xml added.
 After starting the control nodes and activating the Joint Trajectory Controller you can use ros2cli commands to test the actuators:
@@ -44,15 +50,7 @@ ros2 topic pub /joint_trajectory_controller/joint_trajectory trajectory_msgs/msg
 ```
 The example launch file expects the urdf in its robot description package and a robot bringup package with the `controller.yaml` (example for that is also in this package).
 
-## USB parameter tuning
+## USB Latency tuning
-To increase the data rate you can decrease the usb latency of the serial contoller.
-On linux you can do that with the help of sysfs:
-```
-sudo chmod o+w /sys/bus/usb-serial/devices/ttyUSB0/latency_timer
-echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer
-```
-
-# USB Latency tuning
 
 To increase the frequency the actuators can be read and be written to via usb you can decrease the usb latency. On Linux you can do that with the following command (if the dynamixel serial device is on `ttyUSB0`):
 ```
@@ -80,6 +78,5 @@ sudo systemctl enable usb-latency.service --now
 ```
 
 ## Credits
-This is partly based on the work of Youtalk (
+This is partly based on the work of Youtalk ([GitHub link](https://github.com/youtalk/dynamixel_control)), who made a dynamixel ros2_control driver based on the official dynamixel_workbench packages.
-[GitHub link](https://github.com/youtalk/dynamixel_control)), who made a dynamixel ros2_control driver.
+I swaped out the dynamixel_workbench parts with my own implementation, that only uses the dynamixelSDK.
-I swapt out the dynamixel_workbench parts with my own implementation, that only uses the dynamixelSDK.

include/dynamixel_ros2_control/dynamixel_driver.hpp

@@ -9,6 +9,7 @@
 #include <map>
 #include <memory>
 #include <vector>
+#include <limits>
 
 #include "dynamixel_sdk/dynamixel_sdk.h"
 #include "iostream"
@@ -20,9 +21,11 @@ class Driver {
    public:
 	typedef std::function<void(const std::string& joint_name)> hw_error_callback;
    private:
+   	class Motor;
 	struct GroupSyncReader {
 		GroupSyncRead syncRead;
 		std::vector<std::function<void(GroupSyncRead&)>> read_functions;
+		std::vector<Motor*> motors;
 		GroupSyncReader(
 			std::unique_ptr<PortHandler>& port,
 			std::unique_ptr<PacketHandler>& ph,
@@ -41,6 +44,8 @@ class Driver {
 		const model_info* model;
 
 		bool led;
+		bool rebooting;
+		bool rebooted;
 		
 		double goal_position;
 
@@ -48,13 +53,17 @@ class Driver {
 
 		//double goal_velocity;
 		double velocity;
-
 		double effort;
-
+		bool torque_target;
 		bool torque;
 
 		uint8_t hw_error;
 
+		double p_gain, i_gain, d_gain;
+		double p_gain_target, i_gain_target, d_gain_target;
+
+		std::vector<std::function<void()>> after_reboot_functions;
+
 		typedef std::function<void(dynamixel::hardware_status)> hw_error_callback;
 		//hw_error_callback error_callback;
 
@@ -84,9 +93,20 @@ class Driver {
 
 		void set_position_from_raw(uint32_t raw) { position = position_from_raw(raw); }
 		void set_velocity_from_raw(uint32_t raw) { velocity = velocity_from_raw(raw); }
-		void set_effort_from_raw(uint32_t raw) { effort = double(raw)*0.1; }
+		void set_effort_from_raw(uint32_t raw) { effort = double(static_cast<int16_t>(raw))*0.1; }
 
-		Motor(const motor_id p_id, std::string p_name, const model_t type) : id(p_id), name(p_name), goal_position(std::nan("")), position(std::nan("")){
+		Motor(const motor_id p_id, std::string p_name, const model_t type) : 
+				id(p_id), 
+				name(p_name), 
+				rebooting(true), 
+				rebooted(false), 
+				goal_position(std::numeric_limits<double>::quiet_NaN()), 
+				position(std::numeric_limits<double>::quiet_NaN()), 
+				torque_target(false),
+				p_gain(std::numeric_limits<double>::quiet_NaN()), 
+				i_gain(std::numeric_limits<double>::quiet_NaN()), 
+				d_gain(std::numeric_limits<double>::quiet_NaN()) 
+		{
 			try {
 				model = &model_infos.at(type);
 			} catch (const std::exception &e) {
@@ -109,9 +129,9 @@ class Driver {
 				command::Torque_Enable,
 				[this](uint32_t data) {
 					this->torque = bool(data);
-					if (this->torque == false) {
+					//if (this->torque == false) {
-						goal_position = std::nan("");
+						//goal_position = std::numeric_limits<double>::quiet_NaN();
-					}
+					//}
 				});
 
 			//Present Position
@@ -173,10 +193,40 @@ class Driver {
 								error_callback(hardware_status::Overload_Error);
 							}
 							//https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#hardware-error-status
+						} else {
+							this->rebooting = false;
+							this->rebooted = true;
 						}
 					});
 			}
+		}
 
+		void setup_init_sync_readers(
+			std::map<field, std::unique_ptr<GroupSyncReader>>& initSyncReaders,
+			std::unique_ptr<PortHandler>& portHandler,
+			std::unique_ptr<PacketHandler>& packetHandler) 
+		{
+			//Load PID gains if available
+			try {
+				setup_sync_reader(
+					initSyncReaders, portHandler, packetHandler,
+					command::Position_P_Gain,
+					[this](uint32_t raw) {
+						this->p_gain = double(static_cast<int16_t>(raw));
+					});
+				setup_sync_reader(
+					initSyncReaders, portHandler, packetHandler,
+					command::Position_I_Gain,
+					[this](uint32_t raw) {
+						this->i_gain = double(static_cast<int16_t>(raw));
+					});
+				setup_sync_reader(
+					initSyncReaders, portHandler, packetHandler,
+					command::Position_D_Gain,
+					[this](uint32_t raw) {
+						this->d_gain = double(static_cast<int16_t>(raw));
+					});
+			} catch (command_type_not_implemented& e) {}
 		}
 
 		void setup_sync_reader(
@@ -196,6 +246,7 @@ class Driver {
 					 std::make_unique<GroupSyncReader>(portHandler, packetHandler, sr_field)});
 			}
 			sync_reader_it->second->syncRead.addParam(this->id);
+			sync_reader_it->second->motors.push_back(this);
 			sync_reader_it->second->read_functions.push_back(
 				[this, sr_field, callback](GroupSyncRead& read) {
 					if (read.isAvailable(this->id, sr_field.address, sr_field.data_length)) {
@@ -213,6 +264,7 @@ class Driver {
 	std::unique_ptr<PortHandler> portHandler;
 	std::unique_ptr<PacketHandler> packetHandler;
 
+	std::map<field, std::unique_ptr<GroupSyncReader>> initSyncReaders;
 	std::map<field, std::unique_ptr<GroupSyncReader>> syncReaders;
 	std::map<field, std::unique_ptr<GroupSyncWrite>> syncWrites;
 
@@ -271,7 +323,11 @@ class Driver {
 	 * to prepare it for the writeing over the bus.
 	 */
 	void set_torque(const dynamixel::Driver::Motor::Ptr& motor, bool torque) {
+		motor->torque_target = torque;
 		if(torque && motor->hw_error) {
+			if(motor->rebooting) {
+				return;
+			}
 			throw hardware_error(motor, "Can't enable torque!");
 		}
 		field command_field;
@@ -280,8 +336,18 @@ class Driver {
 		add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&p));
 	}
 	
-    void reboot(const dynamixel::Driver::Motor::Ptr& motor) {
+    void reboot(dynamixel::Driver::Motor::Ptr& motor) {
+		motor->after_reboot_functions.clear();
+		//bool old_torque_target = motor->torque_target;
+		motor->after_reboot_functions.push_back([this, motor](){
+			motor->torque = true;
+			set_torque(motor, false);
+			this->pid_gains(motor->name, motor->p_gain_target, motor->i_gain_target, motor->d_gain_target);
+		});
 		packetHandler->reboot(portHandler.get(),motor->id);
+		set_torque(motor, false);
+		motor->torque = false;
+		write();
 	}
 
 	hw_error_callback overload_error_callback;
@@ -362,6 +428,8 @@ class Driver {
 		auto motor = std::make_shared<Motor>(id, joint_name, type);
 		motors[joint_name] = motor;
 		std::weak_ptr<Motor> weak_motor(motors[joint_name]);
+
+		motor->setup_init_sync_readers(initSyncReaders, portHandler, packetHandler);
 		motor->setup_sync_readers(syncReaders, portHandler, packetHandler, 
 			[this, weak_motor](dynamixel::hardware_status s)
 			{
@@ -390,12 +458,15 @@ class Driver {
 	/**
 	 * pushes the commands over the bus and reads the states back.
 	 */
-	void write() {
+	void write(bool enable_torque = false) {
 		for (const auto& [joint_name, motor]: motors) {
-			if(motor->torque) {
+			if(motor->torque_target != motor->torque) {
+				set_torque(motor, motor->torque_target);
+			}
+			if(motor->torque || enable_torque) {
 				if(std::isnan(motor->goal_position))
 					motor->goal_position = motor->position;
-				set_position(joint_name, motor->goal_position);
+				set_position(joint_name, motor->goal_position, enable_torque);
 			}
 		}
 
@@ -410,6 +481,22 @@ class Driver {
 	}
 
 	void read() {
+		bool init_done = true;
+		for (auto it = initSyncReaders.begin(); it != initSyncReaders.end(); std::advance(it,1)) {
+			const auto& [field, syncReader] = *it;
+			(void)field;
+			/*auto result = */ syncReader->syncRead.txRxPacket();
+			//std::cout<<packetHandler->getTxRxResult(result)<<std::endl;
+			for (auto read_f : syncReader->read_functions) {
+				read_f(syncReader->syncRead);
+			}
+			// If no motor is rebooting then the initial read was succesfull.
+			if (!std::all_of(syncReader->motors.cbegin(), syncReader->motors.cend(), 
+					[](const auto& m){ return !m->rebooting; })) {
+				init_done = false;
+			}
+		}
+
 		for (const auto& [field, syncReader] : syncReaders) {
 			(void)field;
 			/*auto result = */ syncReader->syncRead.txRxPacket();
@@ -418,6 +505,21 @@ class Driver {
 				read_f(syncReader->syncRead);
 			}
 		}
+
+		if(init_done) {
+			initSyncReaders.clear();
+			for (const auto& [joint_name, motor]: motors) {
+				if(motor->rebooted) {
+					motor->rebooted = false;
+					for(auto after_reboot_f:motor->after_reboot_functions) {
+						after_reboot_f();
+					}
+				}
+				if(motor->rebooted) {
+					motor->after_reboot_functions.clear();
+				}
+			}
+		}
 	}
 
 	/**
@@ -574,7 +676,6 @@ class Driver {
 	 * @param joint_name the name of the joint
 	 * @param position the position in radians the actuator should move to
 	 * @param enable_torque enable the torque if not enabled already
-	 * @return true if successfull, false otherwise (e.g. joint_name is not available)
 	 */
 	void set_position(const std::string& joint_name, double position, bool enable_torque = false) {
 		if (std::isnan(position))
@@ -585,9 +686,10 @@ class Driver {
 		auto motor = it_motor->second;	//motors[joint_name];
 
 		if (!motor->torque) {
-			if (enable_torque)
+			if (enable_torque) {
 				set_torque(motor, true);
-			else
+				return;
+			} else
 				throw torque_not_enabled(motor);
 		}
 
@@ -598,6 +700,54 @@ class Driver {
 		add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&p));
 	}
 
+	/**
+	 * Sets the pid gains for a motor. This is only possible after the first read 
+	 * as the initial values need to be read from the motor.
+	 * @param joint_name the name of the joint
+	 */
+
+	void pid_gains(const std::string& joint_name, double& p, double& i, double& d) {
+		auto it_motor = motors.find(joint_name);
+		if (it_motor == motors.end())
+			throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!");
+		auto motor = it_motor->second;
+		//std::cout <<joint_name<<" target p:"<< p<<"\ti:"<< i<<"\td:" << d << std::endl;
+		//std::cout <<joint_name<<" current p:"<< motor->p_gain<<"\ti:"<< motor->i_gain<<"\td:" << motor->d_gain << std::endl;
+		if(std::isnan( motor->p_gain) || std::isnan( motor->i_gain) || std::isnan( motor->d_gain)) {
+			throw std::invalid_argument("Cant set pid gains for \"" + joint_name + "\" because motor doesn't have pid control!");
+		}
+		if(std::isnan(p)) {
+			p = motor->p_gain;
+		}
+		if(std::isnan(i)) {
+			i = motor->i_gain;
+		}
+		if(std::isnan(d)) {
+			d = motor->d_gain;
+		}
+		if( p != motor->p_gain) {
+			field command_field;
+			motor->get_command(command::Position_P_Gain, command_field);
+			uint32_t data = static_cast<uint32_t>(p);
+			motor->p_gain_target = p;
+			add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&data));
+		}
+		if( i != motor->i_gain) {
+			field command_field;
+			motor->get_command(command::Position_I_Gain, command_field);
+			uint32_t data = static_cast<uint32_t>(i);
+			motor->i_gain_target = i;
+			add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&data));
+		}
+		if( d != motor->d_gain) {
+			field command_field;
+			motor->get_command(command::Position_D_Gain, command_field);
+			uint32_t data = static_cast<uint32_t>(d);
+			motor->d_gain_target = d;
+			add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&data));
+		}
+	}
+
 	/**
 	 * reboots a joint.
 	 * @param joint_name the name of the joint	
@@ -606,6 +756,7 @@ class Driver {
 		auto it_motor = motors.find(joint_name);
 		if (it_motor == motors.end())
 			throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!");
+		it_motor->second->rebooting = true;
 		reboot(it_motor->second);
 	}
 };

include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp

@@ -18,6 +18,9 @@ class DynamixelHardwareInterface : public hardware_interface::SystemInterface {
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardwareInterface)
 
+  DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC
+  DynamixelHardwareInterface();
+
   DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC
   CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
 

src/dynamixel_ros2_control.cpp

@@ -22,6 +22,7 @@
 #include <thread>
 #include <utility>
 #include <vector>
+#include <limits>
 
 #include "dynamixel_ros2_control/dynamixel_driver.hpp"
 
@@ -38,11 +39,20 @@
 #define DYN_PORTNAME_PARAM_STR "serial_port"
 #define DYN_ENABLE_TORQUE_ON_START_PARAM_STR "enable_torque_on_startup"
 #define DYN_AUTO_REBOOT_PARAM_STR "reboot_on_error"
+#define DYN_P_GAIN_PARAM_STR "p_gain"
+#define DYN_I_GAIN_PARAM_STR "i_gain"
+#define DYN_D_GAIN_PARAM_STR "d_gain"
 #define DYN_ID_PARAM_STR "id"
 
 #define DYN_DEFAULT_PORT_STR "/dev/ttyUSB0"
 
 namespace dynamixel_ros2_control {
+DynamixelHardwareInterface::DynamixelHardwareInterface() {
+  rclcpp::on_shutdown([&](){
+    dynamixel_driver_->set_torque_all(false);
+    dynamixel_driver_->write();
+  });
+}
 
 CallbackReturn DynamixelHardwareInterface::on_init(
     const hardware_interface::HardwareInfo &info) {
@@ -111,9 +121,43 @@ CallbackReturn DynamixelHardwareInterface::on_init(
               (std::string("Motors count: ") +
                std::to_string(dynamixel_driver_->get_motor_count()))
                   .c_str());
+
+  dynamixel_driver_->read();
+
+  for (const auto &joint : info_.joints) {
+    auto p_it = joint.parameters.find(DYN_P_GAIN_PARAM_STR);
+    double p = (p_it != joint.parameters.end()) ? std::stod(p_it->second)
+                                                : std::numeric_limits<double>::quiet_NaN();
+    auto i_it = joint.parameters.find(DYN_I_GAIN_PARAM_STR);
+    double i = (i_it != joint.parameters.end()) ? std::stod(i_it->second)
+                                                : std::numeric_limits<double>::quiet_NaN();
+    auto d_it = joint.parameters.find(DYN_D_GAIN_PARAM_STR);
+    double d = (d_it != joint.parameters.end()) ? std::stod(d_it->second)
+                                                : std::numeric_limits<double>::quiet_NaN();
+    try {
+      dynamixel_driver_->pid_gains(joint.name, p, i, d);
+      if (!std::isnan(p) || !std::isnan(i) || !std::isnan(d)) {
+        RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
+                    ("Joint-Actuator \"" + std::string(joint.name) +
+                     "\" p:%f\t i:%f\t d:%f")
+                        .c_str(),
+                    float(p), float(i), float(d));
+      }
+    } catch (const std::exception &e) {
+      if (p_it != joint.parameters.end() || i_it != joint.parameters.end() ||
+          d_it != joint.parameters.end()) {
+        RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
+                    ("Joint-Actuator \"" + std::string(joint.name) +
+                     "\" Cant set PID gains: " + e.what())
+                        .c_str());
+      }
+    }
+  }
   if (failure) {
     return CallbackReturn::ERROR;
   }
+  RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
+               "init success");
   return CallbackReturn::SUCCESS;
 }
 
@@ -148,7 +192,7 @@ void DynamixelHardwareInterface::overload_error_callback(
     try {
       dynamixel_driver_->reboot(joint_name);
       RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
-                  ("\"" + joint_name + " rebootet").c_str());
+                  ("\"" + joint_name + "\" rebooting").c_str());
       // succeeded = true;
     } catch (std::invalid_argument &e) {
       RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "%s", e.what());
@@ -158,7 +202,7 @@ void DynamixelHardwareInterface::overload_error_callback(
 
 std::vector<hardware_interface::StateInterface>
 DynamixelHardwareInterface::export_state_interfaces() {
-  RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
+  RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
                "export_state_interfaces");
   std::vector<hardware_interface::StateInterface> state_interfaces;
   dynamixel_driver_->for_each_joint([&](const std::string &joint_name) {
@@ -177,7 +221,7 @@ DynamixelHardwareInterface::export_state_interfaces() {
 
 std::vector<hardware_interface::CommandInterface>
 DynamixelHardwareInterface::export_command_interfaces() {
-  RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
+  RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
                "export_command_interfaces");
   std::vector<hardware_interface::CommandInterface> command_interfaces;
   dynamixel_driver_->for_each_joint([&](const std::string &joint_name) {
@@ -193,7 +237,7 @@ CallbackReturn DynamixelHardwareInterface::on_activate(
   RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "on_activate");
   try {
     dynamixel_driver_->ping_all();
-    RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
+    RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
                  "Pinging dynamixels was successfull");
   } catch (const dynamixel::Driver::dynamixel_bus_error &e) {
     RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what());
@@ -211,6 +255,8 @@ CallbackReturn DynamixelHardwareInterface::on_activate(
     RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what());
     return CallbackReturn::ERROR;
   }
+  RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
+               "activate success");
   return CallbackReturn::SUCCESS;
 }
 
@@ -221,6 +267,8 @@ CallbackReturn DynamixelHardwareInterface::on_deactivate(
     dynamixel_driver_->read();
     dynamixel_driver_->set_torque_all(false);
     dynamixel_driver_->write();
+    RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
+                "Motors deaktivated. Torque disabled !");
   } catch (std::invalid_argument &e) {
     RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what());
     return CallbackReturn::ERROR;
@@ -237,7 +285,7 @@ hardware_interface::return_type DynamixelHardwareInterface::read() {
 }
 
 hardware_interface::return_type DynamixelHardwareInterface::write() {
-  dynamixel_driver_->write();
+  dynamixel_driver_->write(true);
   return hardware_interface::return_type::OK;
 }