## page was renamed from controllers/Tutorials/Adding a PID to a realtime joint controller #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0=[[pr2_mechanism/Tutorials/Communicating with a realtime joint controller|Communicating with a realtime joint controller]] ## note.1 ## descriptive title for the tutorial ## title = Adding a PID to a realtime joint controller ## multi-line description to be displayed in search ## description = This tutorial teaches you how to add a PID object to a realtime joint controller ## the next tutorial description. ## the next tutorial description (optional) ## next = ## links to next tutorial (optional) ## next.0.link=[[pr2_mechanism/Tutorials/Capturing data from a controller|Capturing data from a controller]] ## next.1.link= ## what level user is this tutorial for ## level= IntermediateCategory ## keywords = #################################### <> <> == Introduction == This tutorial builds on the controller we created in the [[pr2_mechanism/Tutorials/Communicating with a realtime joint controller|previous tutorial]]. Here we will start using a PID object inside the controller. The PID controller is part of the [[control_toolbox]] package. == Adding the PID == === The code === Now we need to add the pid controller to our controller code. The resulting code is shown below: '''include/my_controller_pkg/my_controller_file.h''' {{{ #!cplusplus block=controller_h #include #include #include #include #include namespace my_controller_ns{ class MyControllerClass: public pr2_controller_interface::Controller { private: bool setAmplitude(my_controller_pkg::SetAmplitude::Request& req, my_controller_pkg::SetAmplitude::Response& resp); pr2_mechanism_model::JointState* joint_state_; double init_pos_; double amplitude_; ros::ServiceServer srv_; control_toolbox::Pid pid_controller_; pr2_mechanism_model::RobotState *robot_; ros::Time time_of_last_cycle_; public: virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); virtual void starting(); virtual void update(); virtual void stopping(); }; } }}} Compared to the original code, the header changed in the following places: <> Here we include the pid controller and a gains setter that will allow us to change to controller gains on the fly using service calls. Very similar to the service we added to change the amplitude of the sinusoid on the fly. <> Here we add a class variable to store * the pid object, * a pointer to the robot model (this is the pointer that is given to us in the init(..) method, and * the last time the controller update loop ran. With this time we can compute the delta time between two consecutive runs of the update loop. Next, here is the new code for the cpp file: '''src/my_controller_file.cpp''' {{{ #!cplusplus block=controller_cpp #include "my_controller_pkg/my_controller_file.h" #include using namespace my_controller_ns; /// Controller initialization in non-realtime bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) { // get joint name std::string joint_name; if (!n.getParam("joint_name", joint_name)) { ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str()); return false; } // get pointer to joint state joint_state_ = robot->getJointState(joint_name); if (!joint_state_) { ROS_ERROR("MyController could not find joint named '%s'", joint_name.c_str()); return false; } // advertise service to set amplitude amplitude_ = 0.5; srv_ = n.advertiseService("set_amplitude", &MyControllerClass::setAmplitude, this); // copy robot pointer so we can access time robot_ = robot; // construct pid controller if (!pid_controller_.init(ros::NodeHandle(n, "pid_parameters"))){ ROS_ERROR("MyController could not construct PID controller for joint '%s'", joint_name.c_str()); return false; } return true; } /// Controller startup in realtime void MyControllerClass::starting() { init_pos_ = joint_state_->position_; time_of_last_cycle_ = robot_->getTime(); pid_controller_.reset(); } /// Controller update loop in realtime void MyControllerClass::update() { double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec()); double current_pos = joint_state_->position_; ros::Duration dt = robot_->getTime() - time_of_last_cycle_; time_of_last_cycle_ = robot_->getTime(); joint_state_->commanded_effort_ = pid_controller_.updatePid(current_pos-desired_pos, dt); } /// Controller stopping in realtime void MyControllerClass::stopping() {} /// Service call to set amplitude of sin bool MyControllerClass::setAmplitude(my_controller_pkg::SetAmplitude::Request& req, my_controller_pkg::SetAmplitude::Response& resp) { if (fabs(req.amplitude) < 2.0){ amplitude_ = req.amplitude; ROS_INFO("Mycontroller: set amplitude too %f", req.amplitude); } else ROS_WARN("MyController: requested amplitude %f too large", req.amplitude); resp.amplitude = amplitude_; return true; } /// Register controller to pluginlib PLUGINLIB_REGISTER_CLASS(MyControllerPlugin, my_controller_ns::MyControllerClass, pr2_controller_interface::Controller) }}} The changes in the cpp file are: <> Here we store a copy to the robot. The robot object is used to get the time at which the controllers are triggered. This time is different from ros::Time::now()! The time provided by the robot model is the same for all controllers, and is the time at which the controller manager started triggering all controllers. <> Here we initialize the new pid controller. The init method takes a NodeHandle that specifies the namespace in which the pid parameters are specified. In this case, the namespace is the controller name plus the string "pid_parameters". This namespace is important to set the pid values in our launch file. <> In the starting method of the controller we store the controller time, so we can later compute the difference in time between two update loops. <> This is the place we actually use the pid controller. The pid needs an error signal, plus the time interval. First we compute the time interval "dt", and then we compute the error signal as the difference between the measured and desired position. /!\ Note that the pid controller uses negative feedback (error = actual - desired) === Configuration === To compile this new code, you should add the [[control_toolbox]] package as a dependency to your package. Add the following line to your '''manifest.xml''': {{{ }}} So now we're ready to build the controller. Because we just added a dependency, and we want to be sure this dependency gets build too, we use "rosmake" instead of just "make" {{{ $ rosmake }}} Next, we should add the parameters for the pid to the yaml file '''my_controller.yaml''': {{{ my_controller_name: type: MyControllerPlugin joint_name: r_shoulder_pan_joint pid_parameters: p: 10.0 i: 0.0 d: 0.0 i_clamp: 0.0 }}} == Running the controller == As before, the new code has to get loaded into the realtime process. So you can either restart all of gazebo with: {{{ $ roslaunch gazebo_worlds empty_world.launch $ roslaunch pr2_gazebo pr2.launch }}} OR if gazebo is still running, you can use the [[pr2_controller_manager]] to request a reload of the libraries: {{{ $ rosrun pr2_controller_manager pr2_controller_manager reload-libraries }}} And now, launch our new controller: {{{ $ roslaunch my_controller.launch }}} ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## WritingYourselfCategory ## JointSpaceExtraCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE