From: costashatz Date: Fri, 6 Mar 2015 14:07:11 +0000 (+0200) Subject: Added PID control capability to mimic joint plugin X-Git-Tag: 0.1.0~18 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=e92133eedc2d96176e06e70d1e56f640d2524fdc;p=roboticsgroup_upatras_gazebo_plugins.git Added PID control capability to mimic joint plugin --- diff --git a/CMakeLists.txt b/CMakeLists.txt index dd3a25c..4e3f7c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,8 @@ project(roboticsgroup_gazebo_plugins) # Load catkin and all dependencies required for this package find_package(catkin REQUIRED COMPONENTS roscpp - gazebo_ros + gazebo_ros + control_toolbox ) # Depend on system install of Gazebo @@ -23,5 +24,6 @@ target_link_libraries(roboticsgroup_gazebo_disable_link_plugin ${catkin_LIBRARIE catkin_package( DEPENDS roscpp - gazebo_ros + gazebo_ros + control_toolbox ) \ No newline at end of file diff --git a/README.md b/README.md index d8e820b..a2afc12 100644 --- a/README.md +++ b/README.md @@ -32,6 +32,12 @@ A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint fun A **double** specifying the sensitiveness of the mimic joint. Defaults to 0.0. It basically is the threshold of the difference between the 2 angles (joint's and mimic's) before applying the "mimicness". + - robotNamespace + A **string** specifying the namespace the robot is under. Defaults to '/'. + + - hasPID + Determines whether the joint has PID in order to be controlled via PID position/effort controller. Takes no value: ** means that the mimic joint is controlled via PID. Ommit it so that the mimic joint is controlled via setAngle. + ###DisableLinkPlugin A simple (Model) plugin for Gazebo that allows you to disable a link in Gazebo's physics engine. diff --git a/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h b/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h index 92e63db..bd21dc8 100644 --- a/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h +++ b/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h @@ -26,6 +26,9 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE // ROS includes #include +// ros_control +#include + // Boost includes #include @@ -35,9 +38,6 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE #include #include - -using std::string; - namespace gazebo { class MimicJointPlugin : public ModelPlugin @@ -51,11 +51,15 @@ namespace gazebo private: // Parameters - string joint_name_, mimic_joint_name_; + std::string joint_name_, mimic_joint_name_, robot_namespace_; double multiplier_, offset_, sensitiveness_, max_effort_; + bool has_pid_; bool kill_sim; + // PID controller if needed + control_toolbox::Pid pid_; + // Pointers to the joints physics::JointPtr joint_, mimic_joint_; diff --git a/package.xml b/package.xml index 49b236f..21a2ceb 100644 --- a/package.xml +++ b/package.xml @@ -37,10 +37,12 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE gazebo_ros roscpp + control_toolbox gazebo_ros roscpp + control_toolbox diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp index 2846cb7..ef8860f 100644 --- a/src/mimic_joint_plugin.cpp +++ b/src/mimic_joint_plugin.cpp @@ -41,8 +41,30 @@ MimicJointPlugin::~MimicJointPlugin() void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ) { - this->model_ = _parent; - this->world_ = this->model_->GetWorld(); + ros::NodeHandle model_nh; + model_ = _parent; + world_ = model_->GetWorld(); + + // Error message if the model couldn't be found + if (!model_) + { + ROS_ERROR("Parent model is NULL! GazeboNaoqiControlPlugin could not be loaded."); + return; + } + + // Check that ROS has been initialized + if(!ros::isInitialized()) + { + ROS_ERROR("A ROS node for Gazebo has not been initialized, unable to load plugin."); + return; + } + + // Check for robot namespace + robot_namespace_ = "/"; + if(_sdf->HasElement("robotNamespace")) + { + robot_namespace_ = _sdf->GetElement("robotNamespace")->Get(); + } // Check for joint element if(!_sdf->HasElement("joint")) @@ -62,6 +84,22 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ) mimic_joint_name_ = _sdf->GetElement("mimicJoint")->Get(); + has_pid_ = false; + // Check if PID controller wanted + if(_sdf->HasElement("hasPID")) + { + has_pid_ = true; + + const ros::NodeHandle nh(model_nh, std::string(robot_namespace_+"/gazebo_ros_control/pid_gains/")+mimic_joint_name_); + double p, i,d ; + // TO-DO: include i_clamp e.t.c. + nh.param("p", p, 0.0); + nh.param("i", i, 0.0); + nh.param("d", d, 0.0); + + pid_ = control_toolbox::Pid(p,i,d); + } + // Check for multiplier element multiplier_ = 1.0; if(_sdf->HasElement("multiplier")) @@ -80,7 +118,9 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ) // Check for max effort max_effort_ = 1.0; if (_sdf->HasElement("maxEffort")) + { max_effort_ = _sdf->GetElement("maxEffort")->Get(); + } // Get pointers to joints joint_ = model_->GetJoint(joint_name_); @@ -97,7 +137,8 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ) } // Set max effort - mimic_joint_->SetMaxForce(0,max_effort_); + if(!has_pid_) + mimic_joint_->SetMaxForce(0,max_effort_); // Listen to the update event. This event is broadcast every // simulation iteration. @@ -107,9 +148,24 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ) void MimicJointPlugin::UpdateChild() { + static ros::Duration period(world_->GetPhysicsEngine()->GetMaxStepSize()); + // Set mimic joint's angle based on joint's angle - if(abs((joint_->GetAngle(0).Radian()*multiplier_+offset_)-mimic_joint_->GetAngle(0).Radian())>=sensitiveness_) - mimic_joint_->SetAngle(0, math::Angle(joint_->GetAngle(0).Radian()*multiplier_+offset_)); + double angle = joint_->GetAngle(0).Radian()*multiplier_+offset_; + + if(abs(angle-mimic_joint_->GetAngle(0).Radian())>=sensitiveness_) + { + if(has_pid_) + { + double a = mimic_joint_->GetAngle(0).Radian(); + if(a!=a) + a = angle; + double error = angle-a; + double effort = gazebo::math::clamp(pid_.computeCommand(error, period), -max_effort_, max_effort_); + } + else + mimic_joint_->SetAngle(0, math::Angle(angle)); + } } GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin); \ No newline at end of file