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
00038 #ifndef SPLINE_SMOOTHER_UTILS_H_
00039 #define SPLINE_SMOOTHER_UTILS_H_
00040
00041 #include <arm_navigation_msgs/JointTrajectoryWithLimits.h>
00042 #include <trajectory_msgs/JointTrajectory.h>
00043 #include <arm_navigation_msgs/JointLimits.h>
00044 #include <spline_smoother/LSPBTrajectoryMsg.h>
00045 #include <spline_smoother/SplineTrajectory.h>
00046 #include <angles/angles.h>
00047 #include <ros/ros.h>
00048 #include <float.h>
00049 namespace spline_smoother
00050 {
00051
00052
00053 template <typename T>
00054 void differentiate(const std::vector<T>& x, std::vector<T>& xd);
00055
00067 template <typename T>
00068 void tridiagonalSolve(std::vector<T>& a,
00069 std::vector<T>& b,
00070 std::vector<T>& c,
00071 std::vector<T>& d,
00072 std::vector<T>& x);
00073
00075
00076 template <typename T>
00077 void differentiate(const std::vector<T>& x, std::vector<T>& xd)
00078 {
00079 int size = x.size();
00080 xd.resize(size-1);
00081 for (int i=0; i<size-1; ++i)
00082 {
00083 xd[i] = x[i+1] - x[i];
00084 }
00085 }
00086
00087 template <typename T>
00088 void tridiagonalSolve(std::vector<T>& a,
00089 std::vector<T>& b,
00090 std::vector<T>& c,
00091 std::vector<T>& d,
00092 std::vector<T>& x)
00093 {
00094 int n = (int)d.size();
00095
00096 x.resize(n);
00097
00098
00099 for (int i=1; i<n; i++)
00100 {
00101 double m = a[i] / b[i-1];
00102 b[i] -= m*c[i-1];
00103 d[i] -= m*d[i-1];
00104 }
00105
00106
00107 x[n-1] = d[n-1]/b[n-1];
00108 for (int i=n-2; i>=0; i--)
00109 {
00110 x[i] = (d[i] - c[i]*x[i+1])/b[i];
00111 }
00112 }
00113
00121 template <typename T>
00122 bool checkTrajectoryConsistency(T& waypoint_traj)
00123 {
00124 unsigned int length = waypoint_traj.request.trajectory.points.size();
00125 unsigned int num_joints = waypoint_traj.request.trajectory.joint_names.size();
00126
00127 double prev_time = -1.0;
00128
00129 for (unsigned int i=0; i<length; i++)
00130 {
00131 if (waypoint_traj.request.trajectory.points[i].positions.size() != num_joints)
00132 {
00133 ROS_ERROR("Number of positions (%d) at trajectory index %d doesn't match number of joint names (%d)",
00134 (int) waypoint_traj.request.trajectory.points[i].positions.size(), (int) i, (int) num_joints);
00135 return false;
00136 }
00137 if (waypoint_traj.request.trajectory.points[i].time_from_start.toSec() < prev_time)
00138 {
00139 ROS_ERROR("Time of waypoint at trajectory index %d (%f) is not greater than or equal to the previous time (%f)",
00140 (int) i, waypoint_traj.request.trajectory.points[i].time_from_start.toSec(), prev_time);
00141 return false;
00142 }
00143 if(waypoint_traj.request.trajectory.points[i].time_from_start.toSec() < 0.0)
00144 {
00145 ROS_ERROR("Time of waypoint at trajectory index %d (%f) is negative",
00146 (int) i, waypoint_traj.request.trajectory.points[i].time_from_start.toSec());
00147 return false;
00148 }
00149 prev_time = waypoint_traj.request.trajectory.points[i].time_from_start.toSec();
00150 if(waypoint_traj.request.trajectory.points[i].velocities.size() != waypoint_traj.request.trajectory.points[i].positions.size())
00151 waypoint_traj.request.trajectory.points[i].velocities.resize(num_joints, 0.0);
00152 if(waypoint_traj.request.trajectory.points[i].accelerations.size() != waypoint_traj.request.trajectory.points[i].positions.size())
00153 waypoint_traj.request.trajectory.points[i].accelerations.resize(num_joints, 0.0);
00154 }
00155 return true;
00156 }
00157
00169 inline double jointDiff(const double &from, const double &to, const arm_navigation_msgs::JointLimits &limit)
00170 {
00171 if(!limit.has_position_limits)
00172 {
00173 return angles::shortest_angular_distance(from,to);
00174 }
00175 else
00176 {
00177 return (to-from);
00178 }
00179 }
00180
00181
00182
00183
00184
00195 bool findSplineSegment(const spline_smoother::SplineTrajectory &spline,
00196 const double& time,
00197 spline_smoother::SplineTrajectorySegment& spline_segment,
00198 double& segment_time,
00199 int start_index=0,
00200 int end_index=-1);
00201
00202 bool findSplineSegment(const spline_smoother::LSPBTrajectoryMsg &spline,
00203 const double& time,
00204 spline_smoother::LSPBTrajectorySegmentMsg& spline_segment,
00205 double& segment_time,
00206 int start_index=0,
00207 int end_index=-1);
00208
00209
00216 bool getTotalTime(const spline_smoother::SplineTrajectory &spline,
00217 double &t);
00218
00219
00226 bool getTotalTime(const spline_smoother::LSPBTrajectoryMsg &spline,
00227 double &t);
00228
00229 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectorySegment &spline,
00230 const double& input_time,
00231 trajectory_msgs::JointTrajectoryPoint &point_out);
00232
00233 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectorySegmentMsg &spline,
00234 const double& input_time,
00235 trajectory_msgs::JointTrajectoryPoint &point_out);
00236
00237 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectory& spline,
00238 const std::vector<double> ×,
00239 trajectory_msgs::JointTrajectory& traj_out);
00240
00241 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectoryMsg& spline,
00242 const std::vector<double> ×,
00243 trajectory_msgs::JointTrajectory& traj_out);
00244
00252 bool write(const spline_smoother::SplineTrajectory &spline,
00253 const std::vector<double> ×,
00254 const std::string &filename);
00255
00262 bool writeSpline(const spline_smoother::SplineTrajectory &spline,
00263 const std::string &filename);
00264
00272 bool write(const spline_smoother::LSPBTrajectoryMsg &spline,
00273 const std::vector<double> ×,
00274 const std::string &filename);
00275
00282 bool writeSpline(const spline_smoother::LSPBTrajectoryMsg &spline,
00283 const std::string &filename);
00284
00285 static const double MAX_ALLOWABLE_TIME = FLT_MAX;
00287 }
00288 #endif