mimic_joint_plugin.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  * Copyright (c) 2014, Konstantinos Chatzilygeroudis
4  * Copyright (c) 2016, CRI Lab at Nanyang Technological University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Univ of CO, Boulder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 
37 namespace gazebo
38 {
39 
41 {
42  kill_sim = false;
43 
44  joint_.reset();
45  mimic_joint_.reset();
46 }
47 
49 {
50  event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
51 
52  kill_sim = true;
53 }
54 
55 void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
56 {
57  ros::NodeHandle model_nh;
58  model_ = _parent;
59  world_ = model_->GetWorld();
60 
61  // Error message if the model couldn't be found
62  if (!model_)
63  {
64  ROS_ERROR("Parent model is NULL! MimicJointPlugin could not be loaded.");
65  return;
66  }
67 
68  // Check that ROS has been initialized
69  if(!ros::isInitialized())
70  {
71  ROS_ERROR("A ROS node for Gazebo has not been initialized, unable to load plugin.");
72  return;
73  }
74 
75  // Check for robot namespace
76  robot_namespace_ = "/";
77  if(_sdf->HasElement("robotNamespace"))
78  {
79  robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
80  }
81 
82  // Check for joint element
83  if(!_sdf->HasElement("joint"))
84  {
85  ROS_ERROR("No joint element present. MimicJointPlugin could not be loaded.");
86  return;
87  }
88 
89  joint_name_ = _sdf->GetElement("joint")->Get<std::string>();
90 
91  // Check for mimicJoint element
92  if(!_sdf->HasElement("mimicJoint"))
93  {
94  ROS_ERROR("No mimicJoint element present. MimicJointPlugin could not be loaded.");
95  return;
96  }
97 
98  mimic_joint_name_ = _sdf->GetElement("mimicJoint")->Get<std::string>();
99 
100  has_pid_ = false;
101  // Check if PID controller wanted
102  if(_sdf->HasElement("hasPID"))
103  {
104  has_pid_ = true;
105 
106  const ros::NodeHandle nh(model_nh, std::string(robot_namespace_+"/gazebo_ros_control/pid_gains/")+mimic_joint_name_);
107  double p, i,d ;
108  // TODO: include i_clamp e.t.c.
109  nh.param("p", p, 0.0);
110  nh.param("i", i, 0.0);
111  nh.param("d", d, 0.0);
112 
113  pid_ = control_toolbox::Pid(p,i,d);
114  }
115 
116  // Check for multiplier element
117  multiplier_ = 1.0;
118  if(_sdf->HasElement("multiplier"))
119  multiplier_ = _sdf->GetElement("multiplier")->Get<double>();
120 
121  // Check for offset element
122  offset_ = 0.0;
123  if (_sdf->HasElement("offset"))
124  offset_ = _sdf->GetElement("offset")->Get<double>();
125 
126  // Check for sensitiveness element
127  sensitiveness_ = 0.0;
128  if (_sdf->HasElement("sensitiveness"))
129  sensitiveness_ = _sdf->GetElement("sensitiveness")->Get<double>();
130 
131  // Check for max effort
132  max_effort_ = 1.0;
133  if (_sdf->HasElement("maxEffort"))
134  {
135  max_effort_ = _sdf->GetElement("maxEffort")->Get<double>();
136  }
137 
138  // Get pointers to joints
139  joint_ = model_->GetJoint(joint_name_);
140  if(!joint_)
141  {
142  ROS_ERROR("No joint named %s. MimicJointPlugin could not be loaded.", joint_name_.c_str());
143  return;
144  }
146  if(!mimic_joint_)
147  {
148  ROS_ERROR("No (mimic) joint named %s. MimicJointPlugin could not be loaded.", mimic_joint_name_.c_str());
149  return;
150  }
151 
152  // Set max effort
153  if(!has_pid_)
154  {
155  #if GAZEBO_MAJOR_VERSION > 2
156  mimic_joint_->SetParam("fmax", 0, max_effort_);
157  #else
158  mimic_joint_->SetMaxForce(0, max_effort_);
159  #endif
160  }
161 
162  // Listen to the update event. This event is broadcast every
163  // simulation iteration.
164  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
165  boost::bind(&MimicJointPlugin::UpdateChild, this));
166 }
167 
169 {
170  static ros::Duration period(world_->GetPhysicsEngine()->GetMaxStepSize());
171 
172  // Set mimic joint's angle based on joint's angle
173  double angle = joint_->GetAngle(0).Radian()*multiplier_+offset_;
174 
175  if(abs(angle-mimic_joint_->GetAngle(0).Radian())>=sensitiveness_)
176  {
177  if(has_pid_)
178  {
179  double a = mimic_joint_->GetAngle(0).Radian();
180  if(a!=a)
181  a = angle;
182  double error = angle-a;
183  double effort = gazebo::math::clamp(pid_.computeCommand(error, period), -max_effort_, max_effort_);
184  }
185  else
186  {
187  #if GAZEBO_MAJOR_VERSION >= 4
188  mimic_joint_->SetPosition(0, angle);
189  #else
190  mimic_joint_->SetAngle(0, angle);
191  #endif
192  }
193  }
194 }
195 
197 
198 }
control_toolbox::Pid pid_
event::ConnectionPtr updateConnection
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin)
physics::JointPtr mimic_joint_


dsr_description
Author(s): Kab Kyoum Kim , Jin Hyuk Gong , Jeongwoo Lee
autogenerated on Sat May 18 2019 02:32:47