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/lspb_trajectory.h>
00038
00039 namespace spline_smoother
00040 {
00041 LSPBTrajectory::LSPBTrajectory()
00042 {
00043 apply_limits_ = true;
00044 }
00045
00046 double LSPBTrajectory::calculateMinimumTime(const trajectory_msgs::JointTrajectoryPoint &start,
00047 const trajectory_msgs::JointTrajectoryPoint &end,
00048 const std::vector<arm_navigation_msgs::JointLimits> &limits)
00049 {
00050 double minJointTime(MAX_ALLOWABLE_TIME);
00051 double segmentTime(0);
00052 int num_joints = (int) start.positions.size();
00053
00054 for(int i = 0; i < num_joints; i++)
00055 {
00056 minJointTime = minSegmentTime(start.positions[i],end.positions[i],start.velocities[i],end.velocities[i],limits[i]);
00057 if(segmentTime < minJointTime)
00058 segmentTime = minJointTime;
00059 }
00060 return segmentTime;
00061 }
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085 double LSPBTrajectory::minSegmentTime(const double &q0,
00086 const double &q1,
00087 const double &v0,
00088 const double &v1,
00089 const arm_navigation_msgs::JointLimits &limit)
00090 {
00091 double vmax = limit.max_velocity;
00092 double amax = limit.max_acceleration;
00093 double diff = jointDiff(q0,q1,limit);
00094
00095 double tb = fabs(vmax/amax);
00096
00097 double acc(0);
00098 if(diff>0)
00099 acc = amax;
00100 else
00101 acc = -amax;
00102 double dist_tb = acc*tb*tb;
00103 double ts = (diff - dist_tb)/(acc*tb);
00104 if(ts < 0)
00105 {
00106 ts = 0.0;
00107 }
00108 return (2*tb+ts);
00109 }
00110
00111
00112 double LSPBTrajectory::blendTime(const double &aa,
00113 const double &bb,
00114 const double &cc)
00115 {
00116 double disc = (pow(bb,2) - 4*aa*cc);
00117 if(disc < 0)
00118 {
00119 ROS_DEBUG("Blend time quadratic coeff: %f %f %f",aa,bb,cc);
00120 return 0.0;
00121 }
00122 double tb1 = (-bb + sqrt(disc))/(2*aa);
00123 double tb2 = (-bb - sqrt(disc))/(2*aa);
00124 if(std::isnan(tb1))
00125 tb1 = 0.0;
00126 if(std::isnan(tb2))
00127 tb2 = 0.0;
00128 return std::min(tb1,tb2);
00129 }
00130
00131
00132
00133
00134
00135
00136 bool LSPBTrajectory::parameterize(const trajectory_msgs::JointTrajectory& trajectory_in,
00137 const std::vector<arm_navigation_msgs::JointLimits>& limits,
00138 spline_smoother::LSPBTrajectoryMsg& spline)
00139 {
00140 int num_traj = trajectory_in.points.size();
00141 int num_joints = trajectory_in.joint_names.size();
00142 spline.names = trajectory_in.joint_names;
00143 spline.segments.resize(num_traj-1);
00144
00145 for(int i=0; i<num_joints; i++)
00146 {
00147 if(!limits[i].has_velocity_limits)
00148 {
00149 ROS_ERROR("Trying to apply velocity limits without supplying them. Set velocity_limits in the message and set has_velocity_limits flag to true.");
00150 return false;
00151 }
00152 if(!limits[i].has_acceleration_limits)
00153 {
00154 ROS_ERROR("Trying to apply acceleration limits without supplying them. Set acceleration_limits in the message and set has_acceleration_limits flag to true.");
00155 return false;
00156 }
00157 }
00158 for (int i=1; i< num_traj; ++i)
00159 {
00160 spline.segments[i-1].joints.resize(num_joints);
00161 for(unsigned int j=0; j < spline.segments[i-1].joints.size(); j++)
00162 spline.segments[i-1].joints[j].coefficients.resize(3);
00163
00164 double dT = (trajectory_in.points[i].time_from_start - trajectory_in.points[i-1].time_from_start).toSec();
00165 if(apply_limits_)
00166 {
00167 double dTMin = calculateMinimumTime(trajectory_in.points[i-1],trajectory_in.points[i],limits);
00168 if(dTMin > dT)
00169 dT = dTMin;
00170 }
00171 spline.segments[i-1].duration = ros::Duration(dT);
00172 for(int j=0; j<num_joints; j++)
00173 {
00174 double diff = jointDiff(trajectory_in.points[i-1].positions[j],trajectory_in.points[i].positions[j],limits[j]);
00175 double acc = 0.0;
00176 if(diff > 0)
00177 acc = limits[j].max_acceleration;
00178 else
00179 acc = - limits[j].max_acceleration;
00180 double tb = blendTime(acc,-acc*dT,diff);
00181
00182
00183 spline.segments[i-1].joints[j].coefficients[0] = trajectory_in.points[i-1].positions[j];
00184 spline.segments[i-1].joints[j].coefficients[1] = 0.0;
00185 spline.segments[i-1].joints[j].coefficients[2] = 0.5*acc;
00186 spline.segments[i-1].joints[j].quadratic_segment_duration = tb;
00187 spline.segments[i-1].joints[j].linear_segment_duration = std::max(dT-2*tb,0.0);
00188 }
00189 }
00190 return true;
00191 }
00192 }