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/spline_smoother_utils.h>
00038 #include <spline_smoother/LSPBTrajectoryMsg.h>
00039 #include <spline_smoother/SplineTrajectory.h>
00040 #include <angles/angles.h>
00041
00042 namespace spline_smoother
00043 {
00044
00055 bool findSplineSegment(const spline_smoother::SplineTrajectory &spline,
00056 const double& time,
00057 spline_smoother::SplineTrajectorySegment& spline_segment,
00058 double& segment_time,
00059 int start_index,
00060 int end_index)
00061 {
00062 if(end_index == -1)
00063 end_index = spline.segments.size();
00064
00065 double segment_start_time = 0.0;
00066 double segment_end_time = 0.0;
00067
00068 for(int i=0; i < (int)spline.segments.size(); i++)
00069 {
00070 segment_start_time = segment_end_time;
00071 segment_end_time += spline.segments[i].duration.toSec();
00072 if(time <= segment_end_time)
00073 {
00074 segment_time = time-segment_start_time;
00075 spline_segment = spline.segments[i];
00076 ROS_DEBUG("Found spline segment: %d, trajectory time: %f, segment_time: %f, segment_end_time: %f",i,time,segment_time,segment_end_time);
00077 return true;
00078 }
00079 }
00080 if(time >= segment_end_time)
00081 {
00082 ROS_DEBUG("Did not find spline segment corresponding to input time: %f",time);
00083 segment_time = segment_end_time - segment_start_time;
00084 spline_segment = spline.segments.back();
00085 return true;
00086 }
00087 ROS_ERROR("Should not be here in findSplineSegment. Input time: %f is invalid",time);
00088 return false;
00089 }
00090
00091 bool findSplineSegment(const spline_smoother::LSPBTrajectoryMsg &spline,
00092 const double& time,
00093 spline_smoother::LSPBTrajectorySegmentMsg& spline_segment,
00094 double& segment_time,
00095 int start_index,
00096 int end_index)
00097 {
00098 if(end_index == -1)
00099 end_index = spline.segments.size();
00100
00101 double segment_start_time = 0.0;
00102 double segment_end_time = 0.0;
00103
00104 for(int i=0; i < (int)spline.segments.size(); i++)
00105 {
00106 segment_start_time = segment_end_time;
00107 segment_end_time += spline.segments[i].duration.toSec();
00108 if(time <= segment_end_time)
00109 {
00110 segment_time = time-segment_start_time;
00111 spline_segment = spline.segments[i];
00112 ROS_DEBUG("Found spline segment: %d, trajectory time: %f, segment_time: %f, segment_end_time: %f",i,time,segment_time,segment_end_time);
00113 return true;
00114 }
00115 }
00116 if(time >= segment_end_time)
00117 {
00118 ROS_DEBUG("Did not find spline segment corresponding to input time: %f",time);
00119 segment_time = segment_end_time - segment_start_time;
00120 spline_segment = spline.segments.back();
00121 return true;
00122 }
00123 ROS_ERROR("Should not be here in findSplineSegment. Input time: %f is invalid",time);
00124 return false;
00125 }
00126
00127
00134 bool getTotalTime(const spline_smoother::SplineTrajectory &spline,
00135 double &t)
00136 {
00137 t = 0.0;
00138 for(int i=0; i < (int)spline.segments.size(); i++)
00139 {
00140 t += spline.segments[i].duration.toSec();
00141 }
00142 return true;
00143 }
00144
00145
00152 bool getTotalTime(const spline_smoother::LSPBTrajectoryMsg &spline,
00153 double &t)
00154 {
00155 t = 0.0;
00156 for(int i=0; i < (int)spline.segments.size(); i++)
00157 {
00158 t += spline.segments[i].duration.toSec();
00159 }
00160 return true;
00161 }
00162
00163
00164 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectorySegment &spline,
00165 const double& input_time,
00166 trajectory_msgs::JointTrajectoryPoint &point_out)
00167 {
00168 double t = input_time;
00169 if(t > spline.duration.toSec())
00170 {
00171 t = spline.duration.toSec();
00172 }
00173 int joint_num = spline.joints.size();
00174 point_out.positions.resize(joint_num);
00175 point_out.velocities.resize(joint_num);
00176 point_out.accelerations.resize(joint_num);
00177 for(unsigned int i=0; i< spline.joints.size(); i++)
00178 {
00179 point_out.positions[i] = 0.0;
00180 point_out.velocities[i] = 0.0;
00181 point_out.accelerations[i] = 0.0;
00182
00183 for(unsigned int j =0; j < spline.joints[i].coefficients.size(); j++)
00184 {
00185 point_out.positions[i] += spline.joints[i].coefficients[j]*pow(t,j);
00186 if(j > 0)
00187 point_out.velocities[i] += j*spline.joints[i].coefficients[j]*pow(t,j-1);
00188 if(j > 1)
00189 point_out.accelerations[i] += j*(j-1)*spline.joints[i].coefficients[j]*pow(t,j-2);
00190 }
00191 }
00192 point_out.time_from_start = ros::Duration(t);
00193 return true;
00194 }
00195
00196 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectorySegmentMsg &spline,
00197 const double& input_time,
00198 trajectory_msgs::JointTrajectoryPoint &point_out)
00199 {
00200 double t = input_time;
00201 if(t > spline.duration.toSec())
00202 {
00203 t = spline.duration.toSec();
00204 }
00205 int joint_num = spline.joints.size();
00206 point_out.positions.resize(joint_num);
00207 point_out.velocities.resize(joint_num);
00208 point_out.accelerations.resize(joint_num);
00209
00210 double taccend(0.0), tvelend(0.0), tvel(0.0), acc(0.0), v0(0.0);
00211
00212 for(int i=0; i< joint_num; i++)
00213 {
00214 taccend = spline.joints[i].quadratic_segment_duration;
00215 tvelend = spline.joints[i].linear_segment_duration + spline.joints[i].quadratic_segment_duration;
00216 tvel = spline.joints[i].linear_segment_duration;
00217 acc = spline.joints[i].coefficients[2]*2;
00218 v0 = spline.joints[i].coefficients[1];
00219
00220 if(t <= taccend)
00221 {
00222 point_out.positions[i] = spline.joints[i].coefficients[0] + t * v0 + 0.5 * t * t * acc;
00223 point_out.velocities[i] = spline.joints[i].coefficients[1] + t * acc;
00224 point_out.accelerations[i] = acc;
00225 }
00226 else if(t >= tvelend)
00227 {
00228 double dT = t - tvelend;
00229 point_out.positions[i] = spline.joints[i].coefficients[0] + v0 * taccend + 0.5 * acc * taccend * taccend + acc * taccend * tvel + acc * taccend * dT - 0.5 * acc * dT * dT;
00230 point_out.velocities[i] = acc*taccend - acc*dT;
00231 point_out.accelerations[i] = -acc;
00232 }
00233 else
00234 {
00235 double dT = t - taccend;
00236 point_out.positions[i] = spline.joints[i].coefficients[0] + v0 * taccend + 0.5 * acc * taccend * taccend + acc * taccend * dT;
00237 point_out.velocities[i] = acc * taccend;
00238 point_out.accelerations[i] = 0.0;
00239 }
00240 }
00241 point_out.time_from_start = ros::Duration(t);
00242 return true;
00243 }
00244
00245 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectory& spline,
00246 const std::vector<double> ×,
00247 trajectory_msgs::JointTrajectory& traj_out)
00248 {
00249 bool success = true;
00250 trajectory_msgs::JointTrajectoryPoint point_out;
00251 spline_smoother::SplineTrajectorySegment spline_segment;
00252 double segment_time;
00253 traj_out.points.clear();
00254 traj_out.points.resize(times.size());
00255 for(int i=0; i < (int)times.size(); i++)
00256 {
00257 ROS_DEBUG("Input time:%d %f",i,times[i]);
00258 success = success && findSplineSegment(spline,times[i],spline_segment,segment_time);
00259 success = success && sampleSplineTrajectory(spline_segment,segment_time,point_out);
00260 point_out.time_from_start = ros::Duration(times[i]);
00261 traj_out.points[i] = point_out;
00262 }
00263 traj_out.joint_names = spline.names;
00264 return success;
00265 }
00266
00267 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectoryMsg& spline,
00268 const std::vector<double> ×,
00269 trajectory_msgs::JointTrajectory& traj_out)
00270 {
00271 bool success = true;
00272 trajectory_msgs::JointTrajectoryPoint point_out;
00273 spline_smoother::LSPBTrajectorySegmentMsg spline_segment;
00274 double segment_time;
00275 traj_out.points.clear();
00276 traj_out.points.resize(times.size());
00277 for(int i=0; i < (int)times.size(); i++)
00278 {
00279 ROS_DEBUG("Input time:%d %f",i,times[i]);
00280 success = success && findSplineSegment(spline,times[i],spline_segment,segment_time);
00281 success = success && sampleSplineTrajectory(spline_segment,segment_time,point_out);
00282 point_out.time_from_start = ros::Duration(times[i]);
00283 traj_out.points[i] = point_out;
00284 }
00285 traj_out.joint_names = spline.names;
00286 return success;
00287 }
00288
00296 bool write(const spline_smoother::SplineTrajectory &spline,
00297 const std::vector<double> ×,
00298 const std::string &filename)
00299 {
00300 trajectory_msgs::JointTrajectory tp;
00301 if(!sampleSplineTrajectory(spline,times,tp))
00302 {
00303 ROS_ERROR("Input spline was not sampled successfully");
00304 return false;
00305 }
00306
00307 if(tp.points.empty())
00308 {
00309 ROS_ERROR("Input trajectory is empty");
00310 return false;
00311 }
00312
00313 FILE *f = fopen(filename.c_str(),"w");
00314 if(!f)
00315 return false;
00316
00317 int num_joints = tp.points[0].positions.size();
00318 for(int i=0; i < (int) tp.points.size(); i++)
00319 {
00320 fprintf(f,"%f ",tp.points[i].time_from_start.toSec());
00321 for(int j=0; j < num_joints; j++)
00322 {
00323 fprintf(f,"%f ",tp.points[i].positions[j]);
00324 }
00325 for(int j=0; j < num_joints; j++)
00326 {
00327 fprintf(f,"%f ",tp.points[i].velocities[j]);
00328 }
00329 for(int j=0; j < num_joints; j++)
00330 {
00331 fprintf(f,"%f ",tp.points[i].accelerations[j]);
00332 }
00333 fprintf(f,"\n");
00334 }
00335 fclose(f);
00336 return true;
00337 }
00338
00345 bool writeSpline(const spline_smoother::SplineTrajectory &spline,
00346 const std::string &filename)
00347 {
00348 if(spline.segments.empty())
00349 return false;
00350
00351 int num_segments = spline.segments.size();
00352 int num_joints = spline.segments[0].joints.size();
00353
00354 FILE *f = fopen(filename.c_str(),"w");
00355 if(!f)
00356 return false;
00357 for(int i=0; i < num_segments; i++)
00358 {
00359 fprintf(f,"%f ",spline.segments[i].duration.toSec());
00360 for(int j=0; j < num_joints; j++)
00361 {
00362 for(unsigned int k=0; k < spline.segments[i].joints[j].coefficients.size(); k++)
00363 fprintf(f,"%f ",spline.segments[i].joints[j].coefficients[k]);
00364 }
00365 fprintf(f,"\n");
00366 }
00367 fclose(f);
00368 return true;
00369 }
00370
00378 bool write(const spline_smoother::LSPBTrajectoryMsg &spline,
00379 const std::vector<double> ×,
00380 const std::string &filename)
00381 {
00382 trajectory_msgs::JointTrajectory tp;
00383 if(!sampleSplineTrajectory(spline,times,tp))
00384 {
00385 ROS_ERROR("Input spline was not sampled successfully");
00386 return false;
00387 }
00388
00389 if(tp.points.empty())
00390 {
00391 ROS_ERROR("Input trajectory is empty");
00392 return false;
00393 }
00394
00395 FILE *f = fopen(filename.c_str(),"w");
00396 if(!f)
00397 return false;
00398
00399 int num_joints = tp.points[0].positions.size();
00400 for(int i=0; i < (int) tp.points.size(); i++)
00401 {
00402 fprintf(f,"%f ",tp.points[i].time_from_start.toSec());
00403 for(int j=0; j < num_joints; j++)
00404 {
00405 fprintf(f,"%f ",tp.points[i].positions[j]);
00406 }
00407 for(int j=0; j < num_joints; j++)
00408 {
00409 fprintf(f,"%f ",tp.points[i].velocities[j]);
00410 }
00411 for(int j=0; j < num_joints; j++)
00412 {
00413 fprintf(f,"%f ",tp.points[i].accelerations[j]);
00414 }
00415 fprintf(f,"\n");
00416 }
00417 fclose(f);
00418 return true;
00419 }
00420
00427 bool writeSpline(const spline_smoother::LSPBTrajectoryMsg &spline,
00428 const std::string &filename)
00429 {
00430 if(spline.segments.empty())
00431 return false;
00432
00433 int num_segments = spline.segments.size();
00434 int num_joints = spline.segments[0].joints.size();
00435
00436 FILE *f = fopen(filename.c_str(),"w");
00437 if(!f)
00438 return false;
00439 for(int i=0; i < num_segments; i++)
00440 {
00441 fprintf(f,"%f ",spline.segments[i].duration.toSec());
00442 for(int j=0; j < num_joints; j++)
00443 {
00444 for(unsigned int k=0; k < spline.segments[i].joints[j].coefficients.size(); k++)
00445 fprintf(f,"%f ",spline.segments[i].joints[j].coefficients[k]);
00446 }
00447 fprintf(f,"\n");
00448 }
00449 fclose(f);
00450 return true;
00451 }
00452
00453 }