00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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
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
00103
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 }
00233 }