Tutorials/intermediate/pid control joint

Create a working directory
 * 1) Setup ##

cd ~ mkdir gazebo_pid_joints cd gazebo_pid_joints


 * 1) World File ##

The world for this tutorial will consist of a box connected to the inertial frame ("world") with a prismatic joint, where the primistic joint operates along the Z axis (vertically).

Copy the SDF code below into a file called `pid_joints.world`

  0.000000 0.000000 0.000000 1.000000      1.000000 1.000000 1.000000 1.000000       1      0.000000 0.000000 -9.800000          quick 0.0010000 100          1.300000       1000.000000  1        0.000000 0.000000 1.000000              100.000000 100.000000                 100.000000 50.000000         0 0.000000 0.000000 1.000000              100.000000 100.000000             Gazebo/Grey  <model name='box'> <link name='base_link'> 0.000000 0.000000 0.500000 0.000000 -0.000000 0.000000            1.000000             0.000000             0.000000             1.000000             0.000000             1.000000           1.000000         <collision name='collision'> 1.000000 1.000000 1.000000        <visual name='visual'> 1.000000 1.000000 1.000000        <velocity_decay/> <joint name='my_joint' type='prismatic'> world base_link 0.000000 0.000000 1.000000      <plugin name='pid_joints' filename='libpid_joints.so'/> <light name='sun' type='directional'> 0.000000 0.000000 10.000000 0.000000 -0.000000 0.000000      1.000000 1.000000 1.000000 1.000000       0.100000 0.100000 0.100000 1.000000         10.000000         0.010000         0.800000         0.000000       0.000000 0.200000 -1.000000       1</cast_shadows>

The model plugin below will cause the prismatic joint to hold position using the `gazebo::common::PID` controller class.
 * 1) Plugin ##

Copy the code below into a file called `pid_joints.cc`


 * 1) include "common/common.h"
 * 2) include "physics/physics.h"
 * 3) include "common/Events.hh"
 * 4) include "common/PID.hh"

namespace gazebo { class PIDJoints : public ModelPlugin {   public: void Load(physics::ModelPtr _model, sdf::ElementPtr /*_sdf*/) {     this->model_ = _model; // initialize a PID class this->target_position_ = 1.8; this->pid.Init(100, 0, 1, 0, 0, 100, -100); this->pid.SetCmd(this->target_position_); this->joint_ = this->model_->GetJoint("my_joint"); this->last_update_time_ = this->model_->GetWorld->GetSimTime; this->update_connection_ = event::Events::ConnectWorldUpdateStart(       boost::bind(&PIDJoints::UpdatePID, this)); }   void UpdatePID {     common::Time current_time = this->model_->GetWorld->GetSimTime; double error = this->joint_->GetAngle(0).GetAsRadian - target_position_; double dt   = current_time.Double - this->last_update_time_.Double; this->pid.Update(error, dt); this->joint_->SetForce(0, this->pid.GetCmd); this->last_update_time_ = current_time; gzdbg << "error [" << error << "] cmd [" << this->pid.GetCmd << "]\n"; }   common::PID pid; double target_position_; physics::JointPtr joint_; physics::ModelPtr model_; event::ConnectionPtr update_connection_; common::Time last_update_time_; };

// Register this plugin with the simulator GZ_REGISTER_MODEL_PLUGIN(PIDJoints) }


 * 1) Build ##

Create the CMake file by copying the script below into a file called `CMakeLists.txt`

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

include (FindPkgConfig) if (PKG_CONFIG_FOUND) pkg_check_modules(GAZEBO gazebo) endif include_directories(${GAZEBO_INCLUDE_DIRS}) link_directories(${GAZEBO_LIBRARY_DIRS})

add_library(pid_joints SHARED pid_joints.cc) target_link_libraries(pid_joints ${GAZEBO_LIBRARIES})

Build the plugin

mkdir build cd build cmake ../ make


 * 1) Run ##

In a terminal run the world cd ~/gazebo_pid_joints/build gzserver ../pid_joints.world

Run the client in a separate terminal

gzclient