motoman: motoman: utils.cpp Source 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 "utils.h"
00033 #include "definitions.h"
00034 #include "ros/ros.h"
00035 #include "urdf/model.h"
00036 
00037 using namespace trajectory_msgs;
00038 using namespace std;
00039 
00040 namespace motoman
00041 {
00042 namespace utils
00043 {
00044 
00045   bool checkTrajectory(JointTrajectoryConstPtr trajectory)
00046   {
00047     bool rtn = false;
00048     rtn = checkJointNames(trajectory->joint_names);
00049     if (!rtn)
00050     {
00051       ROS_WARN("Joint trajectory name check failed");
00052     }
00053     return rtn;
00054   }
00055 
00056 
00057   bool checkJointNames(JointTrajectoryConstPtr trajectory)
00058   {
00059     bool rtn = false;
00060 
00061     // The maximum number of joints in the motoman controller is fixed
00062     if (trajectory->joint_names.size() <= motoman::parameters::Parameters::JOINT_SUFFIXES_SIZE)
00063     {
00064       rtn = true;
00065     }
00066     else
00067     {
00068       rtn = false;
00069       ROS_WARN("Size of joint names: %d, exceeds motoman size: %d",
00070                trajectory->joint_names.size(),
00071                motoman::parameters::Parameters::JOINT_SUFFIXES_SIZE);
00072     }
00073 
00074     if (rtn)
00075     {
00076       for(int i = 0; i<trajectory->joint_names.size(); i++)
00077       {
00078         string suffix(motoman::parameters::Parameters::JOINT_SUFFIXES[i]);
00079         if ( hasSuffix(trajectory->joint_names[i], suffix ) )
00080         {
00081           rtn = true;
00082         }
00083         else
00084         {
00085           rtn = false;
00086           break;
00087         }
00088 
00089       }
00090     }
00091 
00092     return rtn;
00093   }
00094 
00095 
00096   bool hasSuffix(string &str, string &suffix)
00097   {
00098     bool rtn = false;
00099     int result;
00100 
00101     // If an empty suffix is passed to the function it returns true (which is
00102     // to be expected).  This warning as a little help when this occurs.
00103     if (0 == suffix.size() )
00104     {
00105       ROS_WARN("Suffix of size zero passed to hasSuffix, continuing");
00106     }
00107     result = str.rfind(suffix);
00108 
00109     ROS_DEBUG("Checking string: %s for suffix: %s, result %d", str.c_str(), suffix.c_str(), result);
00110     if ( result == str.size()-suffix.size())
00111     {
00112       rtn = true;
00113       ROS_DEBUG("%s has the suffix: %s", str.c_str(), suffix.c_str());
00114     }
00115     else
00116     {
00117       rtn = false;
00118       ROS_DEBUG("%s does not have the suffix: %s", str.c_str(), suffix.c_str());
00119     }
00120 
00121     return rtn;
00122   }
00123 
00124 
00125   bool getVelocityLimits(std::string param_name,
00126                          JointTrajectoryConstPtr trajectory,
00127                          std::vector<double> &joint_velocity_limits)
00128   {
00129     bool rtn = false;
00130     string urdf;
00131 
00132     joint_velocity_limits.clear();
00133     joint_velocity_limits.resize(trajectory->joint_names.size(), 0.0);
00134 
00135     if (ros::param::get(param_name, urdf))
00136     {
00137       urdf::Model urdf_model;
00138       if (urdf_model.initString(urdf))
00139       {
00140 
00141         for(int i = 0; i<joint_names.size(); i++)
00142         {
00143           double limit;
00144           boost::shared_ptr<const urdf::Joint> joint = urdf_model.getJoint(trajectory->joint_names[i]);
00145           limit = joint->limits->velocity;
00146           ROS_DEBUG("Found joint velocity limit: %e for joint: %s", limit, trajectory->joint_names[i].c_str());
00147         }
00148       }
00149       else
00150       {
00151         ROS_ERROR("Failed to parse urdf xml string");
00152       }
00153     }
00154     else
00155     {
00156       ROS_ERROR("Failed to get urdf from parameter: %s", param_name.c_str());
00157       rtn = false;
00158     }
00159 
00160     if (!rtn)
00161     {
00162       ROS_ERROR("Failed to get velocity limits for parameter: %s", param_name.c_str());
00163     }
00164 
00165     return rtn;
00166 
00167 
00168 
00169   }
00170 
00171 
00172   double toMotomanVelocity(std::vector<double> &joint_velocity_limits,
00173                            std::vector<double> &joint_velocities)
00174   {
00175     double maxVelPct = 0.0;
00176 
00177     if (joint_velocity_limits.size() == joint_velocities.size())
00178     {
00179       for(int i = 0; i<joint_velocity_limits.size(); i++)
00180       {
00181         if (joint_velocity_limits[i] > 0.0 && joint_velocities[i] > 0.0)
00182         {
00183           double velPct = joint_velocities[i]/joint_velocity_limits[i];
00184 
00185           ROS_DEBUG("Calculating velocity percent, velocity: %e, limit: %e, percetn: %e",
00186                     joint_velocities[i], joint_velocity_limits[i], velPct);
00187           if (velPct > maxVelPct)
00188           {
00189             ROS_DEBUG("Calculated velocity: %e, greater than current max: %e",
00190                       velPct, maxVelPct);
00191             maxVelPct = velPct;
00192           }
00193         }
00194         else
00195         {
00196           ROS_ERROR("Invalid joint velocity: %e or limit: %e", joint_velocities[i],
00197                     joint_velocity_limits[i]);
00198         }
00199       }
00200     }
00201     else
00202     {
00203       ROS_ERROR("Failed to calculate a velocity (joint velocity and limit vectors different size");
00204     }
00205 
00206 
00207     if (maxVelPct > 1.0)
00208     {
00209       ROS_WARN("Max velocity percent: %e, exceed 1.0, setting to 1.0", maxVelPct);
00210       maxVelPct = 1.0;
00211     }
00212     else if (maxVelPct < 0.0)
00213     {
00214       ROS_WARN("Max velocity percent: %e, is less than 0.0, setting to 0.0", maxVelPct);
00215       maxVelPct = 0.0;
00216     }
00217 
00218     ROS_DEBUG("Returning a motoman velocity of: %e", maxVelPct);
00219 
00220     return maxVelPct;
00221   }
00222 
00223 
00224 } //utils
00225 } //motoman