sim_hw_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, University of Colorado, Boulder
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 
35 /* Author: Dave Coleman
36  Desc: Example ros_control hardware interface that performs a perfect control loop for
37  simulation
38 */
39 
41 
42 // ROS parameter loading
44 
46 {
48  : GenericHWInterface(nh, urdf_model)
49  , name_("sim_hw_interface")
50 {
51  // Load rosparams
52  ros::NodeHandle rpnh(nh_, "hardware_interface");
53  std::size_t error = 0;
54  error += !rosparam_shortcuts::get(name_, rpnh, "sim_control_mode", sim_control_mode_);
55  if (error)
56  {
57  ROS_WARN_STREAM_NAMED(name_, "SimHWInterface now requires the following config in the yaml:");
58  ROS_WARN_STREAM_NAMED(name_, " sim_control_mode: 0 # 0: position, 1: velocity");
59  }
61 }
62 
64 {
65  // Call parent class version of this function
67 
68  // Resize vectors
69  joint_position_prev_.resize(num_joints_, 0.0);
70 
71  ROS_INFO_NAMED(name_, "SimHWInterface Ready.");
72 }
73 
75 {
76  // No need to read since our write() command populates our state for us
77 }
78 
80 {
81  // Safety
82  enforceLimits(elapsed_time);
83 
84  // NOTE: the following is a "simuation" example so that this boilerplate can be run without being
85  // connected to hardware
86  // When converting to your robot, remove the built-in PID loop and instead let the higher leverl
87  // ros_control controllers take
88  // care of PID loops for you. This P-controller is only intended to mimic the delay in real
89  // hardware, somewhat like a simualator
90  for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
91  {
92  switch (sim_control_mode_)
93  {
94  case 0: // hardware_interface::MODE_POSITION:
95  positionControlSimulation(elapsed_time, joint_id);
96  break;
97 
98  case 1: // hardware_interface::MODE_VELOCITY:
99 
100  // // Move all the states to the commanded set points slowly
101  // joint_position_[joint_id] += joint_velocity_[joint_id] * elapsed_time.toSec();
102 
103  // v_error_ = joint_velocity_command_[joint_id] - joint_velocity_[joint_id];
104 
105  // // scale the rate it takes to achieve velocity by a factor that is invariant to the feedback loop
106  // joint_velocity_[joint_id] += v_error_ * VELOCITY_STEP_FACTOR;
107 
108  // Naive
109  joint_velocity_[joint_id] = joint_velocity_command_[joint_id];
110  joint_position_[joint_id] += joint_velocity_command_[joint_id] * elapsed_time.toSec();
111 
112  break;
113 
114  case 2: // hardware_interface::MODE_EFFORT:
115  ROS_ERROR_STREAM_NAMED(name_, "Effort not implemented yet");
116  break;
117  }
118  }
119 }
120 
122 {
123  // Enforces position and velocity
125 }
126 
127 void SimHWInterface::positionControlSimulation(ros::Duration &elapsed_time, const std::size_t joint_id)
128 {
129  const double max_delta_pos = joint_velocity_limits_[joint_id] * elapsed_time.toSec();
130 
131  // Move all the states to the commanded set points at max velocity
132  p_error_ = joint_position_command_[joint_id] - joint_position_[joint_id];
133 
134  const double delta_pos = std::max(std::min(p_error_, max_delta_pos), -max_delta_pos);
135  joint_position_[joint_id] += delta_pos;
136 
137  // Bypass max velocity p controller:
138  //joint_position_[joint_id] = joint_position_command_[joint_id];
139 
140  // Calculate velocity based on change in positions
141  if (elapsed_time.toSec() > 0)
142  {
143  joint_velocity_[joint_id] = (joint_position_[joint_id] - joint_position_prev_[joint_id]) / elapsed_time.toSec();
144  }
145  else
146  joint_velocity_[joint_id] = 0;
147 
148  // Save last position
149  joint_position_prev_[joint_id] = joint_position_[joint_id];
150 }
151 
152 } // namespace
#define ROS_INFO_NAMED(name,...)
#define ROS_ERROR_STREAM_NAMED(name, args)
virtual void init()
Initialize the robot hardware interface.
void enforceLimits(const ros::Duration &period)
virtual void init()
Initialize the hardware interface.
virtual void read(ros::Duration &elapsed_time)
Read the state from the robot hardware.
SimHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model=NULL)
Constructor.
virtual void enforceLimits(ros::Duration &period)
void shutdownIfError(const std::string &parent_name, std::size_t error_count)
virtual void write(ros::Duration &elapsed_time)
Write the command to the robot hardware.
virtual void positionControlSimulation(ros::Duration &elapsed_time, const std::size_t joint_id)
Basic model of system for position control.
bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, bool &value)
joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_
#define ROS_WARN_STREAM_NAMED(name, args)


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Thu Feb 25 2021 03:58:54