default_robot_hw_sim.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Open Source Robotics Foundation
5  * Copyright (c) 2013, The Johns Hopkins University
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Open Source Robotics Foundation
19  * nor the names of its contributors may be
20  * used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Author: Dave Coleman, Jonathan Bohren
38  Desc: Hardware Interface for any simulated robot in Gazebo
39 */
40 
41 
43 #include <urdf/model.h>
44 
45 
46 namespace
47 {
48 
49 double clamp(const double val, const double min_val, const double max_val)
50 {
51  return std::min(std::max(val, min_val), max_val);
52 }
53 
54 }
55 
56 namespace gazebo_ros_control
57 {
58 
59 
61  const std::string& robot_namespace,
62  ros::NodeHandle model_nh,
63  gazebo::physics::ModelPtr parent_model,
64  const urdf::Model *const urdf_model,
65  std::vector<transmission_interface::TransmissionInfo> transmissions)
66 {
67  // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each
68  // parameter's name is "joint_limits/<joint name>". An example is "joint_limits/axle_joint".
69  const ros::NodeHandle joint_limit_nh(model_nh);
70 
71  // Resize vectors to our DOF
72  n_dof_ = transmissions.size();
73  joint_names_.resize(n_dof_);
74  joint_types_.resize(n_dof_);
79  pid_controllers_.resize(n_dof_);
80  joint_position_.resize(n_dof_);
81  joint_velocity_.resize(n_dof_);
82  joint_effort_.resize(n_dof_);
86 
87  // Initialize values
88  for(unsigned int j=0; j < n_dof_; j++)
89  {
90  // Check that this transmission has one joint
91  if(transmissions[j].joints_.size() == 0)
92  {
93  ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
94  << " has no associated joints.");
95  continue;
96  }
97  else if(transmissions[j].joints_.size() > 1)
98  {
99  ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
100  << " has more than one joint. Currently the default robot hardware simulation "
101  << " interface only supports one.");
102  continue;
103  }
104 
105  std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
106  if (joint_interfaces.empty() &&
107  !(transmissions[j].actuators_.empty()) &&
108  !(transmissions[j].actuators_[0].hardware_interfaces_.empty()))
109  {
110  // TODO: Deprecate HW interface specification in actuators in ROS J
111  joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
112  ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The <hardware_interface> element of tranmission " <<
113  transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
114  "The transmission will be properly loaded, but please update " <<
115  "your robot model to remain compatible with future versions of the plugin.");
116  }
117  if (joint_interfaces.empty())
118  {
119  ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
120  " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
121  "Not adding it to the robot hardware simulation.");
122  continue;
123  }
124  else if (joint_interfaces.size() > 1)
125  {
126  ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
127  " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
128  "Currently the default robot hardware simulation interface only supports one. Using the first entry");
129  //continue;
130  }
131 
132  // Add data from transmission
133  joint_names_[j] = transmissions[j].joints_[0].name_;
134  joint_position_[j] = 1.0;
135  joint_velocity_[j] = 0.0;
136  joint_effort_[j] = 1.0; // N/m for continuous joints
137  joint_effort_command_[j] = 0.0;
138  joint_position_command_[j] = 0.0;
139  joint_velocity_command_[j] = 0.0;
140 
141  const std::string& hardware_interface = joint_interfaces.front();
142 
143  // Debug
144  ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j]
145  << "' of type '" << hardware_interface << "'");
146 
147  // Create joint state interface for all joints
150 
151  // Decide what kind of command interface this actuator/joint has
152  hardware_interface::JointHandle joint_handle;
153  if(hardware_interface == "EffortJointInterface" || hardware_interface == "hardware_interface/EffortJointInterface")
154  {
155  // Create effort joint interface
159  ej_interface_.registerHandle(joint_handle);
160  }
161  else if(hardware_interface == "PositionJointInterface" || hardware_interface == "hardware_interface/PositionJointInterface")
162  {
163  // Create position joint interface
167  pj_interface_.registerHandle(joint_handle);
168  }
169  else if(hardware_interface == "VelocityJointInterface" || hardware_interface == "hardware_interface/VelocityJointInterface")
170  {
171  // Create velocity joint interface
175  vj_interface_.registerHandle(joint_handle);
176  }
177  else
178  {
179  ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '"
180  << hardware_interface << "' while loading interfaces for " << joint_names_[j] );
181  return false;
182  }
183 
184  if(hardware_interface == "EffortJointInterface" || hardware_interface == "PositionJointInterface" || hardware_interface == "VelocityJointInterface") {
185  ROS_WARN_STREAM("Deprecated syntax, please prepend 'hardware_interface/' to '" << hardware_interface << "' within the <hardwareInterface> tag in joint '" << joint_names_[j] << "'.");
186  }
187 
188  // Get the gazebo joint that corresponds to the robot joint.
189  //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: "
190  // << joint_names_[j]);
191  gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
192  if (!joint)
193  {
194  ROS_ERROR_STREAM_NAMED("default_robot_hw", "This robot has a joint named \"" << joint_names_[j]
195  << "\" which is not in the gazebo model.");
196  return false;
197  }
198  sim_joints_.push_back(joint);
199 
200  // get physics engine type
201 #if GAZEBO_MAJOR_VERSION >= 8
202  gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics();
203 #else
204  gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine();
205 #endif
206  physics_type_ = physics->GetType();
207  if (physics_type_.empty())
208  {
209  ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "No physics type found.");
210  }
211 
213  joint_limit_nh, urdf_model,
216  if (joint_control_methods_[j] != EFFORT)
217  {
218  // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
219  // joint->SetParam("vel") to control the joint.
220  const ros::NodeHandle nh(robot_namespace + "/gazebo_ros_control/pid_gains/" +
221  joint_names_[j]);
222  if (pid_controllers_[j].init(nh, true))
223  {
224  switch (joint_control_methods_[j])
225  {
226  case POSITION:
228  break;
229  case VELOCITY:
231  break;
232  }
233  }
234  else
235  {
236  // joint->SetParam("fmax") must be called if joint->SetAngle() or joint->SetParam("vel") are
237  // going to be called. joint->SetParam("fmax") must *not* be called if joint->SetForce() is
238  // going to be called.
239 #if GAZEBO_MAJOR_VERSION > 2
240  joint->SetParam("fmax", 0, joint_effort_limits_[j]);
241 #else
242  joint->SetMaxForce(0, joint_effort_limits_[j]);
243 #endif
244  }
245  }
246  }
247 
248  // Register interfaces
253 
254  // Initialize the emergency stop code.
255  e_stop_active_ = false;
256  last_e_stop_active_ = false;
257 
258  return true;
259 }
260 
262 {
263  for(unsigned int j=0; j < n_dof_; j++)
264  {
265  // Gazebo has an interesting API...
266 #if GAZEBO_MAJOR_VERSION >= 8
267  double position = sim_joints_[j]->Position(0);
268 #else
269  double position = sim_joints_[j]->GetAngle(0).Radian();
270 #endif
271  if (joint_types_[j] == urdf::Joint::PRISMATIC)
272  {
273  joint_position_[j] = position;
274  }
275  else
276  {
278  position);
279  }
280  joint_velocity_[j] = sim_joints_[j]->GetVelocity(0);
281  joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0));
282  }
283 }
284 
286 {
287  // If the E-stop is active, joints controlled by position commands will maintain their positions.
288  if (e_stop_active_)
289  {
290  if (!last_e_stop_active_)
291  {
293  last_e_stop_active_ = true;
294  }
296  }
297  else
298  {
299  last_e_stop_active_ = false;
300  }
301 
308 
309  for(unsigned int j=0; j < n_dof_; j++)
310  {
311  switch (joint_control_methods_[j])
312  {
313  case EFFORT:
314  {
315  const double effort = e_stop_active_ ? 0 : joint_effort_command_[j];
316  sim_joints_[j]->SetForce(0, effort);
317  }
318  break;
319 
320  case POSITION:
321 #if GAZEBO_MAJOR_VERSION >= 9
322  sim_joints_[j]->SetPosition(0, joint_position_command_[j], true);
323 #else
324  sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
325 #endif
326  break;
327 
328  case POSITION_PID:
329  {
330  double error;
331  switch (joint_types_[j])
332  {
333  case urdf::Joint::REVOLUTE:
338  error);
339  break;
340  case urdf::Joint::CONTINUOUS:
343  break;
344  default:
346  }
347 
348  const double effort_limit = joint_effort_limits_[j];
349  const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
350  -effort_limit, effort_limit);
351  sim_joints_[j]->SetForce(0, effort);
352  }
353  break;
354 
355  case VELOCITY:
356 #if GAZEBO_MAJOR_VERSION > 2
357  if (physics_type_.compare("dart") == 0)
358  {
359  sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
360  }
361  else
362  {
363  sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
364  }
365 #else
366  sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
367 #endif
368  break;
369 
370  case VELOCITY_PID:
371  double error;
372  if (e_stop_active_)
373  error = -joint_velocity_[j];
374  else
376  const double effort_limit = joint_effort_limits_[j];
377  const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
378  -effort_limit, effort_limit);
379  sim_joints_[j]->SetForce(0, effort);
380  break;
381  }
382  }
383 }
384 
385 void DefaultRobotHWSim::eStopActive(const bool active)
386 {
387  e_stop_active_ = active;
388 }
389 
390 // Register the limits of the joint specified by joint_name and joint_handle. The limits are
391 // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also.
392 // Return the joint's type, lower position limit, upper position limit, and effort limit.
393 void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name,
394  const hardware_interface::JointHandle& joint_handle,
395  const ControlMethod ctrl_method,
396  const ros::NodeHandle& joint_limit_nh,
397  const urdf::Model *const urdf_model,
398  int *const joint_type, double *const lower_limit,
399  double *const upper_limit, double *const effort_limit)
400 {
401  *joint_type = urdf::Joint::UNKNOWN;
402  *lower_limit = -std::numeric_limits<double>::max();
403  *upper_limit = std::numeric_limits<double>::max();
404  *effort_limit = std::numeric_limits<double>::max();
405 
407  bool has_limits = false;
409  bool has_soft_limits = false;
410 
411  if (urdf_model != NULL)
412  {
413  const urdf::JointConstSharedPtr urdf_joint = urdf_model->getJoint(joint_name);
414  if (urdf_joint != NULL)
415  {
416  *joint_type = urdf_joint->type;
417  // Get limits from the URDF file.
418  if (joint_limits_interface::getJointLimits(urdf_joint, limits))
419  has_limits = true;
420  if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
421  has_soft_limits = true;
422  }
423  }
424  // Get limits from the parameter server.
425  if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits))
426  has_limits = true;
427 
428  if (!has_limits)
429  return;
430 
431  if (*joint_type == urdf::Joint::UNKNOWN)
432  {
433  // Infer the joint type.
434 
435  if (limits.has_position_limits)
436  {
437  *joint_type = urdf::Joint::REVOLUTE;
438  }
439  else
440  {
441  if (limits.angle_wraparound)
442  *joint_type = urdf::Joint::CONTINUOUS;
443  else
444  *joint_type = urdf::Joint::PRISMATIC;
445  }
446  }
447 
448  if (limits.has_position_limits)
449  {
450  *lower_limit = limits.min_position;
451  *upper_limit = limits.max_position;
452  }
453  if (limits.has_effort_limits)
454  *effort_limit = limits.max_effort;
455 
456  if (has_soft_limits)
457  {
458  switch (ctrl_method)
459  {
460  case EFFORT:
461  {
463  limits_handle(joint_handle, limits, soft_limits);
464  ej_limits_interface_.registerHandle(limits_handle);
465  }
466  break;
467  case POSITION:
468  {
470  limits_handle(joint_handle, limits, soft_limits);
471  pj_limits_interface_.registerHandle(limits_handle);
472  }
473  break;
474  case VELOCITY:
475  {
477  limits_handle(joint_handle, limits, soft_limits);
478  vj_limits_interface_.registerHandle(limits_handle);
479  }
480  break;
481  }
482  }
483  else
484  {
485  switch (ctrl_method)
486  {
487  case EFFORT:
488  {
490  sat_handle(joint_handle, limits);
491  ej_sat_interface_.registerHandle(sat_handle);
492  }
493  break;
494  case POSITION:
495  {
497  sat_handle(joint_handle, limits);
498  pj_sat_interface_.registerHandle(sat_handle);
499  }
500  break;
501  case VELOCITY:
502  {
504  sat_handle(joint_handle, limits);
505  vj_sat_interface_.registerHandle(sat_handle);
506  }
507  break;
508  }
509  }
510 }
511 
512 }
513 
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_
hardware_interface::PositionJointInterface pj_interface_
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_
virtual void readSim(ros::Time time, ros::Duration period)
Read state data from the simulated robot hardware.
joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_
hardware_interface::VelocityJointInterface vj_interface_
std::vector< control_toolbox::Pid > pid_controllers_
void enforceLimits(const ros::Duration &period)
virtual void eStopActive(const bool active)
Set the emergency stop state.
hardware_interface::JointStateInterface js_interface_
hardware_interface::EffortJointInterface ej_interface_
std::vector< gazebo::physics::JointPtr > sim_joints_
virtual void writeSim(ros::Time time, ros::Duration period)
Write commands to the simulated robot hardware.
std::vector< ControlMethod > joint_control_methods_
bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits &soft_limits)
#define ROS_FATAL_STREAM_NAMED(name, args)
void registerJointLimits(const std::string &joint_name, const hardware_interface::JointHandle &joint_handle, const ControlMethod ctrl_method, const ros::NodeHandle &joint_limit_nh, const urdf::Model *const urdf_model, int *const joint_type, double *const lower_limit, double *const upper_limit, double *const effort_limit)
#define ROS_WARN_STREAM(args)
void registerHandle(const JointStateHandle &handle)
std::vector< double > last_joint_position_command_
def shortest_angular_distance_with_limits(from_angle, to_angle, left_limit, right_limit)
JointStateHandle getHandle(const std::string &name)
virtual bool initSim(const std::string &robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model, const urdf::Model *const urdf_model, std::vector< transmission_interface::TransmissionInfo > transmissions)
Initialize the simulated robot hardware.
Gazebo plugin version of RobotHW.
Definition: robot_hw_sim.h:68
bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits &limits)
joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_
Plugin template for hardware interfaces for ros_control and Gazebo.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
def shortest_angular_distance(from_angle, to_angle)
virtual bool init(ros::NodeHandle &, ros::NodeHandle &)
#define ROS_WARN_STREAM_NAMED(name, args)
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_


gazebo_ros_control
Author(s): Jonathan Bohren, Dave Coleman
autogenerated on Wed Aug 24 2022 02:48:00