utils.cpp
Go to the documentation of this file.
00001 /*
00002 * Software License Agreement (BSD License) 
00003 *
00004 * Copyright (c) 2011, Yaskawa America, Inc.
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 are met:
00009 *
00010 *       * Redistributions of source code must retain the above copyright
00011 *       notice, this list of conditions and the following disclaimer.
00012 *       * Redistributions in binary form must reproduce the above copyright
00013 *       notice, this list of conditions and the following disclaimer in the
00014 *       documentation and/or other materials provided with the distribution.
00015 *       * Neither the name of the Yaskawa America, Inc., nor the names 
00016 *       of its contributors may be used to endorse or promote products derived
00017 *       from this software without specific prior written permission.
00018 *
00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 * POSSIBILITY OF SUCH DAMAGE.
00030 */ 
00031 
00032 #include <dx100/utils.h>
00033 #include <dx100/definitions.h>
00034 #include "ros/ros.h"
00035 #include "urdf/model.h"
00036 #include "math.h"
00037 
00038 using namespace trajectory_msgs;
00039 using namespace std;
00040 
00041 namespace motoman
00042 {
00043 namespace utils
00044 {
00045 
00046 bool checkTrajectory(const trajectory_msgs::JointTrajectoryConstPtr& trajectory)
00047   {
00048     bool rtn = false;
00049     rtn = checkJointNames(trajectory);
00050     if (!rtn)
00051     {
00052       ROS_WARN("Joint trajectory name check failed");
00053     }
00054     return rtn;
00055   }
00056 
00057 
00058   bool checkJointNames(const JointTrajectoryConstPtr& trajectory)
00059   {
00060     bool rtn = false;
00061 
00062     // The maximum number of joints in the motoman controller is fixed
00063     if ((int)trajectory->joint_names.size() <= motoman::parameters::Parameters::JOINT_SUFFIXES_SIZE)
00064     {
00065       rtn = true;
00066     }
00067     else
00068     {
00069       rtn = false;
00070       ROS_WARN("Size of joint names: %zd, exceeds motoman size: %d",
00071                trajectory->joint_names.size(),
00072                motoman::parameters::Parameters::JOINT_SUFFIXES_SIZE);
00073     }
00074 
00075     if (rtn)
00076     {
00077       for(unsigned int i = 0; i<trajectory->joint_names.size(); i++)
00078       {
00079         string suffix(motoman::parameters::Parameters::JOINT_SUFFIXES[i]);
00080         if ( hasSuffix(trajectory->joint_names[i], suffix ) )
00081         {
00082           rtn = true;
00083         }
00084         else
00085         {
00086           rtn = false;
00087           break;
00088         }
00089 
00090       }
00091     }
00092 
00093     return rtn;
00094   }
00095 
00096 
00097   bool hasSuffix(const string &str, const string &suffix)
00098   {
00099     bool rtn = false;
00100     int result;
00101 
00102     // If an empty suffix is passed to the function it returns true (which is
00103     // to be expected).  This warning as a little help when this occurs.
00104     if (0 == suffix.size() )
00105     {
00106       ROS_WARN("Suffix of size zero passed to hasSuffix, continuing");
00107     }
00108     result = str.rfind(suffix);
00109 
00110     ROS_DEBUG("Checking string: %s for suffix: %s, result %d", str.c_str(), suffix.c_str(), result);
00111     if ( result == str.size()-suffix.size())
00112     {
00113       rtn = true;
00114       ROS_DEBUG("%s has the suffix: %s", str.c_str(), suffix.c_str());
00115     }
00116     else
00117     {
00118       rtn = false;
00119       ROS_DEBUG("%s does not have the suffix: %s", str.c_str(), suffix.c_str());
00120     }
00121 
00122     return rtn;
00123   }
00124 
00125 
00126   bool getVelocityLimits(std::string param_name,
00127                          JointTrajectoryConstPtr trajectory,
00128                          std::vector<double> &joint_velocity_limits)
00129   {
00130     bool rtn = false;
00131     string urdf;
00132 
00133     joint_velocity_limits.clear();
00134     joint_velocity_limits.resize(trajectory->joint_names.size(), 0.0);
00135 
00136     if (ros::param::get(param_name, urdf))
00137     {
00138       urdf::Model urdf_model;
00139       if (urdf_model.initString(urdf))
00140       {
00141 
00142         for(unsigned int i = 0; i<trajectory->joint_names.size(); i++)
00143         {
00144           double limit;
00145           boost::shared_ptr<const urdf::Joint> joint = urdf_model.getJoint(trajectory->joint_names[i]);
00146           limit = joint->limits->velocity;
00147           joint_velocity_limits[i] = limit;
00148           ROS_DEBUG("Found joint velocity limit: %e for joint: %s", 
00149                     joint_velocity_limits[i], trajectory->joint_names[i].c_str());
00150         }
00151         ROS_DEBUG("Successefully populated velocity limits, size: %d", joint_velocity_limits.size());
00152         rtn = true;
00153       }
00154       else
00155       {
00156         ROS_ERROR("Failed to parse urdf xml string");
00157       }
00158     }
00159     else
00160     {
00161       ROS_ERROR("Failed to get urdf from parameter: %s", param_name.c_str());
00162       rtn = false;
00163     }
00164 
00165     if (!rtn)
00166     {
00167       ROS_ERROR("Failed to get velocity limits for parameter: %s", param_name.c_str());
00168     }
00169 
00170     return rtn;
00171 
00172 
00173 
00174   }
00175 
00176 
00177   double toMotomanVelocity(std::vector<double> &joint_velocity_limits,
00178                            std::vector<double> &joint_velocities)
00179   {
00180     double maxVelPct = 0.0;
00181 
00182     ROS_DEBUG("Converting to motoman velocity, limit size: %d, velocity size: %d",
00183               joint_velocity_limits.size(), joint_velocities.size());
00184     if (joint_velocity_limits.size() == joint_velocities.size())
00185     {
00186       for(int i = 0; i<joint_velocity_limits.size(); i++)
00187       {
00188         if (joint_velocity_limits[i] > 0.0)
00189         {
00190           ROS_DEBUG("Calculating velocity percent");
00191           double velPct = joint_velocities[i]/joint_velocity_limits[i];
00192 
00193           ROS_DEBUG("Calculating velocity percent, velocity: %e, limit: %e, percent: %e",
00194                     joint_velocities[i], joint_velocity_limits[i], velPct);
00195           if (abs(velPct) > maxVelPct)
00196           {
00197             ROS_DEBUG("Calculated velocity: %e, greater than current max: %e",
00198                       velPct, maxVelPct);
00199             maxVelPct = abs(velPct);
00200           }
00201         }
00202         else
00203         {
00204           ROS_ERROR("Invalid joint velocity: %e or limit: %e", joint_velocities[i],
00205                     joint_velocity_limits[i]);
00206         }
00207       }
00208     }
00209     else
00210     {
00211       ROS_ERROR("Failed to calculate a velocity (joint velocity and limit vectors different size");
00212     }
00213 
00214 
00215     if (maxVelPct > 1.0)
00216     {
00217       ROS_WARN("Max velocity percent: %e, exceed 1.0, setting to 1.0", maxVelPct);
00218       maxVelPct = 1.0;
00219     }
00220     else if (maxVelPct < 0.0)
00221     {
00222       ROS_WARN("Max velocity percent: %e, is less than 0.0, setting to 0.0", maxVelPct);
00223       maxVelPct = 0.0;
00224     }
00225 
00226     ROS_DEBUG("Returning a motoman velocity of: %e", maxVelPct);
00227 
00228     return maxVelPct;
00229   }
00230 
00231 
00232 } //utils
00233 } //motoman


dx100
Author(s): Shaun Edwards (Southwest Research Institute)
autogenerated on Thu Jan 2 2014 11:29:36