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_parameterized_trajectory.h>
00038
00039 namespace spline_smoother
00040 {
00041 CubicParameterizedTrajectory::CubicParameterizedTrajectory()
00042 {
00043 apply_limits_ = true;
00044 }
00045
00046 double CubicParameterizedTrajectory::getDistance(const trajectory_msgs::JointTrajectoryPoint &start,
00047 const trajectory_msgs::JointTrajectoryPoint &end,
00048 const std::vector<arm_navigation_msgs::JointLimits> &limits)
00049 {
00050 double position = 0.0;
00051 for(unsigned int i=0; i < start.positions.size(); i++)
00052 position += jointDiff(start.positions[i],end.positions[i],limits[i]) * jointDiff(start.positions[i],end.positions[i],limits[i]);
00053 position = sqrt(position);
00054 return position;
00055 }
00056
00057 double CubicParameterizedTrajectory::getVelocityLimit(const trajectory_msgs::JointTrajectoryPoint &start,
00058 const trajectory_msgs::JointTrajectoryPoint &end,
00059 const std::vector<arm_navigation_msgs::JointLimits> &limits)
00060 {
00061 double velocity = DBL_MAX;
00062 for(unsigned int i=0; i < start.positions.size(); i++)
00063 {
00064 double tmp_vel = limits[i].max_velocity/fabs(end.positions[i]-start.positions[i]);
00065 if(velocity > tmp_vel)
00066 velocity = tmp_vel;
00067 }
00068 return velocity;
00069 }
00070
00071 double CubicParameterizedTrajectory::getAccelerationLimit(const trajectory_msgs::JointTrajectoryPoint &start,
00072 const trajectory_msgs::JointTrajectoryPoint &end,
00073 const std::vector<arm_navigation_msgs::JointLimits> &limits)
00074 {
00075 double acceleration = DBL_MAX;
00076 for(unsigned int i=0; i < start.positions.size(); i++)
00077 {
00078 double tmp_vel = limits[i].max_acceleration/fabs(end.positions[i]-start.positions[i]);
00079 if(acceleration > tmp_vel)
00080 acceleration = tmp_vel;
00081 }
00082 return acceleration;
00083 }
00084
00085 bool CubicParameterizedTrajectory::hasAccelerationLimits(const std::vector<arm_navigation_msgs::JointLimits> &limits)
00086 {
00087 for(unsigned int i=0; i < limits.size(); i++)
00088 if(!limits[i].has_acceleration_limits)
00089 return false;
00090 return true;
00091 }
00092
00093 void CubicParameterizedTrajectory::getLimit(const trajectory_msgs::JointTrajectoryPoint &start,
00094 const trajectory_msgs::JointTrajectoryPoint &end,
00095 const std::vector<arm_navigation_msgs::JointLimits> &limits,
00096 arm_navigation_msgs::JointLimits &limit_out)
00097 {
00098 limit_out.has_position_limits = true;
00099 limit_out.min_position = 0;
00100 limit_out.max_position = 1;
00101 limit_out.has_velocity_limits = true;
00102 limit_out.max_velocity = getVelocityLimit(start,end,limits);
00103
00104 limit_out.has_acceleration_limits = false;
00105 if(hasAccelerationLimits(limits))
00106 {
00107 limit_out.max_acceleration = getAccelerationLimit(start,end,limits);
00108 limit_out.has_acceleration_limits = true;
00109 }
00110 }
00111
00112
00113 double CubicParameterizedTrajectory::jointDiff(const double &start,
00114 const double &end,
00115 const arm_navigation_msgs::JointLimits &limit)
00116 {
00117 if(limit.has_position_limits)
00118 return end-start;
00119 else
00120 return angles::shortest_angular_distance(start,end);
00121 }
00122
00123 void CubicParameterizedTrajectory::solveCubicSpline(const double &q0,
00124 const double &q1,
00125 const double &v0,
00126 const double &v1,
00127 const double &dt,
00128 std::vector<double> &coefficients)
00129 {
00130 coefficients.resize(4);
00131 double diff = q1-q0;
00132 coefficients[0] = q0;
00133 coefficients[1] = v0;
00134 coefficients[2] = (3*diff-(2*v0+v1)*dt)/(dt*dt);;
00135 coefficients[3] = (-2*diff+(v0+v1)*dt)/pow(dt,3);
00136 }
00137
00138 bool CubicParameterizedTrajectory::parameterize(const trajectory_msgs::JointTrajectory& trajectory_in,
00139 const std::vector<arm_navigation_msgs::JointLimits> &limits,
00140 spline_smoother::SplineTrajectory& spline)
00141 {
00142 int num_traj = trajectory_in.points.size();
00143 int num_joints = trajectory_in.joint_names.size();
00144 spline.names = trajectory_in.joint_names;
00145 spline.segments.resize(num_traj-1);
00146
00147 for(int i=0; i<num_joints; i++)
00148 {
00149 if(!limits[i].has_velocity_limits)
00150 {
00151 ROS_ERROR("Trying to apply velocity limits without supplying them. Set velocity_limits in the message and set has_velocity_limits flag to true.");
00152 return false;
00153 }
00154 }
00155 for (int i=1; i< num_traj; ++i)
00156 {
00157 spline.segments[i-1].joints.resize(num_joints);
00158
00159 for(int j =0; j < num_joints; j++)
00160 spline.segments[i-1].joints[j].coefficients.resize(4);
00161 double dT = (trajectory_in.points[i].time_from_start - trajectory_in.points[i-1].time_from_start).toSec();
00162
00163
00164 trajectory_msgs::JointTrajectoryPoint start,end;
00165 start.positions.resize(1);
00166 start.velocities.resize(1);
00167 start.positions[0] = 0.0;
00168
00169 end.positions.resize(1);
00170 end.velocities.resize(1);
00171 end.positions[0] = 1.0;
00172
00173
00174 arm_navigation_msgs::JointLimits parameter_limits;
00175 parameter_limits.joint_name = "cubic_parameterization";
00176 getLimit(trajectory_in.points[i-1],trajectory_in.points[i],limits,parameter_limits);
00177
00178 if(apply_limits_)
00179 {
00180 spline_smoother::CubicTrajectory solver;
00181 spline_smoother::SplineTrajectory spline;
00182 arm_navigation_msgs::JointTrajectoryWithLimits wpt;
00183 double dTMin;
00184
00185 wpt.trajectory.points.push_back(start);
00186 wpt.trajectory.points.push_back(end);
00187 wpt.limits.push_back(parameter_limits);
00188
00189 solver.parameterize(wpt.trajectory,wpt.limits,spline);
00190 spline_smoother::getTotalTime(spline,dTMin);
00191 if(dTMin > dT)
00192 dT = dTMin;
00193 }
00194
00195 std::vector<double> coefficients;
00196 solveCubicSpline(start.positions[0],
00197 end.positions[0],
00198 0.0,
00199 0.0,
00200 dT,
00201 coefficients);
00202 spline.segments[i-1].duration = ros::Duration(dT);
00203
00204 for(int j=0; j<num_joints; j++)
00205 {
00206 double diff = trajectory_in.points[i].positions[j] - trajectory_in.points[i-1].positions[j];
00207
00208 spline.segments[i-1].joints[j].coefficients[0] = trajectory_in.points[i-1].positions[j] + diff * coefficients[0];
00209 spline.segments[i-1].joints[j].coefficients[1] = diff*coefficients[1];
00210 spline.segments[i-1].joints[j].coefficients[2] = diff*coefficients[2];
00211 spline.segments[i-1].joints[j].coefficients[3] = diff*coefficients[3];
00212 }
00213 }
00214 return true;
00215 }
00216 }
