Go to the documentation of this file.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
00033
00034
00035
00036
00037
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
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
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
00096 size_t tsize = trajectory_.points.size();
00097 for(unsigned int i=1; i<tsize; i++)
00098 {
00099
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
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
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
00133
00134
00135
00136
00137
00138 double TrajectoryStats::getMaxAngularError(trajectory_msgs::JointTrajectory& trajectory_error)
00139 {
00140 double max_error = 0.0;
00141
00142
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
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 }