JointLimits.cpp
Go to the documentation of this file.
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 }


ipa_canopen_ros
Author(s): Tobias Sing, Thiago de Freitas
autogenerated on Mon Oct 6 2014 00:59:31