diff --git a/README.md b/README.md
index b52eeea..a28981f 100644
--- a/README.md
+++ b/README.md
@@ -26,6 +26,9 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part
       </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"/-->
@@ -35,7 +38,9 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part
       </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.
diff --git a/include/dynamixel_ros2_control/dynamixel_driver.hpp b/include/dynamixel_ros2_control/dynamixel_driver.hpp
index 46dfa61..f70f80b 100644
--- a/include/dynamixel_ros2_control/dynamixel_driver.hpp
+++ b/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,
@@ -42,6 +45,7 @@ class Driver {
 
 		bool led;
 		bool rebooting;
+		bool rebooted;
 		
 		double goal_position;
 
@@ -49,13 +53,16 @@ class Driver {
 
 		//double goal_velocity;
 		double velocity;
-
 		double effort;
-
 		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;
 
@@ -87,7 +94,17 @@ class Driver {
 		void set_velocity_from_raw(uint32_t raw) { velocity = velocity_from_raw(raw); }
 		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), rebooting(false), 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()), 
+				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) {
@@ -111,7 +128,7 @@ class Driver {
 				[this](uint32_t data) {
 					this->torque = bool(data);
 					if (this->torque == false) {
-						goal_position = std::nan("");
+						goal_position = std::numeric_limits<double>::quiet_NaN();
 					}
 				});
 
@@ -176,10 +193,38 @@ class Driver {
 							//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(
@@ -199,6 +244,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)) {
@@ -216,6 +262,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;
 
@@ -285,7 +332,11 @@ 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();
+		motor->after_reboot_functions.push_back([this, motor](){
+			this->pid_gains(motor->name, motor->p_gain_target, motor->i_gain_target, motor->d_gain_target);
+		});
 		packetHandler->reboot(portHandler.get(),motor->id);
 	}
 
@@ -367,6 +418,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)
 			{
@@ -415,6 +468,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();
@@ -423,6 +492,18 @@ 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();
+					}
+				}
+			}
+		}
 	}
 
 	/**
@@ -579,7 +660,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))
@@ -603,6 +683,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	
diff --git a/src/dynamixel_ros2_control.cpp b/src/dynamixel_ros2_control.cpp
index eacfe1c..874821a 100644
--- a/src/dynamixel_ros2_control.cpp
+++ b/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,6 +39,9 @@
 #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"
@@ -111,9 +115,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;
 }
 
@@ -158,7 +196,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 +215,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 +231,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 +249,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,7 +261,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 !");
+    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;