00001 00060 #include <ros/ros.h> 00061 #include "ipa_canopen_ros/JointLimits.h" 00062 00063 00064 bool JointLimits::checkVelocityLimits(std::vector<double> velocities) 00065 { 00066 00067 for (int i = 0; i < JointLimits::getDOF(); i++) 00068 { 00069 if(abs(velocities[i]) > JointLimits::getMaxVelocities()[i]) 00070 { 00071 if(velocities[i] >= 0) 00072 // set velocities command to max value 00073 velocities[i] = JointLimits::getMaxVelocities()[i]; 00074 else 00075 velocities[i] = -JointLimits::getMaxVelocities()[i]; 00076 00077 ROS_INFO("Velocity %f exceeds limit %f for axis %i. moving with max velocity %f instead", velocities[i], JointLimits::getMaxVelocities()[i], i, JointLimits::getMaxVelocities()[i]); 00078 return true; 00079 } 00080 00081 } 00082 return false; 00083 } 00084 00085 00086 bool JointLimits::checkPositionLimits(std::vector<double> target_pos,std::vector<double> velocities) 00087 { 00088 00089 for (int i = 0; i < JointLimits::getDOF(); i++) 00090 { 00091 if ((target_pos[i] < JointLimits::getLowerLimits()[i]) && (velocities[i] < 0)) 00092 { 00093 ROS_INFO("Skipping command: %f Target position exceeds lower limit (%f).", target_pos[i], JointLimits::getLowerLimits()[i]); 00094 // target position is set to actual position and velocity to Null. So only movement in the non limit direction is possible. 00095 00096 return true; 00097 } 00098 00099 // if target position is outer limits and the command velocity is in in direction away from working range, skip command 00100 if ((target_pos[i] > JointLimits::getUpperLimits()[i]) && (velocities[i] > 0)) 00101 { 00102 ROS_INFO("Skipping command: %f Target position exceeds upper limit (%f).", target_pos[i], JointLimits::getUpperLimits()[i]); 00103 // target position is set to actual position. So only movement in the non limit direction is possible. 00104 00105 return true; 00106 } 00107 } 00108 00109 return false; 00110 }