trajectory_stats.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, 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
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *
00035  *********************************************************************/
00036 
00037 /* \author: Ken Anderson */
00038 
00039 #include <cmath>
00040 #include <move_arm_warehouse/move_arm_utils.h>
00041 #include <move_arm_warehouse/trajectory_stats.h>
00042 
00043 using namespace std;
00044 using namespace planning_scene_utils;
00045 using namespace planning_models;
00046 
00047 ros::Duration TrajectoryStats::getExecutionDuration()
00048 {
00049   size_t tsize = trajectory_.points.size();
00050   if( tsize < 1 )
00051   {
00052     return ros::Duration(0,0);
00053   }
00054 
00055   trajectory_msgs::JointTrajectoryPoint& point1 = trajectory_.points[0];
00056   trajectory_msgs::JointTrajectoryPoint& point2 = trajectory_.points[tsize-1];
00057   return point2.time_from_start - point1.time_from_start;
00058 }
00059 
00060 double TrajectoryStats::getAngularDistance()
00061 {
00062   double angular_diff_sum = 0.0;
00063   size_t tsize = trajectory_.points.size();
00064 
00065   // Loop through trajectory points
00066   for(unsigned int i=1; i<tsize; i++)
00067   {
00068     trajectory_msgs::JointTrajectoryPoint point1 = trajectory_.points[i-1];
00069     trajectory_msgs::JointTrajectoryPoint point2 = trajectory_.points[i];
00070 
00071     if(point1.positions.size() != point2.positions.size())
00072     {
00073       ROS_ERROR("Invalid Trajectory, the number of joints is inconsistent");
00074       return 0.0;
00075     }
00076 
00077     // Loop through all joints
00078     size_t num_pos = point1.positions.size();
00079     for(unsigned int j=0; j<num_pos; j++)
00080     {
00081       double angular_diff = point1.positions[j] - point2.positions[j];
00082       angular_diff_sum += abs(angular_diff);
00083     }
00084   }
00085 
00086   return angular_diff_sum;
00087 }
00088 
00089 double TrajectoryStats::getCartesianDistance(planning_scene_utils::MotionPlanRequestData& motion_plan_req)
00090 {
00091   double cartesian_distance_sum = 0.0;
00092   planning_models::KinematicState* kin_state = motion_plan_req.getStartState();
00093   KinematicState::JointStateGroup* joint_state_group = kin_state->getJointStateGroup(motion_plan_req.getGroupName());
00094 
00095   // Loop through trajectory points
00096   size_t tsize = trajectory_.points.size();
00097   for(unsigned int i=1; i<tsize; i++)
00098   {
00099     // Get position of end-effector at the first point of the trajectory.
00100     trajectory_msgs::JointTrajectoryPoint point1 = trajectory_.points[i-1];
00101     if( !joint_state_group->setKinematicState(point1.positions) )
00102     {
00103       ROS_ERROR("Mismatch in number of joints.");
00104       return 0.0;
00105     }
00106     joint_state_group->updateKinematicLinks();
00107     KinematicState::LinkState* end_effector_state1 = kin_state->getLinkState(motion_plan_req.getEndEffectorLink());
00108     const tf::Transform& end_effector_transform1 = end_effector_state1->getGlobalLinkTransform();
00109     const tf::Vector3 end_effector_location1 = end_effector_transform1.getOrigin();
00110 
00111     // Get position of end-effector at the next point of the trajectory.
00112     trajectory_msgs::JointTrajectoryPoint point2 = trajectory_.points[i];
00113     if( !joint_state_group->setKinematicState(point2.positions) )
00114     {
00115       ROS_ERROR("Mismatch in number of joints.");
00116       return 0.0;
00117     }
00118     joint_state_group->updateKinematicLinks();
00119     KinematicState::LinkState* end_effector_state2 = kin_state->getLinkState(motion_plan_req.getEndEffectorLink());
00120     const tf::Transform& end_effector_transform2 = end_effector_state2->getGlobalLinkTransform();
00121     const tf::Vector3 end_effector_location2 = end_effector_transform2.getOrigin();
00122 
00123     // Calculate
00124     double cartesian_distance = end_effector_location1.distance(end_effector_location2);
00125     cartesian_distance_sum += abs(cartesian_distance);
00126   }
00127 
00128   return cartesian_distance_sum;
00129 }
00130 
00131 /*
00132 double TrajectoryStats::getClearanceDistance(planning_scene_utils::PlanningSceneData& scene)
00133 {
00134   return 0;
00135 }
00136 */
00137 
00138 double TrajectoryStats::getMaxAngularError(trajectory_msgs::JointTrajectory& trajectory_error)
00139 {
00140   double max_error = 0.0;
00141 
00142   // Loop through trajectory points
00143   size_t tsize = trajectory_error.points.size();
00144   for(unsigned int i=0; i<tsize; i++)
00145   {
00146     trajectory_msgs::JointTrajectoryPoint& point = trajectory_error.points[i];
00147 
00148     // Loop through all joints
00149     size_t num_pos = point.positions.size();
00150     for(unsigned int j=0; j<num_pos; j++)
00151     {
00152       double error = abs(point.positions[j]);
00153       if(error>max_error)
00154       {
00155         max_error = error;
00156       }
00157     }
00158   }
00159 
00160   return max_error;
00161 }


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Fri Dec 6 2013 21:12:34