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/cubic_trajectory.h>
00038
00039 namespace spline_smoother
00040 {
00041 CubicTrajectory::CubicTrajectory()
00042 {
00043 apply_limits_ = true;
00044 }
00045
00046 double CubicTrajectory::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 double CubicTrajectory::minSegmentTime(const double &q0,
00064 const double &q1,
00065 const double &v0,
00066 const double &v1,
00067 const arm_navigation_msgs::JointLimits &limit)
00068 {
00069
00070 double dq = q1-q0;
00071 double vmax = limit.max_velocity;
00072 if( q0 == q1 && fabs(v0-v1) == 0.0)
00073 {
00074 return 0.0;
00075 }
00076 dq = q1-q0;
00077 double v = vmax;
00078 double solution;
00079 std::vector<double> solution_vec;
00080
00081 double a1 = 3*(v0+v1)*v - 3* (v0+v1)*v0 + (2*v0+v1)*(2*v0+v1);
00082 double b1 = -6*dq*v + 6 * v0 *dq - 6*dq*(2*v0+v1);
00083 double c1 = 9*dq*dq;
00084
00085 double a2 = 3*(v0+v1)*v + 3* (v0+v1)*v0 - (2*v0+v1)*(2*v0+v1);
00086 double b2 = -6*dq*v - 6 * v0 *dq + 6*dq*(2*v0+v1);
00087 double c2 = -9*dq*dq;
00088
00089 std::vector<double> t1,t2,t3,t4,t5,t6;
00090
00091 if(quadSolve(a1,b1,c1,t1))
00092 for(unsigned int i=0; i < t1.size(); i++)
00093 solution_vec.push_back(t1[i]);
00094
00095 if(quadSolve(a2,b2,c2,t2))
00096 for(unsigned int i=0; i < t2.size(); i++)
00097 solution_vec.push_back(t2[i]);
00098 double amax = -1.0;
00099
00100 if(limit.has_acceleration_limits)
00101 {
00102 amax = limit.max_acceleration;
00103 double a3 = amax/2.0;
00104 double b3 = 2*v0+v1;
00105 double c3 = -3*dq;
00106 if(quadSolve(a3,b3,c3,t3))
00107 for(unsigned int i=0; i < t3.size(); i++)
00108 solution_vec.push_back(t3[i]);
00109
00110 double a4 = amax/2.0;
00111 double b4 = -(2*v0+v1);
00112 double c4 = 3*dq;
00113 if(quadSolve(a4,b4,c4,t4))
00114 for(unsigned int i=0; i < t4.size(); i++)
00115 solution_vec.push_back(t4[i]);
00116
00117
00118 double a5 = amax;
00119 double b5 = (-2*v0-4*v1);
00120 double c5 = 6*dq;
00121 if(quadSolve(a5,b5,c5,t5))
00122 for(unsigned int i=0; i < t5.size(); i++)
00123 solution_vec.push_back(t5[i]);
00124
00125 double a6 = amax;
00126 double b6 = (2*v0+4*v1);
00127 double c6 = -6*dq;
00128 if(quadSolve(a6,b6,c6,t6))
00129 for(unsigned int i=0; i < t6.size(); i++)
00130 solution_vec.push_back(t6[i]);
00131 }
00132 std::vector<double> positive_durations, valid_durations;
00133 for(unsigned int i=0; i < solution_vec.size(); i++)
00134 {
00135 if(solution_vec[i] > 0)
00136 positive_durations.push_back(solution_vec[i]);
00137 }
00138
00139 for(unsigned int i=0; i < positive_durations.size(); i++)
00140 {
00141 ROS_DEBUG("Positive duration: %f",positive_durations[i]);
00142 if(validSolution(q0,q1,v0,v1,positive_durations[i],vmax,amax))
00143 valid_durations.push_back(positive_durations[i]);
00144 }
00145
00146 ROS_DEBUG("valid size: %d",(int)valid_durations.size());
00147 std::sort(valid_durations.begin(),valid_durations.end());
00148 if(!valid_durations.empty())
00149 solution = valid_durations.front();
00150 else
00151 solution = 0.025;
00152
00153 ROS_DEBUG(" ");
00154 ROS_DEBUG(" ");
00155 ROS_DEBUG(" ");
00156 return solution;
00157 }
00158
00159 bool CubicTrajectory::validSolution(const double &q0,
00160 const double &q1,
00161 const double &v0,
00162 const double &v1,
00163 const double &dT,
00164 const double &vmax,
00165 const double &amax)
00166 {
00167 if (dT == 0.0)
00168 return false;
00169
00170 double a1 = v0;
00171 double a2 = (3*(q1-q0)-(2*v0+v1)*dT)/(dT*dT);
00172 double a3 = (2*(q0-q1)+(v0+v1)*dT)/(dT*dT*dT);
00173
00174 double max_accn = fabs(2*a2);
00175 if(fabs(2*a2+6*a3*dT) > max_accn)
00176 max_accn = fabs(2*a2+6*a3*dT);
00177
00178 bool max_vel_exists = false;
00179 double max_vel = 0.0;
00180
00181 if(fabs(a3) > 0.0)
00182 {
00183 double max_vel_time = (-2*a2)/(6*a3);
00184 if (max_vel_time >= 0 && max_vel_time < dT)
00185 {
00186 max_vel_exists = true;
00187 max_vel = a1-(a2*a2)/(a3*3.0);
00188 }
00189 }
00190
00191 if(amax > 0 && max_accn-amax > 1e-2)
00192 {
00193 ROS_DEBUG("amax allowed: %f, max_accn: %f",amax,max_accn);
00194 return false;
00195 }
00196 if(max_vel_exists)
00197 if(fabs(max_vel)-vmax > 1e-2)
00198 {
00199 ROS_DEBUG("vmax allowed: %f, max_vel: %f",vmax,max_vel);
00200 return false;
00201 }
00202 return true;
00203 }
00204
00205 bool CubicTrajectory::parameterize(const trajectory_msgs::JointTrajectory& trajectory_in,
00206 const std::vector<arm_navigation_msgs::JointLimits> &limits,
00207 spline_smoother::SplineTrajectory& spline)
00208 {
00209 int num_traj = trajectory_in.points.size();
00210 int num_joints = trajectory_in.joint_names.size();
00211 spline.names = trajectory_in.joint_names;
00212 spline.segments.resize(num_traj-1);
00213
00214 for(int i=0; i<num_joints; i++)
00215 {
00216 if(!limits[i].has_velocity_limits)
00217 {
00218 ROS_ERROR("Trying to apply velocity limits without supplying them. Set velocity_limits in the message and set has_velocity_limits flag to true.");
00219 return false;
00220 }
00221 }
00222 for (int i=1; i< num_traj; ++i)
00223 {
00224 spline.segments[i-1].joints.resize(num_joints);
00225
00226 for(int j =0; j < num_joints; j++)
00227 spline.segments[i-1].joints[j].coefficients.resize(4);
00228 double dT = (trajectory_in.points[i].time_from_start - trajectory_in.points[i-1].time_from_start).toSec();
00229 if(apply_limits_)
00230 {
00231 double dTMin = calculateMinimumTime(trajectory_in.points[i-1],trajectory_in.points[i],limits);
00232 if(dTMin > dT)
00233 dT = dTMin;
00234 }
00235 spline.segments[i-1].duration = ros::Duration(dT);
00236 for(int j=0; j<num_joints; j++)
00237 {
00238
00239 double diff = trajectory_in.points[i].positions[j] - trajectory_in.points[i-1].positions[j];
00240 spline.segments[i-1].joints[j].coefficients[0] = trajectory_in.points[i-1].positions[j];
00241 spline.segments[i-1].joints[j].coefficients[1] = trajectory_in.points[i-1].velocities[j];
00242 spline.segments[i-1].joints[j].coefficients[2] = (3*diff-(2*trajectory_in.points[i-1].velocities[j]+trajectory_in.points[i].velocities[j])*spline.segments[i-1].duration.toSec())/(spline.segments[i-1].duration.toSec()*spline.segments[i-1].duration.toSec());;
00243 spline.segments[i-1].joints[j].coefficients[3] = (-2*diff+(trajectory_in.points[i-1].velocities[j]+trajectory_in.points[i].velocities[j])*spline.segments[i-1].duration.toSec())/pow(spline.segments[i-1].duration.toSec(),3);
00244 }
00245 }
00246 return true;
00247 }
00248
00249 bool CubicTrajectory::quadSolve(const double &a,
00250 const double &b,
00251 const double &c,
00252 double &solution)
00253 {
00254 double t1(0.0), t2(0.0);
00255
00256 if (fabs(a) > 0.0)
00257 {
00258 double discriminant = b*b-4*a*c;
00259 if (discriminant >= 0)
00260 {
00261 t1 = (-b + sqrt(discriminant))/(2*a);
00262 t2 = (-b - sqrt(discriminant))/(2*a);
00263 ROS_DEBUG("t1:%f t2:%f",t1,t2);
00264 solution = std::max(t1,t2);
00265 ROS_DEBUG("Solution: %f",solution);
00266 return true;
00267 }
00268 else
00269 return false;
00270 }
00271 else
00272 {
00273 if(fabs(b) == 0.0)
00274 return false;
00275 t1 = -c/b;
00276 t2 = t1;
00277 solution = t1;
00278 ROS_DEBUG("Solution: %f",solution);
00279 return true;
00280 }
00281 }
00282
00283 bool CubicTrajectory::quadSolve(const double &a,
00284 const double &b,
00285 const double &c,
00286 std::vector<double> &solution)
00287 {
00288 double t1(0.0), t2(0.0);
00289 double eps = 2.2e-16;
00290 if (fabs(a) > eps)
00291 {
00292 double discriminant = b*b-4*a*c;
00293 if (discriminant >= 0)
00294 {
00295 t1 = (-b + sqrt(discriminant))/(2*a);
00296 t2 = (-b - sqrt(discriminant))/(2*a);
00297 ROS_DEBUG("t1:%f t2:%f",t1,t2);
00298 solution.push_back(t1);
00299 solution.push_back(t2);
00300 return true;
00301 }
00302 else
00303 {
00304 ROS_DEBUG("Discriminant: %f",discriminant);
00305 return false;
00306 }
00307 }
00308 else
00309 {
00310 if(fabs(b) == 0.0)
00311 return false;
00312 t1 = -c/b;
00313 t2 = t1;
00314 solution.push_back(t1);
00315 solution.push_back(t2);
00316 return true;
00317 }
00318 }
00319
00320
00321 }
