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
00037 #include <spline_smoother/linear_trajectory.h>
00038
00039 namespace spline_smoother
00040 {
00041
00042 LinearTrajectory::LinearTrajectory()
00043 {
00044 apply_limits_ = true;
00045 }
00046
00047 double LinearTrajectory::calculateMinimumTime(const trajectory_msgs::JointTrajectoryPoint &start,
00048 const trajectory_msgs::JointTrajectoryPoint &end,
00049 const std::vector<arm_navigation_msgs::JointLimits> &limits)
00050 {
00051 double minJointTime(MAX_ALLOWABLE_TIME);
00052 double segmentTime(0);
00053 int num_joints = (int) start.positions.size();
00054
00055 for(int i = 0; i < num_joints; i++)
00056 {
00057 double diff = jointDiff(start.positions[i],end.positions[i],limits[i]);
00058 minJointTime = fabs(diff) / limits[i].max_velocity;
00059 if(segmentTime < minJointTime)
00060 segmentTime = minJointTime;
00061 }
00062 return segmentTime;
00063 }
00064
00065 bool LinearTrajectory::parameterize(const trajectory_msgs::JointTrajectory& trajectory_in,
00066 const std::vector<arm_navigation_msgs::JointLimits> &limits,
00067 spline_smoother::SplineTrajectory& spline)
00068 {
00069 int num_traj = trajectory_in.points.size();
00070 int num_joints = trajectory_in.joint_names.size();
00071 spline.names = trajectory_in.joint_names;
00072 spline.segments.resize(num_traj-1);
00073
00074 for(int i=0; i<num_joints; i++)
00075 {
00076 if(!limits[i].has_velocity_limits)
00077 {
00078 ROS_ERROR("Trying to apply velocity limits without supplying them. Set velocity_limits in the message and set has_velocity_limits flag to true.");
00079 return false;
00080 }
00081 }
00082 for (int i=1; i< num_traj; ++i)
00083 {
00084 spline.segments[i-1].joints.resize(num_joints);
00085 for(int j=0; j < num_joints; j++)
00086 {
00087 spline.segments[i-1].joints[j].coefficients.resize(2);
00088 }
00089
00090 double dT = (trajectory_in.points[i].time_from_start - trajectory_in.points[i-1].time_from_start).toSec();
00091 if(apply_limits_)
00092 {
00093 double dTMin = calculateMinimumTime(trajectory_in.points[i-1],trajectory_in.points[i],limits);
00094 if(dTMin > dT)
00095 dT = dTMin;
00096 }
00097 spline.segments[i-1].duration = ros::Duration(dT);
00098 for(int j=0; j<num_joints; j++)
00099 {
00100 spline.segments[i-1].joints[j].coefficients[0] = trajectory_in.points[i-1].positions[j];
00101 spline.segments[i-1].joints[j].coefficients[1] = jointDiff(trajectory_in.points[i-1].positions[j],trajectory_in.points[i].positions[j],limits[j])/spline.segments[i-1].duration.toSec();
00102 }
00103 }
00104 return true;
00105 }
00106 }