tolerances.h
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2013, PAL Robotics S.L.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of PAL Robotics S.L. nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 
00029 
00030 #ifndef JOINT_TRAJECTORY_CONTROLLER_TOLERANCES_H
00031 #define JOINT_TRAJECTORY_CONTROLLER_TOLERANCES_H
00032 
00033 // C++ standard
00034 #include <cassert>
00035 #include <cmath>
00036 #include <string>
00037 #include <vector>
00038 
00039 // ROS
00040 #include <ros/node_handle.h>
00041 
00042 // ROS messages
00043 #include <control_msgs/FollowJointTrajectoryAction.h>
00044 
00045 namespace joint_trajectory_controller
00046 {
00047 
00053 template<class Scalar>
00054 struct StateTolerances
00055 {
00056   StateTolerances(Scalar position_tolerance     = static_cast<Scalar>(0.0),
00057                   Scalar velocity_tolerance     = static_cast<Scalar>(0.0),
00058                   Scalar acceleration_tolerance = static_cast<Scalar>(0.0))
00059     : position(position_tolerance),
00060       velocity(velocity_tolerance),
00061       acceleration(acceleration_tolerance)
00062   {}
00063 
00064   Scalar position;
00065   Scalar velocity;
00066   Scalar acceleration;
00067 };
00068 
00072 template<class Scalar>
00073 struct SegmentTolerances
00074 {
00075   SegmentTolerances(const typename std::vector<StateTolerances<Scalar> >::size_type& size = 0)
00076     : state_tolerance(size, static_cast<Scalar>(0.0)),
00077       goal_state_tolerance(size, static_cast<Scalar>(0.0)),
00078       goal_time_tolerance(static_cast<Scalar>(0.0))
00079   {}
00080 
00082   std::vector<StateTolerances<Scalar> > state_tolerance;
00083 
00085   std::vector<StateTolerances<Scalar> > goal_state_tolerance;
00086 
00088   Scalar goal_time_tolerance;
00089 };
00090 
00097 template <class State>
00098 inline bool checkStateTolerance(const State&                                                 state_error,
00099                                 const std::vector<StateTolerances<typename State::Scalar> >& state_tolerance,
00100                                 bool show_errors = false)
00101 {
00102   const unsigned int n_joints = state_tolerance.size();
00103 
00104   // Preconditions
00105   assert(n_joints == state_error.position.size());
00106   assert(n_joints == state_error.velocity.size());
00107   assert(n_joints == state_error.acceleration.size());
00108 
00109   for (unsigned int i = 0; i < n_joints; ++i)
00110   {
00111     using std::abs;
00112     const StateTolerances<typename State::Scalar>& tol = state_tolerance[i]; // Alias for brevity
00113     const bool is_valid = !(tol.position     > 0.0 && abs(state_error.position[i])     > tol.position) &&
00114                           !(tol.velocity     > 0.0 && abs(state_error.velocity[i])     > tol.velocity) &&
00115                           !(tol.acceleration > 0.0 && abs(state_error.acceleration[i]) > tol.acceleration);
00116 
00117     if (!is_valid)
00118     {
00119       if( show_errors )
00120       {
00121         ROS_ERROR_STREAM_NAMED("tolerances","Path state tolerances failed on joint " << i);
00122 
00123         if (tol.position     > 0.0 && abs(state_error.position[i])     > tol.position)
00124           ROS_ERROR_STREAM_NAMED("tolerances","Position Error: " << state_error.position[i] <<
00125             " Position Tolerance: " << tol.position);
00126         if (tol.velocity     > 0.0 && abs(state_error.velocity[i])     > tol.velocity)
00127           ROS_ERROR_STREAM_NAMED("tolerances","Velocity Error: " << state_error.velocity[i] <<
00128             " Velocity Tolerance: " << tol.velocity);
00129         if (tol.acceleration > 0.0 && abs(state_error.acceleration[i]) > tol.acceleration)
00130           ROS_ERROR_STREAM_NAMED("tolerances","Acceleration Error: " << state_error.acceleration[i] <<
00131             " Acceleration Tolerance: " << tol.acceleration);
00132       }
00133       return false;
00134     }
00135   }
00136   return true;
00137 }
00138 
00150 template<class Scalar>
00151 void updateStateTolerances(const control_msgs::JointTolerance& tol_msg, StateTolerances<Scalar>& tols)
00152 {
00153   if      (tol_msg.position     > 0.0) {tols.position     = static_cast<Scalar>(tol_msg.position);}
00154   else if (tol_msg.position     < 0.0) {tols.position     = static_cast<Scalar>(0.0);}
00155 
00156   if      (tol_msg.velocity     > 0.0) {tols.velocity     = static_cast<Scalar>(tol_msg.velocity);}
00157   else if (tol_msg.velocity     < 0.0) {tols.velocity     = static_cast<Scalar>(0.0);}
00158 
00159   if      (tol_msg.acceleration > 0.0) {tols.acceleration = static_cast<Scalar>(tol_msg.acceleration);}
00160   else if (tol_msg.acceleration < 0.0) {tols.acceleration = static_cast<Scalar>(0.0);}
00161 }
00162 
00170 template<class Scalar>
00171 void updateSegmentTolerances(const control_msgs::FollowJointTrajectoryGoal& goal,
00172                              const std::vector<std::string>&                joint_names,
00173                              SegmentTolerances<Scalar>&                     tols
00174 )
00175 {
00176   // Preconditions
00177   assert(joint_names.size() == tols.state_tolerance.size());
00178   assert(joint_names.size() == tols.goal_state_tolerance.size());
00179 
00180   typedef typename std::vector<std::string>::const_iterator                  StringConstIterator;
00181   typedef typename std::vector<control_msgs::JointTolerance>::const_iterator TolMsgConstIterator;
00182 
00183   for (StringConstIterator names_it = joint_names.begin(); names_it != joint_names.end(); ++names_it)
00184   {
00185     const typename std::vector<std::string>::size_type id = std::distance(joint_names.begin(), names_it);
00186 
00187     // Update path tolerances
00188     const std::vector<control_msgs::JointTolerance>& state_tol = goal.path_tolerance;
00189     for(TolMsgConstIterator state_tol_it = state_tol.begin(); state_tol_it != state_tol.end(); ++state_tol_it)
00190     {
00191       if (*names_it == state_tol_it->name) {updateStateTolerances(*state_tol_it, tols.state_tolerance[id]);}
00192     }
00193 
00194     // Update goal state tolerances
00195     const std::vector<control_msgs::JointTolerance>& g_state_tol = goal.goal_tolerance;
00196     for(TolMsgConstIterator g_state_tol_it = g_state_tol.begin(); g_state_tol_it != g_state_tol.end(); ++g_state_tol_it)
00197     {
00198       if (*names_it == g_state_tol_it->name) {updateStateTolerances(*g_state_tol_it, tols.goal_state_tolerance[id]);}
00199     }
00200   }
00201 
00202   // Update goal time tolerance
00203   const ros::Duration& goal_time_tolerance = goal.goal_time_tolerance;
00204   if      (goal_time_tolerance < ros::Duration(0.0)) {tols.goal_time_tolerance = 0.0;}
00205   else if (goal_time_tolerance > ros::Duration(0.0)) {tols.goal_time_tolerance = goal_time_tolerance.toSec();}
00206 }
00207 
00229 template<class Scalar>
00230 SegmentTolerances<Scalar> getSegmentTolerances(const ros::NodeHandle& nh,
00231                                                const std::vector<std::string>& joint_names)
00232 {
00233   const unsigned int n_joints = joint_names.size();
00234   joint_trajectory_controller::SegmentTolerances<Scalar> tolerances;
00235 
00236   // State and goal state tolerances
00237   double stopped_velocity_tolerance;
00238   nh.param("stopped_velocity_tolerance", stopped_velocity_tolerance, 0.01);
00239 
00240   tolerances.state_tolerance.resize(n_joints);
00241   tolerances.goal_state_tolerance.resize(n_joints);
00242   for (unsigned int i = 0; i < n_joints; ++i)
00243   {
00244     nh.param(joint_names[i] + "/trajectory", tolerances.state_tolerance[i].position,      0.0);
00245     nh.param(joint_names[i] + "/goal",       tolerances.goal_state_tolerance[i].position, 0.0);
00246     tolerances.goal_state_tolerance[i].velocity = stopped_velocity_tolerance;
00247   }
00248 
00249   // Goal time tolerance
00250   nh.param("goal_time", tolerances.goal_time_tolerance, 0.0);
00251 
00252   return tolerances;
00253 }
00254 
00255 } // namespace
00256 
00257 #endif // header guard


joint_trajectory_controller
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Sat Aug 13 2016 04:20:51