sim_hw_interface.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman
00036    Desc:   Example ros_control hardware interface that performs a perfect control loop for
00037    simulation
00038 */
00039 
00040 #include <ros_control_boilerplate/sim_hw_interface.h>
00041 
00042 namespace ros_control_boilerplate
00043 {
00044 SimHWInterface::SimHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model)
00045   : GenericHWInterface(nh, urdf_model)
00046   , name_("sim_hw_interface")
00047 {
00048 }
00049 
00050 void SimHWInterface::init()
00051 {
00052   // Call parent class version of this function
00053   GenericHWInterface::init();
00054 
00055   // Resize vectors
00056   joint_position_prev_.resize(num_joints_, 0.0);
00057 
00058   ROS_INFO_NAMED(name_, "SimHWInterface Ready.");
00059 }
00060 
00061 void SimHWInterface::read(ros::Duration &elapsed_time)
00062 {
00063   // No need to read since our write() command populates our state for us
00064 }
00065 
00066 void SimHWInterface::write(ros::Duration &elapsed_time)
00067 {
00068   // Safety
00069   enforceLimits(elapsed_time);
00070 
00071   // Send commands in different modes
00072   int joint_mode = 0;  // TODO implement mode switching
00073 
00074   // NOTE: the following is a "simuation" example so that this boilerplate can be run without being
00075   // connected to hardware
00076   // When converting to your robot, remove the built-in PID loop and instead let the higher leverl
00077   // ros_control controllers take
00078   // care of PID loops for you. This P-controller is only intended to mimic the delay in real
00079   // hardware, somewhat like a simualator
00080   for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
00081   {
00082     switch (joint_mode)
00083     {
00084       case 0:  // hardware_interface::MODE_POSITION:
00085         positionControlSimulation(elapsed_time, joint_id);
00086         break;
00087 
00088       case 1:  // hardware_interface::MODE_VELOCITY:
00089         ROS_ERROR_STREAM_NAMED(name_, "Velocity not implemented yet");
00090 
00091         /*
00092         TODO: remove VELOCITY_STEP_FACTOR
00093 
00094         // Position - Move all the states to the commanded set points slowly
00095         joint_position_[joint_id] += joint_velocity_[joint_id] * elapsed_time.toSec();
00096 
00097         // Velocity - Move all the states to the commanded set points slowly
00098         v_error_ = joint_velocity_command_[joint_id] - joint_velocity_[joint_id];
00099         // scale the rate it takes to achieve velocity by a factor that is invariant to the feedback
00100         // loop
00101         joint_velocity_[joint_id] += v_error_ * VELOCITY_STEP_FACTOR;
00102         */
00103         break;
00104 
00105       case 2:  // hardware_interface::MODE_EFFORT:
00106         ROS_ERROR_STREAM_NAMED(name_, "Effort not implemented yet");
00107         break;
00108     }
00109   }
00110 }
00111 
00112 void SimHWInterface::enforceLimits(ros::Duration &period)
00113 {
00114   // Enforces position and velocity
00115   pos_jnt_sat_interface_.enforceLimits(period);
00116 }
00117 
00118 void SimHWInterface::positionControlSimulation(ros::Duration &elapsed_time, const std::size_t joint_id)
00119 {
00120   const double max_delta_pos = joint_velocity_limits_[joint_id] * elapsed_time.toSec();
00121 
00122   // Move all the states to the commanded set points at max velocity
00123   p_error_ = joint_position_command_[joint_id] - joint_position_[joint_id];
00124 
00125   const double delta_pos = std::max(std::min(p_error_, max_delta_pos), -max_delta_pos);
00126   joint_position_[joint_id] += delta_pos;
00127 
00128   // Bypass max velocity p controller:
00129   //joint_position_[joint_id] = joint_position_command_[joint_id];
00130 
00131   // Calculate velocity based on change in positions
00132   if (elapsed_time.toSec() > 0)
00133   {
00134     joint_velocity_[joint_id] = (joint_position_[joint_id] - joint_position_prev_[joint_id]) / elapsed_time.toSec();
00135   }
00136   else
00137     joint_velocity_[joint_id] = 0;
00138 
00139   // Save last position
00140   joint_position_prev_[joint_id] = joint_position_[joint_id];
00141 }
00142 
00143 }  // namespace


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Sun Apr 24 2016 04:39:29